# TensorMap: Lidar-based Topological Mapping and Localization

via Tensor Decompositions

###### Abstract

We propose a technique to develop (and localize in) topological maps from light detection and ranging (Lidar) data. Localizing an autonomous vehicle with respect to a reference map in real-time is crucial for its safe operation. Owing to the rich information provided by Lidar sensors, these are emerging as a promising choice for this task. However, since a Lidar outputs a large amount of data every fraction of a second, it is progressively harder to process the information in real-time. Consequently, current systems have migrated towards faster alternatives at the expense of accuracy. To overcome this inherent trade-off between latency and accuracy, we propose a technique to develop topological maps from Lidar data using the orthogonal Tucker3 tensor decomposition. Our experimental evaluations demonstrate that in addition to achieving a high compression ratio as compared to full data, the proposed technique, TensorMap, also accurately detects the position of the vehicle in a graph-based representation of a map. We also analyze the robustness of the proposed technique to Gaussian and translational noise, thus initiating explorations into potential applications of tensor decompositions in Lidar data analysis.

TensorMap: Lidar-based Topological Mapping and Localization

via Tensor Decompositions

Sirisha Rambhatla, Nikos D. Sidiropoulos and Jarvis Haupt ^{†}^{†}thanks: The authors graciously acknowledge support from the DARPA Young Faculty Award, Grant N66001-14-1-4047. |

Dept. of Electrical & Computer Engineering, University of Minnesota–Twin Cities, Minneapolis, MN 55455 |

Dept. of Electrical & Computer Engineering, University of Virginia, Charlottesville, Virginia 22904 |

rambh002@umn.edu, nikos@virginia.edu, jdhaupt@umn.edu |

Index Terms— Topological maps, Lidar, localization of autonomous vehicles, orthogonal Tucker decompositions, scan-matching.

## 1 Introduction

Autonomous vehicles are gaining significant traction due to the advent of smaller footprint, yet fast processors. One of the major steps in autonomous vehicle navigation is to keep track of the state of the vehicle which, among other things, includes the position of the vehicle with respect to the global frame of reference. For this, vehicles often employ a wide range of sensors like GPS, cameras and inertial measurement units (IMU). However, these sensors usually do not provide the accuracies required to establish safe (and stable) operation.

The advances in Lidar technology coupled with its increasing affordability have made it the most popular sensor for tracking position with millimeter accuracies. However, the Lidar technology comes with its own set of drawbacks. Each scan (the range measurements received by the sensors at different angles of azimuth and elevation) obtained by the Lidar sensor is a point cloud containing millions of data points. Although this data provides very accurate details about the operating environment, the sheer volume of the data thrown at the processor every fraction of a second, often forces us to choose between speed of operation (latency) and accuracy.

One way of addressing this issue is to develop efficient representations of the map. To develop these representations, often a map as the one shown in Fig. 1 (a), can be viewed as a graph with nodes as turns/landmarks, with roads as the edges or segments of the graph. Such a map is known as a topological map; Fig. 1 (b) shows an example of the nodes in such a map. The problem of localization then becomes a problem of identifying which segment the vehicle is on, and how far along in the segment it is positioned.

(a) | (b) |

### 1.1 Prior-Art

Building topological maps for localization using imaging-based techniques has gained traction in recent times since these are inexpensive to implement and faster to process [2, 3, 4, 5, 6, 7, 8, 9], as compared to Lidar sensors. However, these vision-based techniques are sensitive to changing weather and illumination (day and night).

The process of identifying the rigid body transformation that aligns a scan with a map is known as scan matching, and is a very effective choice for localization. Significant advances have been made in the area of developing better and accurate representations for scan-matching using Lidar data [10, 11, 12, 13, 14], but the time, and computational overhead, associated with it are still prohibitive. The state-of-the-art techniques deal with the computational overhead by acquiring Lidar data at lower rate in order to operate in real-time [15, 16].

