How to call up a 3D map in rviz?

[miku54]

Hello everyone! --I am debugging the RTABMAP Slam function on Jetson Orin nano. I use Astra camera instead of Intel D435*. Although the camera has no odometry data, I publish the odometry topic named odom_combined from another node. --My RGB camera (/camera/color/image_raw) and depth camera (/camera/depth/image_raw) data size are both 640×480. Refer to https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3_rgbd_sync.launch.py ​​file to modify the camera data. After running rtabmap, open rviz and no 3D MAP appears. My 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')
    qos = LaunchConfiguration('qos')
    localization = LaunchConfiguration('localization')

    icp_parameters={
          'odom_frame_id':'odom_combined',
          'qos':qos
    }

    rtabmap_parameters={
          'subscribe_rgbd':True,
          'subscribe_scan':True,
          'use_action_for_goal':True,
          'odom_sensor_sync': True,
          'qos_scan':qos,
          'qos_image':qos,
          'qos_imu':qos,
          # RTAB-Map's parameters should be strings:
          'Mem/NotLinkedNodesKept':'false'
    }

    # Shared parameters between different nodes
    shared_parameters={
          'frame_id':'camera_link',
          'use_sim_time':use_sim_time,
          # RTAB-Map's parameters should be strings:
          'Reg/Strategy':'1',
          'Reg/Force3DoF':'true',
          'Mem/NotLinkedNodesKept':'false',
          'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry
    }

    remappings=[
          ('odom', '/odom_combined'),
          ('scan', '/scan'),
          ('rgb/image', '/camera/color/image_raw'), 
          ('rgb/camera_info', '/camera/color/camera_info'),
          ('depth/image', '/camera/depth/image_raw')]

    return LaunchDescription([

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

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

        DeclareLaunchArgument(
            'localization', default_value='false', choices=['true', 'false'],
            description='Launch rtabmap in localization mode (a map should have been already created).'),

        # Nodes to launch
        Node(
            package='rtabmap_sync', executable='rgbd_sync', output='screen',
            parameters=[{'approx_sync':True, 'use_sim_time':use_sim_time, 'qos':qos}],
            remappings=remappings),

        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[icp_parameters, shared_parameters],
            remappings=remappings,
            arguments=["--ros-args", "--log-level", 'icp_odometry:=warn']),

        # SLAM Mode:
        Node(
            condition=UnlessCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[rtabmap_parameters, shared_parameters],
            remappings=remappings,
            arguments=['-d']),

        # Localization mode:
        Node(
            condition=IfCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[rtabmap_parameters, shared_parameters,
              {'Mem/IncrementalMemory':'False',
               'Mem/InitWMWithAllNodes':'True'}],
            remappings=remappings),

    ])

--Use the following command to run rtabmap with the same effect: ros2 launch rtabmap_launch rtabmap.launch.py ​​visual_odometry:=false \ rtabmap_args:="--delete_db_on_start" \ rgb_topic:=/camera/color/image_raw \ depth_topic:=/camera/depth/image_raw \ camera_info_topic :=/camera/color/camera_info \ odom_topic:=/odom_combined \ frame_id:=base_footprint \ approx_sync:=true \ rtabmap_viz:=false \ wait_imu_to_init:=false \ qos:=1 \ rviz:=false

Expected Results： Actual Results：

[matlabbe]

Try adding rtabmap_ros/MapCloud display. It looks what it was used in the other screen shot to show the 3D point cloud.

[miku54]

Try adding rtabmap_ros/MapCloud display. It looks what it was used in the other screen shot to show the 3D point cloud.

I tried adding "MapCloud" from "add->by display type->rtabmap_rviz_plugins", and selecting /rtabmap/mapData or /mapData, but it didn't show up. Is my approach wrong or is there something wrong? Please help me, I can't find a solution.

[matlabbe]

