SLAM not running on Lidar

classic Classic list List threaded Threaded
4 messages Options
Tho
Reply | Threaded
Open this post in threaded view
|

SLAM not running on Lidar

Tho
Hi.

I am trying to run SLAM on a series of lidar point clouds (x,y,z,intensity) with GPS (x [m], y [m], yaw), twist (linear & angular) and acceleration (linear, angular).

I use this config file (rtabmap_params.yaml):

rtabmap:
  ros__parameters:
    frame_id: "base_link"
    subscribe_scan: false
    subscribe_scan_cloud: true
    subscribe_rgbd: false
    subscribe_rgb: false
    subscribe_depth: false
    subscribe_odom_info: false
    subscribe_user_data: false
    subscribe_imu: true
    subscribe_stereo: false
    subscribe_point_cloud2: true
    use_sim_time: false
    subscribe_gps: true


    Grid/Sensor: 'false'

    Rtabmap/DetectionRate: '10.0'
    Rtabmap/MaxRepublished: '10'

    Mem/IncrementalMemory: 'true'
    Mem/InitWMWithAllNodes: 'true'
    Mem/STMSize: '100'

    Reg/Strategy: '1'

    RGBD/ProximityPathMaxNeighbor: '0'
    RGBD/ProximityBySpace: 'true'

    approx_sync: true
    sync_queue_size: 100
    queue_size: 100

    Mem/NotLinkedNodesKept: 'false'
    Mem/STMSize: '30'
    Reg/Strategy: '1'
    RGBD/ProximityMaxGraphDepth: '0'
    RGBD/ProximityPathMaxNeighbors: '1'
    RGBD/AngularUpdate: '0.05'
    RGBD/LinearUpdate: '0.05'
    RGBD/CreateOccupancyGrid: 'false'
    RGBD/LoopClosureThreshold: '0.05'
    Icp/PointToPlane: 'true'
    Icp/Strategy: '1'
    Icp/VoxelSize: '0.1'
    Icp/MaxCorrespondenceDistance: '0.5'
    Icp/CorrespondenceRatio': '0.7'


my publisher is initialized like this:

class DataPublisher(Node):
    def __init__(self, gps_imu_data: GPSIMUData, pc_loader: PointCloudsLoader):
        super().__init__('data_publisher')

        # Publishers
        self.cloud_pub = self.create_publisher(PointCloud2, '/scan_cloud', 10)
        self.gps_pub = self.create_publisher(Odometry, '/odom', 10)
        self.imu_pub = self.create_publisher(Imu, '/imu', 10)
        self.tf_broadcaster = TransformBroadcaster(self)

        # Load data
        self.gps_imu_data = gps_imu_data
        self.pc_loader = pc_loader
       
        self.idx_lidar = 0
        #self.idx_gps = 0

        self.timer = self.create_timer(0.1, self.publish_data)  # 10 Hz
        self.progressbar = tqdm(total=len(pc_loader))


I run the stuff like this:
slam: ros2 run rtabmap_slam rtabmap --ros-args --params-file rtabmap_params.yaml
visualizer: ros2 run rtabmap_viz rtabmap_viz --params-file rtabmap_params.yaml
publisher: python3 publisher.py

currently rtabmap is listening to odom and scan_cloud. the visualizer shows that the data is received. but the resulting trajectory is the same as my input - so no slam is applied. I believe that loop closures are not detected / accepted.
how can I fix it?

bonus question: for now I set the publisher to publish at a rate of 10Hz using the current timestamp. but the data frequency comes with some variance and I would prefer to use the original timestamps and process it as fast as possible - but offline (no real time requirement). is that feasible?

thanks a lot.

Reply | Threaded
Open this post in threaded view
|

Re: SLAM not running on Lidar

matlabbe
Administrator
Only proximity detection can be done without a camera. Do you have some loops in your trajectory? otherwise, the trajectory would be exactly the same than your odometry.

Do you want SLAM be done at 10 Hz offline, or still 1 Hz? For 1Hz, you could run it once live, then you can reprocess the database afterwards and changing parameters offline. For 10 Hz or to process all frames, I would make an app based on this example (without events) and that lidar example. A ros option would be to start rtabmap with Rtabmap/DetectionRate=0 and disable proximity detection (RGBD/ProximityBySpace=false) and occupancy grid creation (RGBD/CreateOccupancyGrid=0) to setup rtabmap node as recorder node (just save data in database without any processing). You could then reprocess the database with RGBD/ProximityBySpace=true and other parameters afterwards (using rtabmap-reprocess or RTAB-Map standalone app).

Note that in your setup, only the slam back-end is used, if it is only what you need it is fine, but if you need to do ICP odometry, you would need to use also icp_odometry node with possibly using your odometry as guess.
Tho
Reply | Threaded
Open this post in threaded view
|

Re: SLAM not running on Lidar

Tho
Hi.
Thanks for your reply.
I think what I am looking for is icp_odometry - I actually intended to use provided odometry as guess.
Currently, if I enable it (setting it to true) it crashes:


[ WARN] (2025-08-07 17:10:47.969) SensorData.cpp:855::uncompressDataConst() Requested raw image data, but the sensor data (1) doesn't have image.
[ WARN] (2025-08-07 17:10:47.970) SensorData.cpp:870::uncompressDataConst() Requested depth/right image data, but the sensor data (1) doesn't have depth/right image.
[ WARN] (2025-08-07 17:10:47.970) SensorData.cpp:855::uncompressDataConst() Requested raw image data, but the sensor data (185) doesn't have image.
[ WARN] (2025-08-07 17:10:47.970) SensorData.cpp:870::uncompressDataConst() Requested depth/right image data, but the sensor data (185) doesn't have depth/right image.
rtabmap: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:366: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index, Eigen::Index) [with Derived = Eigen::Block<Eigen::Matrix<float, -1, -1>, -1, -1, false>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = float; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed.
[ros2run]: Aborted


Can you provide an example for icp_odometry lidar-only?

Best regards
Reply | Threaded
Open this post in threaded view
|

Re: SLAM not running on Lidar

matlabbe
Administrator
Are you using rtabmap built from source or the binaries? If you built from source I suspect that you included dependencies usng Eigen that were built with/without "-march=native" flags. Mixing up dependencies like that can result in similar crashes. Either ALL dependencies using Eigen are built with that flag, or neither they are.