Correct occupancy grid map generated using point cloud obtained from depth camera and odometry from turtlebot

[sagardhatrak]

Hi,
we are trying to generate map using point cloud provided by camera. We have mounted camera on turtlebot. Odometry is obtained from wheel encoders of turtlebot.

But not able to generate correct map and odometry. We are getting overlapped point cloud map and occupancy grid map as shown in images provided.

How Can we generate correct map and odometry ?

[matlabbe]

What kind of "depth camera" is it? Do you have a RGB image too? If you are using scan_cloud input, it seems pretty dense to do odometry on it. You will need to downsample the point cloud, otherwise ICP will take a lot of time to process.

[sagardhatrak]

Sorry for incorrect title. We are using ADI ToF, which provides IR and depth images. There is no RGB image. we have to perform two experiments.

1. We have to use odometry obtained from wheel encoders of turtlebot and want to generate map using point cloud.
2. We have to generate odometry from point cloud as icp odometry and map from point cloud.

Initially, we started with first experiments. The above results are of odometry from turtlebot wheel encoders and map is generated using point cloud. if you can help me obtain good map and odometry for both experiments, it will be really helpful. we are very eager to learn and work on rtabmap.

it will also be helpful if we can obtain odometry from down sampled point cloud.

Thank you very much for early reply as we have to perform various experiments with rtabmap.

This my launch file:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
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')
    qos = LaunchConfiguration('qos')

    parameters=[{
          'frame_id':'base_footprint',
          'use_sim_time':use_sim_time,
          'subscribe_depth':False,
          'subscribe_rgb':False,
          'subscribe_scan':False,
          'subscribe_scan_cloud':True,
          'approx_sync':True,
          'use_action_for_goal':True,
          'qos_scan':qos,
          'qos_imu':qos,
          'Reg/Force3DoF':'true',
          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
        }]

    remappings=[
        ('scan_cloud', '/tof_camera/point_cloud2'),
        # ('odom', '/tof_camera/odom')
        ('odom', '/odom')
        ]

    return LaunchDescription([

          # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument(
            'qos', default_value='2',
            description='QoS used for input sensor topics'),

        Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            output="screen" ,
            arguments=["0", "0", "0", "-1.86", "0", "-1.76", "base_link", "camera_link"]
        ), 

        Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            output="screen" ,
            arguments=["0", "0", "0", "0", "0", "0", "camera_link", "aditof_pointcloud2"]
        ),

        Node(
            package='rtabmap_ros', executable='rtabmap', output='screen',

        parameters=[{
          'frame_id':'base_footprint',
          'odom_frame_id':'odom',
          'use_sim_time':use_sim_time,
          'subscribe_depth':False,
          'subscribe_rgb':False,
          'subscribe_scan':False,
          'subscribe_scan_cloud':True,
          'approx_sync':True,
          'use_action_for_goal':True,
          'qos_scan':qos,
          'qos_imu':qos,
          'Reg/Strategy':'1',
          'Reg/Force3DoF':'true',
          'RGBD/NeighborLinkRefining':'True',

          'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)

            }], #False,

            remappings=[
            ('rgb/camera_info', '/tof_camera/camera_info'),
            ('imu', '/imu_turtle'),
            ('odom', '/odom'),
            # ('odom', '/tof_camera/odom'),
            ('scan_cloud', '/tof_camera/point_cloud2')],
            arguments=['-d']),

        Node(
            package='rtabmap_ros', executable='rtabmapviz', output='screen',
            parameters=parameters,
            remappings=remappings),
    ])

[matlabbe]

If you can record a rosbag with all topics and share it, that could be useful.

[sagardhatrak]

Hi, Please find the recorded rosbag link.

https://drive.google.com/file/d/1M8IPJKjE7iXZYbrmGzZj2sJ6yMaJJ3Qf/view?usp=share_link

[matlabbe]

The image looks distorted (images should be rectified to be used in rtabmap), as well as the point cloud: why are the surfaces not flat but curvy? It seems there is also a scale issue, the digikey box looks small, but in point cloud, it looks 2x3 meters. I cannot really take time to tune parameters if input data is not right.

[sagardhatrak]

Thanks for guidance. We are trying to obtain the calibrated images and point cloud. Once it is done, will come back.

[sagardhatrak]

There was issue with camera calibration matrix and camera firmware. Issue has been resolved after correction in camera calibration matrix and modification in camera firmware.