Height-embedded Iterative Closest Point (ICP)

I enhanced pose tracking by integrating scene understanding. I utilized vertically oriented objects in urban settings, such as poles and trees, as landmarks. I created a 2D grid data structure that records the vertical extent of these objects, efficiently condensing the spatial information into a representative value. Then, I developed a pose-tracking algorithm based on 2D Iterative Closest Point (ICP). In the process, each cell was assigned a weight proportional to its height value, considering the significance of each cell's spatial contribution. This approach significantly reduced computational load while maintaining accuracy in relation to the 3D point cloud-based pose-tracking algorithms.