Indoor LiDAR Odometry – ICP & EKF (ROS 2 Humble)

This repository implements three different odometry estimation algorithms using LiDAR and IMU data in ROS 2 Humble:

1. LiDAR-only Incremental Odometry (scan-to-scan translation estimation)
2. ICP-based Odometry (using PCL Iterative Closest Point)
3. Extended Kalman Filter (EKF) Fusion of IMU (predict) + ICP (update)

All methods publish odometry on: /odom_est

The algorithms operate on Livox sensor topics:

• /livox/amr/lidar (PointCloud2)
• /livox/amr/imu (IMU)

---

📂 Repository Structure

├── src/
│ ├── lidar_odometry.cpp # Method 1: Pure LiDAR odom
│ ├── icp_odometry.cpp # Method 2: ICP-based odom
│ ├── ekf_lidar_imu.cpp # Method 3: EKF fusion of ICP + IMU
│ ├── ekf_lidar_odom.hpp # EKF implementation
│ └── ...
├── config/
│ └── ekf_lidar_imu.yaml # Optional EKF config
├── result/
│ ├── result1.png # Sample trajectory / ATE plot
│ └── result2.png # Yaw error or comparison plot
└── README.md

---

🚀 Algorithms Implemented

1️⃣ LiDAR Scan Translation Odometry (Baseline)

A fast scan-to-scan translation estimation method:

• Extract 2D slice from 3D LiDAR
• Compute nearest-neighbor point correspondences
• Estimate translation (dx, dy)
• Accumulate pose

Pros: Fast, simple
Cons: Drift over time, fails on rotation-only motion

---

2️⃣ ICP-Based Odometry (Using PCL)

This uses PCL’s Iterative Closest Point:

• Convert PointCloud2 → PCL
• Apply filtering (voxel grid, z-slice, range gating)
• Align curr_cloud to prev_cloud using ICP
• Extract dx, dy, dyaw
• Publish odometry

Pros: More accurate, handles rotation
Cons: Slower, may fail with repeated structures

---

3️⃣ EKF Fusion: IMU (Prediction) + ICP (Correction)

State vector:

x = [x, y, yaw, vx, vy]

Prediction (IMU):

• Integrate angular_velocity.z → yaw
• Convert IMU acceleration to world frame
• Update velocity & position
• Propagate covariance

Update (ICP):

• ICP computes relative translation
• Use as measurement to correct EKF state

Pros:

• Best accuracy
• Robust against motion types
• Smooth output

Cons:

• Requires tuning (Q, R, ICP thresholds)

---

📊 Results

Trajectory Comparison

### **Absolute Trajectory Error Plot**

### **Yaw Error Plot**

---

🛠 Dependencies

Install required ROS packages:

sudo apt install ros-humble-pcl-conversions \
                 ros-humble-nav-msgs \
                 ros-humble-tf2 \
                 ros-humble-robot-localization

PCL is included with ROS 2 Humble.

🔧 Build

cd ~/ros_ws2  
colcon build --packages-select indoor_lidar_odom_cpp  
source install/setup.bash

▶ Run the Algorithms

Method 1: LiDAR-only odometry

ros2 run indoor_lidar_odom_cpp lidar_odometry

Method 2: ICP odometry

ros2 run indoor_lidar_odom_cpp icp_odometry

Method 3: EKF (IMU + ICP)

ros2 run indoor_lidar_odom_cpp ekf_lidar_imu

📌 Topics

Topic Type Description

/odom_est	nav_msgs/Odometry	Output odometry
/tf	transform tree	odom → base_link_est

Tunable Parameters

EKF Parameters
Q: IMU process noise
R: ICP measurement noise
Initial covariance
Velocity process variance

ICP Parameters
MaximumIterations
MaxCorrespondenceDistance
TransformationEpsilon
EuclideanFitnessEpsilon
Voxel leaf size
Z-slice limits

Range gating (min/max distance)

📈 Runtime Performance

Method Speed (Hz) Notes
LiDAR-only ~25–30 Hz Fastest
ICP ~7–20 Hz Depends on cloud size & filtering
EKF IMU rate (predict) + ~10–25 Hz (update) Smooth & accurate

📜 License
MIT License.