Simultaneous Localization and Mapping (SLAM) Using LiDAR and ICP: Mapping and Localization in Python

Introduction

Artificial intelligence plays a pivotal role in the functioning of modern robotics and autonomous technologies. Yet, one of the most critical challenges prior to implementing AI is enabling machines to understand their location and surroundings simultaneously. The hidden algorithm performing this task is Simultaneous Localization and Mapping (SLAM). By leveraging SLAM, robots and vehicles can navigate unfamiliar terrain by building a real-time map and continuously estimating their position, which is essential for navigation, path planning, and obstacle avoidance in real-time.

Among the many SLAM modalities, LiDAR-based SLAM has gained traction due to its high accuracy, rich spatial data, and resilience to lighting conditions, making it a top choice in autonomous vehicles, drones, and mapping robotics. When paired with algorithms like ICP (Iterative Closest Point) and modern libraries like Open3D, SLAM becomes a practical and powerful tool even for developers and researchers working in Python.

SLAM overview

Simultaneous Localization and Mapping (SLAM) is the process by which a robot or autonomous system creates a map of its environment while determining its location within that map. Instead of relying on pre-defined maps, SLAM allows systems to:

• Map out unknown environments

• Track and update their own position (localization)

• Avoid obstacles and plan efficient paths

• Optimize movement using front-end and back-end processing

This is especially useful for indoor (household) robots, autonomous vehicles, and drones, where GPS may be limited or unavailable.

SLAM powered by LIDAR

LiDAR (Light Detection and Ranging) uses laser pulses to measure distances with high precision. In SLAM, LiDAR offers:

• Accurate 2D or 3D point cloud generation

• Precise distance measurement

• Effective performance in low-light or dark conditions

• Consistent results even in visually ambiguous environments

The result is a detailed, real-time spatial model that the SLAM algorithm can use to track the robot's position as it moves through the environment.

Iterative Closest Point (ICP) in SLAM

To compute movement, the system registers sequential LiDAR scans, comparing new frames to prior ones. Algorithms like ICP (Iterative Closest Point) or LOAM (LiDAR Odometry and Mapping) are used to estimate how the robot has moved relative to its previous position.

ICP is the key algorithm administering the SLAM framework. The algorithm is implemented between 2 point clouds. A point cloud is a collection of data points in 3D space that represent the external surface of an object or environment. Point Clouds are generated using LiDAR as laser-reflected measurements are received from objects and structures in the environment. 

Here is a simple breakdown of how ICP functions: 

• A source point cloud: e.g., the latest LiDAR scan
• A target point cloud: e.g., a previous scan or a pre-built map

Unanimous functions: 

• Find correspondences: For each point in the source cloud, find the closest point in the target cloud.
• Estimate transformation: Compute the best rigid transformation (rotation + translation) that aligns the source to the target, based on correspondences.
• Apply transformation: Transform the source cloud.
• Check for convergence: Stop when the error change is below a threshold or the maximum iterations are reached.

ICP diverges into 2 different formats, Point-to-point & Point-to-Plane

Point-to-point

• Correspondence rule: Match each point in the source to the closest point in the target (Euclidean distance).
• Error metric: Minimize the sum of squared Euclidean distances between matched point pairs.
• Transformation estimation: Use SVD (Singular Value Decomposition) to solve for the optimal rotation and translation.

Point-to-Plane

• Correspondence rule: Match source points to the closest target points with known surface normals.
• Error metric: Minimize the distance along the target point’s normal vector, not the full Euclidean distance. (This results in faster convergence and better fitting on smooth surfaces.)
• Transformation estimation: Solves a linearized system of equations, often using least-squares optimization.

This allows the system to estimate its movement and update its position accurately.

Utilization of Python for ICP

A common method to implement ICP is through Python. In Python, ICP (Iterative Closest Point) can be implemented using libraries like Open3D, which provides high-level functions for point cloud processing and registration.

Down Below is a sample code outlining the processes of ICP

import import numpy as np
import open3d as o3d
import copy

#Read Source and Target PCD
demo_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])        

