# A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm

^{1}

^{2}

^{3}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Feature Extraction

#### 2.1. The Main Concept of Feature Extraction

#### 2.2. A Novel Method for Finding Clusters

#### 2.3. Split and Merge for Corner and Line Extractions

## 3. The Proposed Method: WP-ICP

#### 3.1. Pose Estimation Algorithm

#### 3.2. A Weighted Parallel ICP Pose Estimation Algorithm

## 4. Experiments and Discussions

_{c}= 10 cm is used for the split and merge process. For each iteration, the weights for ICP will be set to zero if the distances between the points’ correspondences are greater than 50 cm. This threshold is determined in accordance with the maximum moving speed of the robot.

^{2}and contained different sized objects like flowerpots and water-cooler as shown in Figure 14, which can be taken as unknown disturbances for post estimation. These objects were added later in the environment, and hence can be used to test the feasibility and robustness of the proposed WP-ICP algorithm in dynamic environments.

## 5. Conclusions

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- Khoshelham, K.; Zlatanova, S. Sensors for Indoor Mapping and Navigation. Sensors
**2016**, 16, 655. [Google Scholar] [CrossRef] [PubMed] - Nützi, G.; Weiss, S.; Scaramuzza, D.; Siegwart, R. Fusion of IMU and vision for absolute scale estimation in monocular SLAM. J. Intell. Robot. Syst.
**2011**, 61, 287–299. [Google Scholar] [CrossRef] - Se, S.; Lowe, D.G.; Little, J.J. Vision-based global localization and mapping for mobile robots. IEEE Trans. Robot.
**2005**, 21, 364–375. [Google Scholar] [CrossRef] - Lowe, D.G. Object recognition from local scale-invariant features. In Proceedings of the Seventh IEEE International Conference on Computer Vision, Kerkyra, Greece, 20–22 September 1999; pp. 1150–1157. [Google Scholar]
- Bay, H.; Ess, A.; Tuytelaars, T.; Gool, V.L. Speeded-up robust features (SURF). Comput. Vis. Image Underst.
**2008**, 110, 346–359. [Google Scholar] [CrossRef] - Husen, M.N.; Lee, S. Indoor Location Sensing with Invariant Wi-Fi Received Signal Strength Fingerprinting. Sensors
**2016**, 16, 1898. [Google Scholar] [CrossRef] [PubMed] - Chang, Q.; Li, Q.; Shi, Z.; Chen, W.; Wang, W. Scalable Indoor Localization via Mobile Crowdsourcing and Gaussian Process. Sensors
**2016**, 16, 381. [Google Scholar] [CrossRef] [PubMed] - Chen, Z.; Zou, H.; Jiang, H.; Zhu, Q.; Soh, Y.C.; Xie, L. Fusion of WiFi, smartphone sensors and landmarks using the Kalman filter for indoor localization. Sensors
**2015**, 15, 715–732. [Google Scholar] [CrossRef] [PubMed] - Passafiume, M.; Maddio, S.; Cidronali, A. An Improved Approach for RSSI-Based Only Calibration-Free Real-Time Indoor Localization on IEEE 802.11 and 802.15.4 Wireless Networks. Sensors
**2017**, 17, 717. [Google Scholar] [CrossRef] [PubMed] - Wei, C.C.; Lin, J.S.; Chang, C.C.; Huang, Y.F.; Lin, C.B. The Development of E-Bike Navigation Technology Based on an OpenStreetMap. J. Smart Sci.
**2017**, 6, 29–35. [Google Scholar] [CrossRef] - D’Alfonso, L.; Grano, A.; Muraca, P.; Pugliese, P. A polynomial based SLAM algorithm for mobile robots using ultrasonic sensors-Experimental results. In Proceedings of the 16th International Conference on Advanced Robotics (ICAR), Montevideo, Uruguay, 25–29 November 2013. [Google Scholar]
- Yu, C.W.; Liu, C.H.; Chen, Y.L.; Lee, P.; Tian, M.S. Vision-based Hand Recognition Based on ToF Depth Camera. J. Smart Sci.
**2017**, 6, 21–28. [Google Scholar] [CrossRef] - Fankhauser, P.; Bloesch, M.; Rodriguez, D.; Kaestner, R.; Hutter, M.; Siegwart, R. Kinect v2 for Mobile Robot Navigation: Evaluation and Modeling. In Proceedings of the 2015 International Conference on Advanced Robotics (ICAR), Istanbul, Turkey, 27–31 July 2015. [Google Scholar]
- Chen, C.; Yang, B.; Song, S.; Tian, M.; Li, J.; Dai, W.; Fang, L. Calibrate Multiple Consumer RGB-D Cameras for Low-Cost and Efficient 3D Indoor Mapping. Remote Sens.
**2018**, 10, 328. [Google Scholar] [CrossRef] - Nguyen, V.; Martinelli, A.; Tomatis, N.; Siegwart, R. A comparison of line extraction algorithms using 2D range data for indoor mobile robotics. Auton. Robot.
**2007**, 23, 97–111. [Google Scholar] [CrossRef] - Aghamohammadi, A.; Tamjidi, A.H.; Taghirad, H.D. SLAM Using Single Laser Range Finder. In Proceedings of the 17th IFAC World Congress, Seoul, Korea, 6–11 July 2008; pp. 14657–14662. [Google Scholar]
- Besl, P.J.; McKay, N.D. Method for registration of 3-D shapes. Proc. SPIE
**1992**, 1611, 239–256. [Google Scholar] [CrossRef] - Lu, F.; Milios, E. Robot pose estimation in unknown environments by matching 2D range scans. J. Intell. Robot. Syst.
**1997**, 18, 249–275. [Google Scholar] [CrossRef] - Rusinkiewicz, S.; Levoy, M. Efficient variants of the ICP algorithm. In Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling, Quebec, Canada, 28 May–1 June 2001. [Google Scholar]
- Montesano, L.; Minguez, J.; Montano, L. Probabilistic scan matching for motion estimation in unstructured environments. In Proceedings of the IEEE/RSJ International conference on Intelligent Robots and Systems (IROS), Edmonton, AB, Canada, 2–6 August 2005. [Google Scholar]
- Minguez, J.; Lamiraux, F.; Montesano, L. Metric-based scan matching algorithms for mobile robot displacement estimation. In Proceedings of the IEEE International Conference on Robotics and Automation, Barcelona, Spain, 18–22 April 2005. [Google Scholar]
- Biber, P.; Strasser, W. The Normal Distributions Transform: A New Approach to Laser Scan Matching. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robotics and Systems (IROS), Las Vegas, NV, USA, 27–31 October 2003. [Google Scholar]
- Censi, A. An ICP variant using a point-to-line metric. In Proceedings of the 2008 IEEE International Conference on Robotics and Automation (ICRA), Pasadena, CA, USA, 19–23 May 2008. [Google Scholar]
- Pottmann, H.; Huang, Q.X.; Yang, Y.L.; Hu, S.M. Geometry and convergence analysis of algorithms for registration of 3D shapes. Int. J. Comput. Vis.
**2006**, 67, 277–296. [Google Scholar] [CrossRef] - Kalman, R.E. A new approach to linear filtering and prediction problems. J. Basic Eng.
**1960**, 82, 35–45. [Google Scholar] [CrossRef] - Armesto, L.; Tornero, J. SLAM based on Kalman filter for multi-rate fusion of laser and encoder measurements. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Sendai, Japan, 28 September–2 October 2004. [Google Scholar]
- Zhang, J.; Ou, Y.; Jiang, G.; Zhou, Y. An Approach to Restaurant Service Robot SLAM. In Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO), Qingdao, China, 3–7 December 2016. [Google Scholar]
- Borges, G.A.; Aldon, M.J. Line extraction in 2D range images for mobile robotics. J. Intell. Robot. Syst.
**2004**, 40, 267–297. [Google Scholar] [CrossRef] - Lv, J.; Kobayashi, Y.; Ravankar, A.A.; Emaru, T. Straight Line Segments Extraction and EKF-SLAM in Indoor Environment. J. Auto. Cont. Eng.
**2014**, 2, 270–276. [Google Scholar] [CrossRef] - An, S.Y.; Kang, J.G.; Lee, L.K.; Oh, S.Y. Line segment-based indoor mapping with salient line feature extraction. Adv. Robot.
**2012**, 26, 437–460. [Google Scholar] [CrossRef] - Ravankar, A.; Ravankar, A.A.; Hoshino, Y.; Emaru, T.; Kobayashi, Y. On a hopping-points SVD and hough transform-based line detection algorithm for robot localization and mapping. Int. J. Adv. Robot. Syst.
**2016**, 13, 98. [Google Scholar] [CrossRef] - Duda, R.O.; Hart, P.E. Pattern Classification and Scene Analysis; John Wiley & Sons: New York, NY, USA, 1973. [Google Scholar]
- Reina, A.; Gonzalez, J. A two-stage mobile robot localization method by overlapping segment-based maps. Robot. Auton. Syst.
**2000**, 31, 213–225. [Google Scholar] [CrossRef] - Guivant, J.E.; Nebot, E.M. Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Trans. Robot. Autom.
**2001**, 17, 242–257. [Google Scholar] [CrossRef] - Sorkine-Hornung, O.; Rabinovich, M. Least-Squares Rigid Motion Using SVD. Available online: http://www.igl.ethz.ch/projects/ARAP/svd_rot.pdf (accessed on 19 April 2018).
- Ravankar, A.A.; Hoshino, Y.; Ravankar, A.; Jixin, L.; Emaru, T.; Kobayashi, Y. Algorithms and a framework for indoor robot mapping in a noisy environment using clustering in spatial and hough domains. Int. J. Adv. Robot. Syst.
**2015**, 12, 27. [Google Scholar] [CrossRef]