On the other hand, low rank tensor models, specifically Tucker3 [17] decomposition, popularized by the higher-order singular value decomposition (HO-SVD) technique[18], have gained success in a wide variety of applications; see [19, 20] and the references therein for details. Viewed as a generalization of SVD, here the tensor is factorized as core tensor multiplied by factor matrices in each dimension (mode); the size of the matrices controlling the respective mode ranks (collectively, the so-called multi-linear rank of the tensor). In addition to compressing approximately low multi-linear rank tensors, this decomposition exhibits an interesting property – the core tensor is all orthogonal, i.e., each slice of this tensor is orthogonal to all the other slices; see [18] for details.

It is worth noting that recently, [21] employed tensor models to classify objects in a Lidar scan based on dictionary learning. As opposed to this work, our aim here is to localize a vehicle on a map using the Lidar scans.

### 1.2 Summary of Our Technique

In this work, we present a tensor decompositions-based technique for building topological maps using Lidar data. To this end, we first represent the 3D-point cloud Lidar scans as a 3-way tensor. Next, we learn orthogonal Tucker3 models on partitions of this tensor by exploiting the approximate low multi-linear rank structure, arising from the fact that scans in a local neighborhood – specifically straight paths – are similar; see Fig. 2. Further, we develop a technique to localize in this map by leveraging the “all-orthogonal” property of the aforementioned tensor decomposition; see Fig. 3. To the best of our knowledge, this is the first application to exploit the orthogonality of the core tensor slices.

### 1.3 Our Contributions

We make the following contributions: 1) we develop TensorMap^{1}^{1}1Details about the implementation can be found at https://github.com/srambhatla/TensorMap.: a technique to build Lidar-based topological maps using tensor decompositions and perform localization in them, 2) we analyze the efficiency of the proposed representation in terms of its space complexity in comparison to using the full Lidar data, 3) we show the performance of TensorMap for a localization task on real Lidar data, and 4) we demonstrate the robustness properties of the proposed technique to different types of simulated noise (Gaussian and translational).

## 2 Problem formulation

We illustrate TensorMap using the Ford campus vision and Lidar dataset [1], henceforth referred to as “the Ford Dataset.” The Ford Dataset contains a set of 3800 Lidar scans corresponding to a loop in downtown Dearborn, Michigan. The trajectory of the scans collected by the Ford Dataset is shown in Fig. 1(a). The data is collected using a Velodyne 3D-Lidar scanner which has a vertical field of view (FOV) of (apx. from to ) and a lateral FOV of (from ), with the Lidar spinning at 10 Hz.

(a) | (b) | (c) | (d) |

(e) | (f) | (g) | (h) |

(i) Seg. Len. | (j) Seg. Len. | (k) Seg. Len. | (l) Seg. Len. | (m) Seg. Len. |

### 2.1 Modeling Lidar data as a Tensor

Each scan in the dataset is a list of about 77,000 returns or a point cloud represented in 3D Cartesian coordinates i.e. corresponding to the position of objects reflecting the incident laser, as shown in Fig. 2 (a). Here, the number of returns per scan depends on the scene. To represent Lidar scans as a tensor, we first convert the the data to polar coordinates, which results in a list of returns expressed as , where is the range, is the elevation and is the azimuth. Next, we form a matrix with rows corresponding to elevation angles , and columns corresponding to azimuth angles , by rounding these to whole angles (this discretization is a design choice). Then, for each entry in the list of returns in polar coordinates, we place the range values () at the rounded-off () location, as shown in Fig. 2 (b). Due to this quantization (of the and ), multiple returns may get mapped to a single entry of the matrix. For the given sensor, is restricted between and between . Therefore, each scan is transformed to a matrix, and collecting all the scans, results in a tensor . Therefore, for the Ford data set .

### 2.2 Building TensorMap

