Hesai QT128 with Zed2i and external fused Odometry

[aninath93]

Hi Everyone,

Just wanted to reach out to community and see if my vision has any problems or need lookouts. I am planning to fuse RGB stereo camera (Zed2i) with QT128 Lidar and fusing with external odometry (passing zed2i odom, wheel encoders, zed2i imu to EKF node) ? Its a diff bot robot.

My question is, any alongside suggestions to go this route for attaining less than 2 cm accuracy ? or any other alternative route to be considered ?

[matlabbe]

Hi,

With a QT128, you may also consider LiDAR odometry approaches (ICP odometry, LOAM-derived approaches,...).

What do you mean by 2 cm accuracy? on how much distance? absolute average error against ground truth trajectory (ATE from RGB-D dataset)? Maximum drift (2cm on 1m -> 2% drift)?

cheers,
Mathieu

[aninath93]

Hi Mathieu,

Thank you for your reply, I appreciate you. Based on this example below how can I fuse (wheel encoders + IMU = robot localization odom/filtered node into rtabmap) https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch ?

I know you are extremely busy, but will it possible for you to post any other reference launch file or please point me to the right direction, I will appreciate it.

Thanks
Ani

[matlabbe]

Hi,

To use only wheel encoders + IMU with rtabmap node (no icp odometry), you may use that launch file and remove icp_odometry part. For deskewing, you may use the filtered odometry frame instead, then plug the output deskewed cloud topic to rtabmap node.

cheers,
Mathieu

[aninath93]

Hi Mathieu,

Thank you for the reply, I appreciate you.  I should have mentioned in my previous post and hopefully after this post I will be able to go in development phase and will clear all my doubts.

Can I use FLOAM approach with ICP odometry with my filtered odometry frame (IMU + encoders) from robot localization package to the Odom topic ? and in this case the floam_sensor " <arg name="floam_sensor" default="0"/>" should be set to 3 as its 128 rings correct (QT128) and this set to "<arg name="floam"    default="false"/>" true ?

Also , anything suggestion to just go under the cart with my robot and come out the other side to latch on (thinking of disabling lidar and driving straight but anything you can advise in your experience ) ?

Also is there any python launch file incorporating above parameters (icp with floam and using robot localization into odom) porting over to ros2 or can the above file from ros1 be still reference with latest 0.21.1 build to create   ?
Any input on this from you would be appreciated.

Regards
Ani

[matlabbe]

Hi,

I don't know if FLOAM supports 128 rings. The number 0->2 are corresponding to some pre-defined models from floam. Would have to check their code to know if 128 rings model is supported. On rtabmap side, this should be modified to set 128 for number 3.

aninath93 wrote

Also , anything suggestion to just go under the cart with my robot and come out the other side to latch on (thinking of disabling lidar and driving straight but anything you can advise in your experience ) ?

I don't know the full context, unless you have a safety lidar that estops as soon there is an obstacle under a fixed distance or the lidar is seeing the cart (not the free space under the cart), not sure why you would need to disable the lidar.

To convert ros1 launch files to ros2, well I didn't have time to do that. ros2 launch files can be found here and here (vlp16 demo here, you may compare with the ros1 example). Note that when my new projects will work on ros2 (currently still mostly working with ros1), I may add more ros2 examples over time.

Note also that on ROS 2, you may not be able to build rtabmap with floam.

cheers,
Mathieu

[aninath93]

Thank you for the reply, I appreciate you. I did go through FLOAM after your post and you are right they do not support 128 rings.
So that leaves me at least in this configuration for robustness (robot localization (wheels + imu + icp_odom) to compute for odom and RGB from Zed2i for loop closures with April tags integration. That brings me to me to this launch file and ekf parameters, does this configuration makes sense ? Or Am I still missing something ?

"" ros__parameters:
        frequency: 50.0
        two_d_mode: true
        publish_tf: true
        use_control: false
       
        map_frame: map            
        odom_frame: odom            
        base_link_frame: base_link
        world_frame: odom
        transform_time_offset: 0.0
        sensor_timeout: 0.1

        #x     , y     , z,
        #roll  , pitch , yaw,
        #vx    , vy    , vz,
        #vroll , vpitch, vyaw,
        #ax    , ay    , az
        odom0: /odom
        odom0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]

        imu0: /zed2i/zed_node/imu/data
        imu0_remove_gravitational_acceleration: true
        imu0_queue_size: 2
       
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,
                      true, false, false]
       
        odom1: /odom_icp
        odom1_config: [true, true, false,
                   false, false, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]