**Figure 5.**Incorrect point registration caused by the nearest neighbor search (NNS) Iterative Closest Point (ICP) algorithm.

**Figure 6.**Illustration of feature-based point cloud registration. (

**a**) Corner features are matched to corresponding corner features (

**b**) Line features are matched to corresponding line features.

**Figure 9.**Scene-1: a robot moves in a clear environment with no obstacles. (

**a**) Experimental environment. (

**b**) Robot is moved in a guided rectangular path. (

**c**) Robot is moved randomly.

**Figure 15.**Localization by the WP-ICP with 10 cm interpolation. Upper left corner subplot: corner features. Lower left corner subplot: line features. Right subplot: layout and estimated robot trajectory.

**Figure 20.**An environment with unknown moving objects. (

**a**) Random snapshot 1. (

**b**) Random snapshot 2.

**Figure 21.**Localization result by using full-points ICP. (

**a**) Point registration. (

**b**) Robot trajectories.

**Figure 22.**Localization result by using corner features only (where the line information was not fused for the localization).

Experimental Environment | Pose Estimation Algorithm | Average ICP Iteration |
---|---|---|

Scene-1 Exp.1 | Full-Points ICP | 25.36 |

Corner-Based ICP | 2.06 | |

WP-ICP | 2.06/2.04 (corner/line) | |

