ICP odometry with only pointclouds

ashBabu commented 1 year ago

Hi, I am on Ubuntu 22.04, ROS Humble and installed Rtabmap from apt. I have a realsense D435i camera with which I would like to do pointcloud odometry. Reading through the docs, I gather that ICP odometry is the way forward. I am assuming that if I use ICP odometry, I dont have to provide any RGB topics (is this correct?)

The launch file is

include_rtabmap_ros = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('rtabmap_launch'), 'launch', 'rtabmap.launch.py')),
        launch_arguments={
            "rtabmap_args": "--delete_db_on_start",
            # "rgb_topic": "/camera/color/image_raw",
            "depth_topic": "/camera/aligned_depth_to_color/image_raw",
            # "camera_info_topic": "/camera/color/camera_info",
            "depth_camera_info_topic": "/camera/depth/camera_info",
            "scan_cloud_topic": "/camera/depth/color/points",
            "frame_id": arg_frame_id,
            "vo_frame_id": "odom",
            "subscribe_scan_cloud": "true",
            "approx_sync": "true",
            "approx_sync_max_interval": "0.01",
            "wait_imu_to_init": "true",
            "imu_topic": "/imu/data",
            "qos": "2",
            "rviz": "true",
            "publish_tf": "true",
            "use_sim_time": "false",
            "visual_odometry": "false",
            "icp_odometry": "true",
            "gps_topic": arg_gps_topic,
            "rtabmapviz": "false",
            "queue_size": "200",
            "publish_tf_map": "true",
            "output": "both"
        }.items()
    )
    ld.add_action(include_rtabmap_ros)

I get this error

rtabmap-5] [WARN] [1699287178.657411064] [rtabmap.rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=20). 
[rtabmap-5] rtabmap subscribed to (approx sync):
[rtabmap-5]    /rtabmap/odom \
[rtabmap-5]    /camera/rgb/image_rect_color \
[rtabmap-5]    /camera/aligned_depth_to_color/image_raw \
[rtabmap-5]    /camera/rgb/camera_info \
[rtabmap-5]    /camera/depth/color/points \
[rtabmap-5]    /rtabmap/odom_info
[icp_odometry-4] [INFO] [1699287178.714480015] [rtabmap.icp_odometry]: Odom: ratio=0.757709, std dev=0.002070m|0.000655rad, update time=0.155286s
[icp_odometry-4] [ERROR] [1699287178.776220349] [rtabmap.icp_odometry]: Overwriting previous data! Make sure IMU is published faster than data rate. (last image stamp buffered=1699287178.664224 and new one is 1699287178.697580, last imu stamp received=1699287178.507793)
[icp_odometry-4] [ERROR] [1699287178.789228036] [rtabmap.icp_odometry]: Overwriting previous data! Make sure IMU is published faster than data rate. (last image stamp buffered=1699287178.697580 and new one is 1699287178.730936, last imu stamp received=1699287178.527796)
[icp_odometry-4] [INFO] [1699287179.927257766] [rtabmap.icp_odometry]: Odom: ratio=0.738938, std dev=0.002222m|0.000703rad, update time=1.136457s

What am I doing wrong?

ashBabu commented 1 year ago

A bit more info. Launching the realsense camera as

include_rs_cam = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('realsense2_camera'), 'launch', 'rs_launch.py')),
        launch_arguments={
            "align_depth.enable": "true",
            "depth_module.profile": "640x480x30",
            "unite_imu_method": "2",  # for linear_interpolation
            "enable_gyro": "true",
            "enable_accel": "true",
        }.items()
    )
    ld.add_action(include_rs_cam)

    include_container = ComposableNodeContainer(
        name='pointcloud_from_depth',   # pointcloud.enable = true in rs_launch.py won't produce pointcloud in Pi
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='rtabmap_util',
                plugin='rtabmap_util::PointCloudXYZ',
                name='points_xyz_rt',
                remappings=[
                    ("depth/image", "/camera/depth/image_rect_raw"),
                    ("depth/camera_info", "/camera/depth/camera_info"),
                    ("cloud", "/camera/depth/color/points")
                ],
                parameters=[
                    {"decimation": 4},
                    {"voxel_size": 0.05},
                    {"approx_sync": True}
                ]
            ),
        ],
        output='screen',
    )
    ld.add_action(include_container)

A rosbag of all the topics is available at rosbag

matlabbe commented 1 year ago

icp_odometry will run poorly or not at all with D435i's point cloud because it is a stereo camera. I'll suggest to look at the D435i examples here: https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch

ashBabu commented 1 year ago

Thank you @matlabbe. Closing this

introlab / rtabmap_ros

ICP odometry with only pointclouds #1061