Map overlap

[vanbacvb]

OS: ubuntu 22.04
ROS2 humble

I use RTABmap to draw map using only 3D lidar signal from SICK nanoscan100 and odomentry from wheel. but map is overlapped. can you help me fix it? thanks

launch file:

def generate_launch_description():
    ld = LaunchDescription()
    sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd')
    sick_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_multiscan_rtabmap.launch')
    sick_node_arguments=[sick_launch_file_path]
    
    # Append optional commandline arguments in name:=value syntax
    for arg in sys.argv:
        if len(arg.split(":=")) == 2:
            sick_node_arguments.append(arg)
    # Create sick_scan_xd node
    sick_node = Node(
        package='sick_scan_xd',
        executable='sick_generic_caller',
        output='log',
        arguments=sick_node_arguments
    )

    # Common parameter
    wait_for_transform = 0.01
    point_cloud_topic = "/cloud_all_fields_fullframe" # Point cloud topic published by sick_scan_xd
    point_cloud_frame_id = "base_cloud"                    # Point cloud frame id published by sick_scan_xd
    imu_topic = "imu_not_used" #"/multiScan/imu"                      # IMU topic published by sick_scan_xd
    deskewing = True                                  # Optional lidar deskewing

    if deskewing: # Create deskewing node
        point_cloud_topic_desk = f"{point_cloud_topic}/deskewed"
        deskewing_node = Node(
            package='rtabmap_util', executable='lidar_deskewing', name="lidar_deskewing", output="screen",
            parameters=[{
                "wait_for_transform": wait_for_transform,
                "fixed_frame_id": point_cloud_frame_id,
                "slerp": False,
             }],
            remappings=[("input_cloud", point_cloud_topic)],
            arguments=[],
            namespace="deskewing")
    else:
        point_cloud_topic_desk = point_cloud_topic
        deskewing_node = None

    # Create rtabmap_odom node
    rtabmap_odom_node = Node(
            package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
            parameters=[{

                
                "frame_id": point_cloud_frame_id,
                "odom_frame_id": "odom",
                "guess_frame_id": point_cloud_frame_id,
                "wait_imu_to_init": False,
                "wait_for_transform_duration": str(wait_for_transform),
                "Icp/PointToPlane": "True",
                "Icp/Iterations": "10",
                "Icp/VoxelSize": "0.2",
                "Icp/DownsamplingStep": "1",
                "Icp/Epsilon": "0.001",
                "Icp/PointToPlaneK": "20",
                "Icp/PointToPlaneRadius": "0",
                "Icp/MaxTranslation": "2",
                "Icp/MaxCorrespondenceDistance": "1",
                "Icp/PM": "True",
                "Icp/PMOutlierRatio": "0.1",
                "Icp/CorrespondenceRatio": "0.01",
                "Icp/ReciprocalCorrespondences": "False",
                "Odom/ScanKeyFrameThr": "0.8",
                "Odom/Strategy": "0",
                "OdomF2M/ScanSubtractRadius": "0.2",
                "OdomF2M/ScanMaxSize": "15000",
            }],
            remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic)],
            arguments=[],
            namespace="rtabmap")


    # Create rtabmap_slam node, see https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_launch/launch/rtabmap.launch.py for an example configuration
    rtabmap_slam_node = Node(
            package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
            parameters=[{
                'Grid/RayTracing': 'true',
                'Grid/FromDepth': True,
                "subscribe_odom_info": False,
                "publish_tf_odom": False,
                "frame_id": point_cloud_frame_id,
                "subscribe_depth": False,
                "subscribe_rgb": False,
                "subscribe_rgbd": False,
                "subscribe_scan": False,
                "subscribe_scan_cloud": True,
                "approx_sync": True,
                "Rtabmap/DetectionRate": "1",
                "RGBD/NeighborLinkRefining": "False",
                "RGBD/ProximityBySpace": "True",
                "RGBD/ProximityMaxGraphDepth": "0",
                "RGBD/ProximityPathMaxNeighbors": "1",
                "RGBD/AngularUpdate": "0.05",
                "RGBD/LinearUpdate": "0.05",
                "Mem/NotLinkedNodesKept": "False",
                "Mem/STMSize": "30",
                "Reg/Strategy": "1",
                "Grid/CellSize": "0.1",
                "Grid/RangeMax": "20",
                "Grid/ClusterRadius": "1",
                "Grid/GroundIsObstacle": "True",
                "Optimizer/GravitySigma": "0.3",
                "Icp/VoxelSize": "0.3",
                "Icp/PointToPlaneK": "20",
                "Icp/PointToPlaneRadius": "0",
                "Icp/PointToPlane": "False",
                "Icp/Iterations": "10",
                "Icp/Epsilon": "0.001",
                "Icp/MaxTranslation": "3",
                "Icp/MaxCorrespondenceDistance": "1",
                "Icp/PM": "True",
                "Icp/PMOutlierRatio": "0.7",
                "Icp/CorrespondenceRatio": "0.4",
            }],
            remappings=[("scan_cloud", point_cloud_topic_desk), ("imu", imu_topic),('odom', 'odom')],
            arguments=["-d"], # parameter "-d": delete_db_on_start
            namespace="")

    # Create rtabmap_viz node
    rtabmap_viz_node = Node(
            package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output="screen",
            parameters=[{
                "frame_id": point_cloud_frame_id,
                "odom_frame_id": "odom",
                "subscribe_odom_info": False,
                "subscribe_scan_cloud": True,
                "approx_sync": False,
             }],
            remappings=[("scan_cloud", point_cloud_topic_desk)],
            arguments=[],
            namespace="rtabmap")

    ld.add_action(sick_node)
    if deskewing:
        ld.add_action(deskewing_node)
    # ld.add_action(rtabmap_odom_node)
    ld.add_action(rtabmap_slam_node)
    ld.add_action(rtabmap_viz_node)
    return ld

[matlabbe]

That is the TF published by wheel odometry? Is it odom? If so, try for icp_odometry:

"frame_id": point_cloud_frame_id,
"odom_frame_id": "icp_odom",
"guess_frame_id": "odom",

...
#remaps:
("odom", "icp_odom")

If you do deskweing, use odom (wheel odometry) as fixed_frame_id. However, wheel odometry should be published faster than lidar and be pretty accurate under lidar full rotation time. Otherwise, you may enable deskewing inside icp_odometry node instead.

See also demos on this page: https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos

[vanbacvb]

yes. i use tf odom -> base_footprint from the odometry of the wheel. i dont want to use icp_odometry because it has oscillation when the robot is stationary.