Scene-1 Exp.2 | Full-Points ICP | 30.10 |

Corner-Based ICP | 2.04 | |

WP-ICP | 2.05/2.04 (corner/line) | |

Scene-2 | Full-Points ICP | 49.92 |

Corner-Based ICP | 2.49 | |

WP-ICP | 2.61/2.85 (corner/line) | |

WP-ICP with 10cm Interpolation | 2.87/9.25 (corner/line) |

Localization Algorithm | Model Set Size (Points) | Average ICP Iterations | Total ICP Iterations | Local. Error (mm) Avg/Max |
---|---|---|---|---|

Full-Points ICP (Point-to-Point ICP) | 1991 | 35 | 86687 | taken as ground truth |

WP-ICP with 10 cm Interpolation | 213 | 13 | 30838 | 53.7/261.3 |

WP-ICP with 20 cm Interpolation | 114 | 9 | 22981 | 60.5/264.6 |

© 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Wang, Y.-T.; Peng, C.-C.; Ravankar, A.A.; Ravankar, A.
A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm. *Sensors* **2018**, *18*, 1294.
https://doi.org/10.3390/s18041294

**AMA Style**

Wang Y-T, Peng C-C, Ravankar AA, Ravankar A.
A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm. *Sensors*. 2018; 18(4):1294.
https://doi.org/10.3390/s18041294

**Chicago/Turabian Style**

Wang, Yun-Ting, Chao-Chung Peng, Ankit A. Ravankar, and Abhijeet Ravankar.
2018. "A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm" *Sensors* 18, no. 4: 1294.
https://doi.org/10.3390/s18041294