For learning the topological map, we use the orthogonal Tucker decomposition to exploit the low mode-rank (in two of the three modes) structure of the tensor. Lidar data is particularly amenable to this model because the scene at each step is highly correlated to the previous one. To leverage this relationship, let denote a tensor in containing all scans corresponding to a map. Next, let denote length- disjoint partitions of for each for , where we assume that divides perfectly; see Fig. 2(c). As a result, we have short tensors for each length- segment along the path whose orthogonal Tucker3 decomposition can be written as

Here, “” denotes kronecker product, , and denote the factors where and , and denotes the vectorized core tensor shown in Fig. 2(c). Note that, to preserve the position information we do not compress along the third dimension of the segment tensor , i.e., we set , where denotes an identity matrix. The core tensor along with factors and corresponding to each segment form the TensorMap, as shown in Fig. 2(d).

### 2.3 Localizing in TensorMap

Since each slice of the core tensor (corresponding to the scans in a segment) is orthogonal to the other slices, each slice of the core tensor can be viewed as a “signature” of the associated scan. As shown in Fig. 3, we exploit this property for localization. Specifically, to localize any test scan (point cloud), we first convert it into a matrix as described in Section 2.1. Next, we form “signature" corresponding to as

for all . Then, we find the closest matching core tensor slice (in Frobenius norm sense) across all segments. This process identifies the scan that is a closest match to the test scan, hence also identifies the segment.

### 2.4 Memory Considerations

We consider the space complexity of TensorMap for its implementation on real-world systems and embedded platforms. We propose to learn a orthogonal Tucker3 model for each length- segment, and there are such models to be learnt. Therefore, the total number of memory units required to store TensorMap are,

This storage requirement is significantly smaller than the original tensor, i.e. , for small values of and . Note that we do not store since in each case it is an identity matrix.

Interestingly, the expression above supports longer segments which still yield a lower error for smaller and . In the context of maps, this means that scans of a segment should be accumulated as long as they are similar to each other. Therefore, suitable segment length is closely related to the number of straight line paths in the map. Note that, although we consider a fixed segment length for the current exposition, there is no requirement that the segments be of equal length. We leave exploration of these extensions to future work.

## 3 Numerical Evaluations

(a) | (d) | (g) |

(b) | (e) | (h) |

(c) | (f) |

We now discuss the performance of the proposed procedure on the Ford Dataset.

### 3.1 Experimental Set-up

We evaluate the performance of TensorMap based on its classification accuracy of assigning test scans to their respective segments, using a - Train : Test split of scans in each segment. To this end, we first learn orthogonal Tucker representations (TensorMap) on the training data for each segment using the HO-SVD algorithm [18][19]. We also analyze the within-segment classification performance by analyzing the train scan sequence which was found closest to the test sequence.

### 3.2 Selecting the Parameters

There are a few design parameters that we need to choose, namely the length of the segment , and the number of columns and in factors and , respectively. To find the best choice(s), we search over various values of , , and , to arrive at a which yields highest accuracy, while being efficient in terms of the storage requirements.

Fig. 4 shows accuracies over different choices of , and segment lengths . We observe that for a specific choice of and , the segment classification performance is better for longer segments as compared to shorter ones. This is because scans in shorter segments are very similar to those in neighboring segments; see Fig. 4 (i)-(m). Also, although longer segments choices sometimes perform better for larger values of and , we prefer smaller and to reduce the computational and memory overhead. Overall, by this analysis, we arrive at the choice of for , respectively.

### 3.3 Results

In Fig. 5, we present the results for chosen as , respectively. We observe that our method identifies the test segments accurately, except for two scans; see Fig. 5(a). To investigate these misclassifications, we turn to Fig. 5(b), which shows the relationship of the errors with the motion surrogate, which is formed by evaluating the norm of change in -DOF pose – provided by the Ford Dataset – of the vehicle. We observe that the errors seem to arise only when the vehicle is stationary. This is due to the fact that the scan acquisition process does not stop when the vehicle is not moving. As a result, scenes in consecutive segments can be very similar to each other. However, attributing scans to any one of the these segments does not adversely effect the localization performance. Therefore, to account for this effect we report errors on parts where the vehicle is moving, using the motion surrogate.