This portion of Python code sets up the environment for performing ICP (Iterative Closest Point) using the Open3D library. It begins by importing essential modules: NumPy for numerical operations, Open3D for 3D data processing, and the copy module for duplicating objects safely. The o3d.data.DemoICPPointClouds() function is used to load a sample dataset containing two point clouds—a source and a target—which are commonly used for ICP demonstrations. These point clouds are then read from the provided file paths using o3d.io.read_point_cloud(), preparing them for further processing such as visualization, transformation, or alignment. Notably, a variable naming typo (demo_icp_pcds instead of demo_pcds) should be corrected to ensure the code runs properly.

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      zoom=0.4459,
                                      front=[0.9288, -0.2951, -0.2242],
                                      lookat=[1.6784, 2.0612, 1.4451],
                                      up=[-0.3402, -0.9189, -0.1996])In this tutorial, we show two ICP variants, the point-to-point ICP and the point-to-plane ICP [Rusinkiewicz2001]_.        

The draw_registration_result() function visualizes the alignment between a source and target point cloud using a given transformation matrix. It begins by creating deep copies of the input clouds to avoid altering the originals. The source is painted a yellow-orange color ([1, 0.706, 0]), while the target is painted light blue ([0, 0.651, 0.929]) for clear visual distinction. The source cloud is then transformed using the provided transformation, which is a result of an ICP alignment. Finally, Open3D's draw_geometries() function renders the two point clouds with custom camera settings. This visualization step is especially useful in tutorials that demonstrate ICP variants, such as point-to-point ICP, which minimizes Euclidean distances between points, and point-to-plane ICP, which minimizes distances along surface normals for faster and more accurate convergence on smooth surfaces.

trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)        

The variable trans_init defines a 4×4 transformation matrix in homogeneous coordinates, composed of a rotation (top-left 3×3 submatrix) and a translation (rightmost column of the top three rows). This transformation is typically an initial guess of the source point cloud’s pose relative to the target. The matrix is converted into a NumPy array using np.asarray. The draw_registration_result() function is then called with the source and target point clouds and this transformation matrix. As a result, the visualization shows how well the initial alignment positions the source cloud relative to the target, which is an important step before performing precise alignment using ICP.

print("Initial alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(
    source, target, threshold, trans_init)
print(evaluation)        

This code evaluates how well the initial transformation (trans_init) aligns the source and target point clouds using a distance threshold to define valid point correspondences. It then prints metrics like fitness and RMSE to assess the quality of the initial alignment before applying ICP.

Point-to-Point implementation

threshold=0.02
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)        

This code performs point-to-point ICP (Iterative Closest Point) to refine the alignment between the source and target point clouds. It sets a correspondence threshold of 0.02 meters and uses the previously defined trans_init as the starting transformation. The registration_icp() function is called with the TransformationEstimationPointToPoint() method, which minimizes the Euclidean distance between closest point pairs. The result, stored in reg_p2p, includes the optimized transformation matrix, fitness score, and RMSE, all of which are printed for analysis. Finally, draw_registration_result() visualizes the point clouds after alignment using the newly computed transformation.

Point-to-Plane implementation

threshold=0.02
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)        

This code performs point-to-plane ICP, which refines the alignment between the source and target point clouds by minimizing the distance from each source point to the plane defined by its corresponding target point and normal. Using a threshold of 0.02 meters and the same initial transformation (trans_init), the registration_icp() function is called with TransformationEstimationPointToPlane(), requiring that both point clouds have estimated surface normals. The resulting registration object reg_p2p contains the optimized transformation matrix, along with evaluation metrics like fitness and RMSE, which are printed. The final alignment is then visualized with draw_registration_result() to show the improved fit.

Conclusion

In conclusion, integrating SLAM with LiDAR and ICP provides a powerful framework for real-time mapping and localization in robotics and autonomous systems. Through Python and libraries like Open3D, developers can implement and visualize the core mechanics of SLAM, from capturing LiDAR-generated point clouds to refining pose estimates using point-to-point or point-to-plane ICP. These methods not only enhance spatial understanding but also enable precise navigation in GPS-denied environments. Whether for autonomous vehicles, drones, or household robots, this approach highlights how classical algorithms like ICP, when combined with modern tools, make SLAM both accessible and practical for a wide range of real-world applications.

Program used for reference can be found here: https://medium.com/@amnahhmohammed/gentle-introduction-to-point-cloud-registration-using-open3d-c8503527f421