# Topological Navigation and Localization in
## 3 Topological Navigation and Localization for LHD
### Topological Map and Physical Representation
An example of the TMM creation from the TM of Tunnel 1 (in Figure 5) is shown in Figures 6 and 7. For clarity of purpose, only half of the connections are shown in each figure. In Figure 6, only connections in the direction of the tunnel are shown, while in Figure 7, only connections against the direction of the tunnel are shown. Each node in the TMM is represented by a blue arrow. Each node has an associated LHD heading and movement direction. The heading is represented by the LHD image and the movement direction by  
An example is shown in Figure 8 for the inversion maneuver. Relevant TMM nodes involved in the inversion movement are highlighted. Once the TMM is set up, topological paths can be calculated upon demand. When a new request is received, the vehicle's current state is pushed to the TMM as a starting point, and then the optimal route to the desired destination is calculated using [PERSON]'s algorithm [28]. Traveling costs between TMM nodes are set up initially based on the  
Figure 8: Representation of an _inversion_ maneuver in the TMM. Figure 7: TMM construction example. Connections heading in the opposite direction of the Tunnel are shown. distance between AP/WP, but can be fined-tuned with additional considerations such as: the change in the vehicle's movement direction, the feasibility of the movement (sometimes a curve is too sharp for the vehicle to turn in a certain direction), and unwanted paths for other reasons such as road repairs, narrow spaces, or uneven tracks. Path planning using the TMM is extremely fast. Its execution in a very large mine layout was simulated in a layout similar to that of the new Chuquicamata underground mine, with eight parallel streets, each one having 19 intersections. Under these conditions, the TMM contained 5378 nodes and 8760 vertices, and path planning using the TMM took less than 5 [ms] on an i7 Intel processor [27]. Each node contains so-called LIDAR-landmarks that are stored when the map is built, and are used for the LHD self-localization. In regard to tunnel nodes, the LIDAR-landmarks correspond to LIDAR point clouds acquired at certain selected positions of the tunnel. In the case of intersection nodes, the LIDAR-landmark corresponds to an integration of the point clouds acquired inside the intersection. Map Building--Determination of LIDAR-Landmarks  
For map building the LHD needs to visit all the tunnels of the mine in one direction. In order to achieve this, the LHD is teleoperated. At the end of this process the LIDAR-landmarks need to be determined. The following procedure is followed for the tunnel nodes:  
1. LIDAR point clouds are stored at fixed positions inside each node (e.g., every 2 [m] in our current implementation). Each of these point clouds are built by joining the LIDAR scans acquired using the front-facing and the rear-facing LIDARs. Then, a filtering process is applied: points colliding with the LHD's body are removed, points that are located too far (>10 [m]) to the center of the LHD are also removed, and finally, the density of the point cloud is normalized. This final point clouds will be the LIDAR-landmarks. 2. The similarity among all pairs of stored LIDAR-landmarks is computed using the ICP algorithm, generating a similarity matrix. The ICP's fitness value is used as a measurement of the geometric similarity between two LIDAR-landmarks (a low fitness value means that ICP managed to fit both landmarks correctly). 3. Using the similarity matrix, representative/key LIDAR-landmarks are chosen. The selected representative LIDAR-landmarks have two important characteristics: (i) They can be matched with their local neighbors using the ICP algorithm (a radius is used for determining the local neighbors); (ii) they are different from other representative LIDAR-landmarks, guaranteeing that two different representative LIDAR-landmarks cannot be mistaken. This selection process is done by the following algorithm:  
* For each LIDAR-landmark:
* The values of similarity with other LIDAR-landmarks are ordered from lowest to highest.