In panel Fig. 5(d) and (e), we show the actual train scan (scan sequence number) found to be the closest to the test set and the misclassified scans, respectively. We note that when the vehicle is in motion, TensorMap indeed performs very well. In practice, we can run TensorMap only when the vehicle is in motion, holding the currently estimated value when the vehicle is stopped.

We also report the error between the original segment tensor and the orthogonal Tucker3 model learnt in Fig. 5(c) and (f), replicated to improve readability. Further, Fig. 5 panel (g) shows the corresponding confusion matrix for segment classification problem shown in Fig. 5(a). Also, the topological map learnt is shown in panel (h). Notice that the nodes of this topological map are not spaced uniformly, this is due to the movement of the vehicle.

### 3.4 Effect of Gaussian noise and Translations

We now study the effect of Gaussian noise and translations on the performance of TensorMap. Here, we generate the noisy tensor by adding zero-mean Gaussian random noise of variance to each coordinate of the Lidar scan, and process these noisy Lidar scans using the procedure described in Section 2.1.

Fig. 6 (a) shows the effect of adding zero-mean Gaussian random noise of variance to each coordinate of the returns (point cloud) on accuracy. We notice that although the technique seems to be robust to lower levels of noise, the performance degrades with increasing . This is because the “signatures" are heavily dependent on the relative position of objects in the environment. This is somewhat reassuring, it points to the fact that TensorMap is basing its decision on the relative placement of features, leveraged at the classification stage.

(a) | (b) |

Next we study the effect of a second, perhaps more challenging type of noise: translations. Fig. 6 (b) shows the effect of successively shifting the test sequence to the right on the accuracy (%). We notice that the technique is successful up-to a translation of about m, beyond which, the performance quickly degrades. Note that a similar effect can be observed for translations to the left. The translations we consider here are artificially generated, in practice the effect of translation may be worse. This is because, the Lidar “sees” additional objects in the direction of translation; posing a potential challenge for our approach.

### 3.5 Compression Ratio

Finally, we analyze the compression ratio of the proposed technique in terms of number of elements to be stored. For the given choice of parameters we achieve the ratio of TensorMap : Tensor representation : Lidar Scan representation of about 1 : 400 : 8300. This significant improvement in terms of memory requirement enables use of TensorMap in real-world applications.

### 3.6 Other Applications and Future Work

Applications of TensorMap also include secure and efficient location communication by transmission of the “signatures” (which in the current case are just matrices), these “signatures” can be viewed as encoded location information. These can be directly understood by the sender and receiver(s), who have access to the a priori known topological map. Further, as alluded to in Section 1, TensorMap can be used for coarse localization before scan-matching thus reducing the associated computational and storage overhead, potentially making scan-matching viable for real-time localization. Further, TensorMap can also be used to detect false loop-closures while scan-matching.

Future work includes fusing data from other sensors to improve the robustness of TensorMap in order to develop techniques for localization, and comparison of such a technique with related works. Also, as alluded to in this discussion, using unequal segment lengths, instead of the fixed ones considered here, remains a potential direction.

## 4 Conclusions

Lidar scan-matching provides the most accurate information about the position of the autonomous vehicle, yet it is computationally expensive, prohibiting its use in real-time localization. Popular techniques reduce the rate of data acquisition to cope with this overhead. In this work, we present a technique based on tensor decompositions for building efficient (in terms of space complexity) graph representations of maps. Our preliminary investigation of the proposed technique via experimental evaluations on real-world Lidar data for a localization task shows promising results, and opens exciting avenues for future explorations, in order to make autonomous vehicle navigation safer and more stable.

## References