"""
""Launch file

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time')
    deskewing = LaunchConfiguration('deskewing')
   
    return LaunchDescription([

        # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'),
       
        DeclareLaunchArgument(
            'deskewing', default_value='true',
            description='Enable lidar deskewing'),
       
        # Compute quaternion of the IMU
        Node(
            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
            parameters=[{'use_mag': False,
                         'world_frame':'enu',
                         'publish_tf':False}],
            remappings=[
                ('imu/data_raw', '/zed2i/zed_node/imu/data')
                ('imu/data', '/zed2i/zed_node/imu/data/filtered')
           
            ]),
       
       
        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{
              'frame_id':'base_link',
              'odom_frame_id':'odom',
              'wait_for_transform':0.2,
              'expected_update_rate':15.0,
              'deskewing':deskewing,
              'use_sim_time':use_sim_time,
              'deskewing_slerp':False,
            }],
            remappings=[
              ('scan_cloud', '/lidar_points1')
              ('odom','/odom_icp')
            ],
            arguments=[
              'Icp/PointToPlane', 'true',
              'Icp/Iterations', '10',
              'Icp/VoxelSize', '0.1',
              'Icp/Epsilon', '0.001',
              'Icp/PointToPlaneK', '20',
              'Icp/PointToPlaneRadius', '0',
              'Icp/MaxTranslation', '2',
              'Icp/MaxCorrespondenceDistance', '1',
              'Icp/Strategy', '1',
              'Icp/OutlierRatio', '0.7',
              'Icp/CorrespondenceRatio', '0.01',
              'Odom/ScanKeyFrameThr', '0.4',
              'OdomF2M/ScanSubtractRadius', '0.1',
              'OdomF2M/ScanMaxSize', '15000',
              'OdomF2M/BundleAdjustment', 'false',
            ]),
           
        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[{
              'frame_id':'base_link',
              'subscribe_depth':False,
              'subscribe_rgb':True,
              'subscribe_scan_cloud':True,
              'approx_sync':True,
              'wait_for_transform':0.2,
              'use_sim_time':use_sim_time,
            }],
            remappings=[
              ('scan_cloud', 'assembled_cloud')
              ('rgb/image', '/zed2i/zed_node/rgb/image_rect_color')
              ('rgb/camera_info', '/zed2i/zed_node/rgb/camera_info')
              ('imu', '/zed2i/zed_node/imu/data/filtered')
            ],
            arguments=[
              '-d', # This will delete the previous database (~/.ros/rtabmap.db)
              'RGBD/ProximityMaxGraphDepth', '0',
              'RGBD/ProximityPathMaxNeighbors', '1',
              'RGBD/AngularUpdate', '0.05',
              'RGBD/LinearUpdate', '0.05',
              'RGBD/CreateOccupancyGrid', 'false',
              'Mem/NotLinkedNodesKept', 'false',
              'Mem/STMSize', '30',
              'Mem/LaserScanNormalK', '20',
              'Reg/Strategy', '1',
              'Icp/VoxelSize', '0.1',
              'Icp/PointToPlaneK', '20',
              'Icp/PointToPlaneRadius', '0',
              'Icp/PointToPlane', 'true',
              'Icp/Iterations', '10',
              'Icp/Epsilon', '0.001',
              'Icp/MaxTranslation', '3',
              'Icp/MaxCorrespondenceDistance', '1',
              'Icp/Strategy', '1',
              'Icp/OutlierRatio', '0.7',
              'Icp/CorrespondenceRatio', '0.2',
            ]),
     
        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=[{
              'frame_id':'base_link',
              'odom_frame_id':'odom',
              'subscribe_odom_info':True,
              'subscribe_scan_cloud':True,
              'approx_sync':False,
              'use_sim_time':use_sim_time,
            }],
            remappings=[
               ('scan_cloud', 'odom_filtered_input_scan')
            ]),

        Node(
            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
            parameters=[{
              'max_clouds':10,
              'fixed_frame_id':'',
              'use_sim_time':use_sim_time,
            }],
            remappings=[
              ('cloud', 'odom_filtered_input_scan')
            ]),
    ]) ""


Do I need add anything else apart from April tags Node and tf relation for better loop closures ?
Also is there any documentation or previous references for integration with Nav2 stack and localization using AMCL side to use the map above with integration of Rtabmap ? What would your thoughts be in the environment for localization using Rtabmap vs AMCL ?
Also for 2d occupancy grid map creation do I need to subscribe to depth topic and make this to true ? ('RGBD/CreateOccupancyGrid', 'false',)


Appreciate your reply and time.

Thanks
Ani

[aninath93]

Hi,

I am trying to see results with different 32 ring sensor instead of existing 128 ring sensor due to several processor reasons, currently with RTABMAP installed using binaries. I see in CLI FLOAM is set to false, how can I leverage FLOAM library in ROS2 side as I have already cloned and build the workspace with it ? and previously you had mentioned the max rings support is 64.

I appreciate your response.

Thanks
Anirudh

[matlabbe]

FLOAM is a ros1 package. You may remove any ros1 in it and build it as a library, then make rtabmap library linking on it. That may need significant work.

For nav2 with rtabmap, there are demos here: https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos/launch

AMCL and rtabmap are not compatible, it is either one or the other.

[aninath93]

Hi Mathieu,

Opened Issue

I have a issue opened at GITHUB and just wanted your feedback on this, as I am just trying to figure out lidar_deskewing with LIDAR on ROS2 side trying to follow your example from ROS1. If I compute ICP odometry as you had mentioned, it works. As soon as I plug EKF frame into guess_frame_id and odom_frame for lidar_deskewing. It gives me below error.


ekf_filter_node:
    ros__parameters:
        frequency: 40.0
        two_d_mode: true
        publish_tf: true
        use_control: false
        print_diagnostics: false
        debug: false
       
        map_frame: map            
        odom_frame: odom  
        base_link_frame: base_link
        world_frame: odom
        sensor_timeout: 0.15

        #x     , y     , z,
        #roll  , pitch , yaw,
        #vx    , vy    , vz,
        #vroll , vpitch, vyaw,
        #ax    , ay    , az
        odom0: /diffbot/odom
        odom0_differential: false
        odom0_relative: false
        odom0_queue_size: 2
        odom0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]

        imu0: /zed2i/zed_node/imu/data
        imu0_remove_gravitational_acceleration: true
        imu0_queue_size: 2
        imu0_relative: false
        imu0_config:    [false, false, false,
                        false, false, true,
                        false, false, false,
                        false, false, true,
                        false, false, false]

        Node(
          package='rtabmap_util', executable='lidar_deskewing', output='screen',
          parameters=[{
              'fixed_frame_id':'odom',
              'slerp':False,
              'wait_for_transform':0.01,
            }],
          remappings=[
                ('input_cloud', '/lidar_points3') #subs
                #('/lidar_points3/deskewed','/lidar_points3/deskewed') #publish
            ]),      
       
        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{
              'ground_truth_base_frame_id':'',
              'frame_id':'base_link',
              'odom_frame_id':'odom_icp',
              'guess_frame_id':'odom',
              'publish_tf':False,
              'approx_sync':False,
              'tf_prefix':'',
              'wait_for_transform':0.2,
              'queue_size':5,
              'publish_null_when_lost':True,
              'expected_update_rate':10.0,
              'deskewing':deskewing,
              'use_sim_time':use_sim_time,
              'deskewing_slerp':False,
              'scan_cloud_max_points':15000,
              'wait_imu_to_init':False,
            }],
            remappings=[
              ('scan_cloud', '/lidar_points3'), #subs
              ('odom','/odom/icp'), #publis
              #('imu','/zed2i/zed_node/imu/data/filtered')
            ],
            arguments=[
              '--udebug',
              #ICP
              'Icp/PointToPlane', 'true',
              'Icp/Iterations', '15',
              'Icp/VoxelSize', '0.05', #0 means all scans used, check computation power to levrage this
              'Icp/Epsilon', '0.001',
              'Icp/DownsamplingStep','1',
              'Icp/PointToPlaneK', '20',
              'Icp/PointToPlaneRadius', '0',
              'Icp/MaxTranslation', '2', #to reject ICP jumps
              'Icp/MaxCorrespondenceDistance', '0.5', #voxel size res*10
              'Icp/CorrespondenceRatio', '0.01',
              'Icp/PointToPlaneGroundNormalsUp','0.8',
              'Icp/Strategy', '1',
              'Icp/ReciprocalCorrespondences','False',
              'Icp/OutlierRatio', '0.7',
              #Odom
              'Odom/ScanKeyFrameThr', '0.4',
              'Odom/Strategy', '0', #(F2F) (1=F2F)
              'OdomF2M/ScanSubtractRadius', '0.1', #resolution
              'OdomF2M/ScanMaxSize', '15000',
              'OdomF2M/BundleAdjustment', 'false',
              'Odom/ResetCountdown','1',
              'Icp/CCFilterOutFarthestPoints','True',
              'Icp/PointToPlaneLowComplexityStrategy','2',
              'Odom/FilteringStrategy','2',
              'Odom/GuessMotion','false',
              'OdomF2M/MaxSize','1000',
              'Optimizer/Robust','true',
              'Reg/Strategy','1',







I tried doing echo between frames, just to make sure my lidar frame is connected to odom and base_link and it is. Any thoughts on this please ?

[aninath93]

Hi Mathieu,

Can this be a issue, different timeclocks of ROS and LIDAR ? As upon further investigating in rviz2 I found PointCloud2 topic cannot be subscribed in fixed frame of "odom" which is what I feel lidar_deskewing is complaining about but in base_link frame I can see the data, but also in rqt tree I do not see any data not linked even in odom frame. If you can shed any light on this in your experience I will appreciate it.

Thanks
Ani

[aninath93]

Hi Mathieu,

By making the controller as PTP master, syncing the LIDAR with it, time of ROS and LIDAR matches and I can display of Pointcloud2 wrt odom frame, but still when I just launch lidar_deskewing node and no icp_odom I still see warning , deskewing failed, possible skewed cloud with published topic input_cloud/deskewed.

MsgConversion.cpp

Also just to add, I had added datatype 8 for timestamp in this file to support my lidar and changed the references to double, I don't know if anywhere else this needs to be changed, but I did build the project.




It looks to me as if it is issue with tf2 buffer size.

[matlabbe]

See second comment https://github.com/introlab/rtabmap_ros/issues/1013#issuecomment-1696693850

[aninath93]

Hi Mathieu,

I am just going to continue here and close the GITHUB issue topic, but as you mentioned I tried ICP odometry this time with another LIDAR on bench Ouster OS0-32, and it worked and I noted down the timestamps field how they are supposed to be by adding to the log, currently trying to debug hesai lidar to make that work.

But even with Ouster I had "Deskewing Failed message, outputting possible skewed cloud from Lidar_deskewing node but ICP odometry worked so far merging with wheel + imu guess frame. Any thoughts on that  ?

So I did look into SDK for Hesai and looks like the message set in ros side is float64 and getting converted to uint32_t, so I made the changes appropriately in the MsgConversion file attached but I get now first and last scan timestamp are same. Also I did tried putting print function in the "timeOnColumns" function but the code never reaches that point, keeps looping in timestamp side. Any input on this will be appreciated while I keep looking into SDK !





Thanks
Ani

[matlabbe]

What was the full error message for the ouster? Is it a TF extrapolation error?

The check about the first and last stamps is here: https://github.com/introlab/rtabmap_ros/blob/b3f33cef228357c87a61a76db712ad4c1b6cbf3d/rtabmap_conversions/src/MsgConversion.cpp#L2694-L2698

Based on your log messages, the firstStamp and lastStamp should have at least epoch timestamp of the header of the pointcloud. There is something wrong with "input.header.stamp".

[aninath93]

Hi Mathieu,

Thank you for replying,

For the ouster its my error, after adding deskewing node, I did not make the ICP_odom node deskewing parameter to false which caused the error on the previous post, after making it false the below result was obtained:





Hopefully hesai SDK I can resolve soon on the header stamp side, I am diving deep into there SDK.

[aninath93]

Hi Mathieu,

Thank you again, I have 2 questions please, one with ouster (Lidar_deskewing) and one with Hesai timestamp ( as you mentioned in part 2 for the

OUSTER:

deskewing failed, warning in the picture below: (I get this same message even if I just lidar_deskewing node without icp_odom node), is this tf buffer issue, time ? (I am printing the message at the node) ?



EKF configuration:


Launch file:


HESAI: (getting same errror as Ouster)


Launch file is same, but with pointcloud from Hesai.

Can you please advise ?

[matlabbe]

Would have to debug from where "rtabmap_conversions::deskew" is returning false.

[aninath93]

For OUSTER OS0-32 testing in deskewing:

I put some warning loggers, the nanoseconds field is changing coming from lidar, don't know why still I am getting this error of first and last stamp are same ! (Could it be adding of times ? nanoseconds and header stamp ?) Any inputs on this, I would appreciate it.



I don't know but if you can test this function maybe ?

[matlabbe]

Can you record 30 seconds of ouster point cloud and share it?