Combine ICP with PGO for SLAM

[IshaanParikh]

I have the PGO(pose graph optimization) backend ready in cpp in the SLAMv2 repository. We have an ICP implementation in python done before. So I need to understand that and find a way for the 2 programs to work with each other.

[IshaanParikh]

Motor controller for the LiDAR works.

#include "rclcpp/rclcpp.hpp"
#include
#include
#include

class MotorController : public rclcpp::Node {
public:
MotorController() : Node("motor_controller") {
RCLCPP_INFO(this->get_logger(), "Starting RPLiDAR motor on GPIO 18...");

    // Export GPIO 18
    std::ofstream export_file("/sys/class/gpio/export");
    if (export_file.is_open()) {
        export_file << "18";
        export_file.close();
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Wait for OS to create the virtual files
    }

    // Set direction to output
    std::ofstream dir_file("/sys/class/gpio/gpio18/direction");
    if (dir_file.is_open()) {
        dir_file << "out";
        dir_file.close();
    }

    // Set pin HIGH (100% speed)
    std::ofstream val_file("/sys/class/gpio/gpio18/value");
    if (val_file.is_open()) {
        val_file << "1";
        val_file.close();
    } else {
        RCLCPP_ERROR(this->get_logger(), "FAILED to open GPIO 18. Check permissions!");
    }
}

~MotorController() {
    RCLCPP_INFO(this->get_logger(), "Stopping RPLiDAR motor...");
    
    // Set pin LOW (Stop motor)
    std::ofstream val_file("/sys/class/gpio/gpio18/value");
    if (val_file.is_open()) {
        val_file << "0";
        val_file.close();
    }
    
    // Unexport GPIO 18 to clean up
    std::ofstream unexport_file("/sys/class/gpio/unexport");
    if (unexport_file.is_open()) {
        unexport_file << "18";
        unexport_file.close();
    }
}

};

int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
}

[IshaanParikh]

Launch file works, and we are publishing this to Foxglove. Foxglove is a mapping software for LiDAR data which can help visualize what the LiDAR is seeing

[IshaanParikh]

Got a working LiDAR thanks to Erica. It works allegedly, but I cannot get Foxglove to visualize the data. Taking help from Gemini. Current problem seems to be with permissions on the Pi, and some problems in my launch file, so working around them. Shouldn't take too long.

[IshaanParikh]

I had a lot of trouble with CEV router, which meant I couldn't log into any Pi. I tried switching the network to RedRover, my hotspot and usb debugging, but couldn't figure it out. I couldn't do much work on Sunday.

[IshaanParikh]

CEV router is back! And everything is working. However, there is a very weird error. The lidar is spinning, and publishing, but it is not being found. This is really annoying, I am super close to just starting with the IMU. Trying my best to figure this out soon so that I can test PGO :(

[IshaanParikh]

PGO and SLAM run, but it looks weird on Foxglove(just random lines) since there is no IMU data. So the goal now is to read the IMU data and make sure everything works.

[IshaanParikh]

Current progress
Hardware Debugging: The WitMotion IMU initially suffered from silent deadlocks on the Linux USB serial port. I identified that the sensor’s firmware was defaulting to a 9600 baud rate and a 1Hz return rate, bottlenecking the system. I resolved this by manually flashing the ROM via a Windows configuration tool, successfully pushing the hardware to 115200 baud and a stable 50Hz return rate.

Front-End (ICP): I implemented a Lidar Odometry node using a trimmed ICP method. This node successfully calculates the robot's short-term movement (/odom) by aligning consecutive laser scans, providing the foundational spatial awareness before IMU fusion.

--> The EKF Bottleneck and current bypass solution
To fuse the ICP odometry with the 50Hz IMU data, I integrated the robot_localization Extended Kalman Filter (EKF). However, the EKF failed to output an optimized odometry frame due to three issues:

• Frame ID Mismatch: The WitMotion driver hardcoded its frame as base_link, conflicting with my static transform expectation of imu_link.
• Divide-by-Zero Trap: The IMU driver outputs 0.0 for all covariance matrices. The EKF mathematics attempt to divide by these covariances, resulting in NaN (infinity) matrix explosions.
• Hardware Resets: Occasional timestamp gaps indicated the hardware briefly reverted to 1Hz polling.

To maintain development velocity, I executed a tactical bypass: I temporarily disabled the EKF node and routed the raw ICP odometry directly to the TF tree, ensuring the /scan data had a baseline spatial reference.

--> Custom Pose Graph Optimization (PGO) Backend
I created a custom SLAM backend in C++ utilizing the Eigen matrix library, leaving room for potential improvements(similar to G2O).

Graph Construction: The system tracks the trajectory by tracking keyframes every 0.3 meters or 15 degrees of rotation.

It manually calculate the error residuals and Jacobians, constructing a sparse Information Matrix and applying 5 iterations of a Gauss-Newton solver per keyframe to smooth the trajectory.

Spatial Memory: I implemented a radius search algorithm that looks backward through the keyframe history. If the current pose falls within 1.0 meter of a historical node, it flags a loop closure candidate.

--> Current Problems
Despite the math being compiled, there are some problems with components talking to ach other;

1. Foxglove Visual Failures (Yellow Lines): The Foxglove dashboard displays raw, unprojected laser rays instead of a 3D point cloud. This confirms a broken TF tree (map -> odom link is missing).

2. The "Loop Closure Candidate Found!" event is not triggering. C++ backend is not receiving the bypassed /odom data to build its keyframes.

3. Dead IMU Fusion: The 0.0 covariance output from the IMU prevents the use of any standard ROS 2 filtering nodes.

--> Next Steps

Fix PGO Subscriptions: Hardcode the SLAM_backend_math.cpp node to subscribe directly to /odom instead of the dead /odometry/filtered topic[I am 99% sure this is the issue, but if its not may take some time to figure out what exactly is going on].

Accelerate Testing: Lower the loop closure trigger threshold from 20 keyframes to 5 keyframes to allow for rapid physical testing within a smaller footprint[right now is too slow -- probably cus we are running on the pi, what exactly will we be running on?].

Covariance issues: Write code intercept the /witmotion_node/imu topic, get a baseline diagonal covariance matrix (e.g., 0.01), and republish it to allow standard filters to process the data.

--> Long-Term for better accuracy
Once the custom PGO loop closures are verified, I will pursue one of the following architectural upgrades:

Unscented Kalman Filter (UKF): Upgrade the robot_localization node to a UKF, which utilizes sigma-point sampling to better handle non-linear motion and aggressive IMU noise spikes without mathematically exploding.

Factor Graph Integration (GTSAM): Deprecate the ROS 2 filtering nodes entirely. Transition the custom Eigen backend to GTSAM. This will bundle high-frequency IMU data directly into the pose graph, correcting Lidar drift and IMU bias simultaneously during loop closures.

[IshaanParikh]

current 'map'. Need a better way to test - maybe mini cars or something.