- [1] G. Pandey, J. R. McBride, and R. M. Eustice, “Ford campus vision and lidar data set,” International Journal of Robotics Research, vol. 30, no. 13, pp. 1543–1552, 2011.
- [2] C. Siagian and L. Itti, “Biologically inspired mobile robot vision localization,” IEEE Transactions on Robotics, vol. 25, no. 4, pp. 861–873, 2009.
- [3] J. Wang, H. Zha, and R. Cipolla, “Coarse-to-fine vision-based localization by indexing scale-invariant features,” IEEE Transactions on Systems, Man, and Cybernetics, Part B: Cybernetics, vol. 36, no. 2, pp. 413–422, 2006.
- [4] F. Fraundorfer, C. Engels, and D. Nistér, “Topological mapping, localization and navigation using image collections,” in 2007 IEEE/RSJ International Conference on intelligent Robots and Systems (IROS). IEEE, 2007, pp. 3872–3877.
- [5] O. Booij, B. Terwijn, Z. Zivkovic, and B. Kröse, “Navigation using an appearance based topological map,” in 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007, pp. 3927–3932.
- [6] C. K. Chang, C. Siagian, and L. Itti, “Mobile robot vision navigation & localization using gist and saliency,” in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2010, pp. 4147–4154.
- [7] M. J. Milford and G. F. Wyeth, “Seqslam: Visual route-based navigation for sunny summer days and stormy winter nights,” in 2012 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2012, pp. 1643–1649.
- [8] G. Schindler, M. Brown, and R. Szeliski, “City-scale location recognition,” in 2007 IEEE Conference on Computer Vision and Pattern Recognition (CVPR). IEEE, 2007, pp. 1–7.
- [9] A. Angeli, S. Doncieux, J. A. Meyer, and D. Filliat, “Visual topological slam and global localization,” in 2009 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2009, pp. 4300–4305.
- [10] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Robotics-DL tentative. International Society for Optics and Photonics, 1992, pp. 586–606.
- [11] P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in 2003 IEEE International Conference on Intelligent Robots and Systems (IROS). IEEE, 2003, vol. 3, pp. 2743–2748.
- [12] A. Morris, D. Silver, D. Ferguson, and S. Thayer, “Towards topological exploration of abandoned mines,” in Proceedings of the 2005 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2005, pp. 2117–2123.
- [13] A. Myronenko and X. Song, “Point set registration: Coherent point drift,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 32, no. 12, pp. 2262–2275, 2010.
- [14] A. Mueller, M. Himmelsbach, T. Luettel, F. V. Hundelshausen, and H. J. Wuensche, “Gis-based topological robot localization through lidar crossroad detection,” in 2011 14th International IEEE Conference on Intelligent Transportation Systems (ITSC). IEEE, 2011, pp. 2001–2008.
- [15] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time,” in Robotics: Science and Systems Conference (RSS), 2014, pp. 109–111.
- [16] J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift, robust, and fast,” in IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 2174–2181.
- [17] L. R. Tucker and R. Ledyard, “Some mathematical notes on three-mode factor analysis,” Psychometrika, vol. 31, no. 3, pp. 279–311, 1966.
- [18] L. De Lathauwer, B. De Moor, and J. Vandewalle, “A multilinear singular value decomposition,” SIAM journal on Matrix Analysis and Applications, vol. 21, no. 4, pp. 1253–1278, 2000.
- [19] T. G. Kolda and B.W. Bader, “Tensor decompositions and applications,” SIAM review, vol. 51, no. 3, pp. 455–500, 2009.
- [20] N. D. Sidiropoulos, L. De Lathauwer, X. Fu, K. Huang, E. E. Papalexakis, and C. Faloutsos, “Tensor decomposition for signal processing and machine learning,” IEEE Transactions on Signal Processing, vol. 65, no. 13, pp. 3551–3582, July 2017.
- [21] N. Li, N. Pfeifer, and C. Liu, “Tensor-based sparse representation classification for urban airborne lidar points,” Remote Sensing, vol. 9, no. 12, 2017.