Can you record a rosbag with /rtabmap/mapData? and share the corresponding ~/.ros/rtabmap.db created?

[miku54]

Can you record a rosbag with /rtabmap/mapData? and share the corresponding ~/.ros/rtabmap.db created?

Thanks for your reply. This is a piece of data I recorded. If you need more information, please contact me. https://drive.google.com/file/d/11ot1RBlt3gAa9zuNDejJr3MTBuwsawN9/view?usp=drive_link

[matlabbe]

There is something wrong with the calibration, rtabmap doesn't see it as valid for some reason, so clouds cannot be created from depth (note also that depth is not always valid):

Can you record raw images while moving slightly? ros2 bag record /camera/color/camera_info /camera/color/image_raw /camera/depth/image_raw

[miku54]

There is something wrong with the calibration, rtabmap doesn't see it as valid for some reason, so clouds cannot be created from depth (note also that depth is not always valid):

Can you record raw images while moving slightly? ros2 bag record /camera/color/camera_info /camera/color/image_raw /camera/depth/image_raw

Thank you for your analysis. I did not do camera calibration before running rtab. Is it necessary to do camera calibration? Please wait for the raw images data after moving. I will upload it in 2 hours. Thank you again for your reply.

[matlabbe]

You may not need to calibrate, if the camera driver already provides one. I would expect that the Astra RGB-D camera provides already that kind of information, but that's why I want to look in the camera_info. The depth images look strange too. Maybe there is an issue with the camera/driver.

[miku54]

You may not need to calibrate, if the camera driver already provides one. I would expect that the Astra RGB-D camera provides already that kind of information, but that's why I want to look in the camera_info. The depth images look strange too. Maybe there is an issue with the camera/driver.

Sorry for the delay due to some personal matters. Here are the data I recorded and the corresponding db map file. If you need other data, I can provide it at any time: https://drive.google.com/file/d/1rMxsKN1mARFVBBW6QMp_NRpZnHZKizUP/view?usp=sharing

I observed that the error "util3d.cpp:1186::cloudsRGBFromSensorData() Camera model 0 is invalid" would appear during the run. I referred to https://github.com/introlab/rtabmap_ros/issues/866, but it did not help me.

[miku54]

You may not need to calibrate, if the camera driver already provides one. I would expect that the Astra RGB-D camera provides already that kind of information, but that's why I want to look in the camera_info. The depth images look strange too. Maybe there is an issue with the camera/driver.

Sorry for the delay due to some personal matters. Here are the data I recorded and the corresponding db map file. If you need other data, I can provide it at any time: https://drive.google.com/file/d/1rMxsKN1mARFVBBW6QMp_NRpZnHZKizUP/view?usp=sharing

I observed that the error "util3d.cpp:1186::cloudsRGBFromSensorData() Camera model 0 is invalid" would appear during the run. I referred to #866, but it did not help me.

Added: I have saved other db map files and used rtabmap-database tool to check [Calib], all of them are NA, so I think there is no problem with my camera driver.

[matlabbe]

The camera_info is wrong, this is an invalid projection matrix

p:
- 570.3422047415297
- 0.0
- 0.0
- 0.0
- 0.0
- 570.3422047415297
- 570.3422047415297
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0

It should look like this (based on k):

p:
- 570.3422047415297
- 0.0
- 319.5
- 0.0
- 0.0
- 570.3422047415297
- 239.5
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0

[miku54]

The camera_info is wrong, this is an invalid projection matrix

p:
- 570.3422047415297
- 0.0
- 0.0
- 0.0
- 0.0
- 570.3422047415297
- 570.3422047415297
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0

It should look like this (based on k):

p:
- 570.3422047415297
- 0.0
- 319.5
- 0.0
- 0.0
- 570.3422047415297
- 239.5
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0

Thanks for your reply. When I tested with other cameras, the camere_info data looked normal. After running rtab map, I could see the 3D map in rviz2. Thanks again for your help.😊