0
$\begingroup$

Rosanswers logo

Hi,

I am running RTABMAP against a stereo camera that provides left and right and depth images.

When I run RTABMAP in stereo_mode (based on left and right image pairs) RTABMAP is working OK (e.g. I can see the 3d point cloud being build in Rviz via the topic /rtabmap/mapData)

Now I want to test RTABMAP in rgbd mode (based on right and depth image pairs).

In this mode the rgbd_odometry runs OK, and produces odometry results (snippet1). I also echo the /rtabmap/odom topic and I can see published data.

But nothing is published for topic /rtabmap/mapData.

RTABMAP does publish data for topic /rtabmap/odom_local_map (see snippet2)

To debug the problem, I list odom_local_map in Rviz and set the field "Fixed Frame" to "odom", but nothing is displayed in the 3d pane.

How can I debug the problem, to see the accumulated 3d point cloud in Rviz?

Or, how can I debug the problem, to see intermediate results such as odom_local_map in Rviz?

Thanks,

Avner


EDIT:

The RTABMAP launch files are listed in snippet3, snippet4

The ros related processes are in snippet5


snippet1 - rgb_odometry runs ok

[ INFO] [1565916017.025276774, 1556057414.818000078]: Odom: quality=508, std dev=0.000465m|0.000032rad, update time=0.028195s
[ INFO] [1565916017.052003568, 1556057414.818000078]: Odom: quality=513, std dev=0.000365m|0.000032rad, update time=0.026503s
[ INFO] [1565916017.117518421, 1556057414.884999990]: Odom: quality=513, std dev=0.000324m|0.000032rad, update time=0.027049s...

snippet2 - I can echo topic from /rtabmap/odom_local_map

header: 
  seq: 6484
  stamp: 
    secs: 1556058813
    nsecs: 713999987
  frame_id: "odom"
height: 1
width: 550
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "rgb"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32
row_step: 17600
data: [55, 45, 144, 194, 0, 0, 0, 0, 14, 209, 130 ...

snapshot3 - RTABMAP launch file

<?xml version="1.0"?>
<launch>
    <!-- Choose visualization -->
    <arg name="rviz"       default="true" />
    <arg name="rtabmapviz" default="false" />

    <arg name="rate"       default="30" />

    <arg name="rviz_cfg"                default="/home/catkin_ws/src/slam_rtabmap/launch/slam.rtabmap.live2.rviz" />

    <param name="/use_sim_time" value="true"/>

    <arg name="pi/2" value="1.5707963267948966" />
    <include file="/home/catkin_ws/src/slam_rtabmap/launch/live1_tf.launch"></include>

    <include file="/home/catkin_ws/src/slam_rtabmap/launch/rtabmap.base.launch">
        <arg name="rtabmap_args" value="--delete_db_on_start" />
        <arg name="stereo"           value="false" />
        <arg name="subscribe_rgbd"           value="true" />

        <arg name="queue_size"           value="10" />
        <arg name="rgbd_sync"               default="true"/>
        <arg name="approx_rgbd_sync"        default="false"/>
        <!-- <arg name="frame_id"                default="censys_stern"/> -->

        <arg name="rviz"             value="$(arg rviz)" />
        <arg name="rtabmapviz"       value="$(arg rtabmapviz)" />
        <arg name="rviz_cfg"       value="$(arg rviz_cfg)" />
        <arg name="rgb_topic"               value="/rectified_right" />
        <arg name="depth_topic"             value="/3d_z_coords" />
        <arg name="camera_info_topic"       value="/right/camera_info" />
    </include>
</launch>

snippet4 - RTABMAP base launch file

This code block was moved to the following github gist: https://gist.github.com/answers-se-migration-openrobotics/7d52079a3a0da172a2d0589405a4a206

snippet5 - ros related processes

ps aux | grep -i ros

/usr/bin/python /opt/ros/melodic/bin/roslaunch /home/avnerm/avner/PGR/vision/isv/branches/ISV-11/catkin_ws/src/slam_rtabmap/launch/rtabmap.live1.3dpoints.launch
grep --color=auto -i ros

/opt/ros/melodic/lib/tf/static_transform_publisher 0 0.0 0 0 0 0 /camera_link /camera_rgb_frame 100 __name:=camera_base_link1 __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/camera_base_link1-2.log

/opt/ros/melodic/lib/nodelet/nodelet standalone rtabmap_ros/rgbd_sync rgb/image:=/rectified_right depth/image:=/3d_z_coords rgb/camera_info:=/right/camera_info rgbd_image:=rgbd_image __name:=rgbd_sync __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/rtabmap-rgbd_sync-3.log

/opt/ros/melodic/lib/rtabmap_ros/rgbd_odometry --delete_db_on_start rgb/image:=/rectified_right depth/image:=/3d_z_coords rgb/camera_info:=/right/camera_info rgbd_image:=rgbd_image odom:=odom __name:=rgbd_odometry __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/rtabmap-rgbd_odometry-4.log

/opt/ros/melodic/lib/rviz/rviz -d /home/avnerm/avner/PGR/vision/ros/branches/RES-104/catkin_ws/src/slam_rtabmap/launch/slam.rtabmap.live2.rviz __name:=rviz __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/rviz-6.log

/opt/ros/melodic/lib/nodelet/nodelet standalone rtabmap_ros/point_cloud_xyzrgb rgbd_image:=rgbd_image cloud:=voxel_cloud1 __name:=points_xyzrgb __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/points_xyzrgb-7.log

/usr/bin/python /opt/ros/melodic/bin/roscore

/usr/bin/python /opt/ros/melodic/bin/rosmaster --core -p 11311 -w 3 __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/master.log

/opt/ros/melodic/lib/rosout/rosout __name:=rosout __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/rosout-1.log

/home/avnerm/avner/PGR/vision/isv/branches/ISV-11/catkin_ws/devel/lib/rm_mapping_node --doPublishCameraInfo --doEnableXYZ --doPublishClock __name:=rm_mapping_node __log:=/home/avnerm/.ros/log/fe3d9456-bfba-11e9-975a-74d02b912ce1/rm_mapping_node-1.log

Originally posted by Avner on ROS Answers with karma: 96 on 2019-08-15

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The reason for the failure was due to streaming the images in resolution of 384x240, but using a camera_info .yaml file that was based on a different resolution (640x400).

This caused a mismatch between the images size of 3d_z_coords and rectified_right depth/image (both are 384x240) and camera_info (640x400), which caused the following error message, when running the launch file:

[FATAL] (2019-08-16 14:21:54.219) util3d.cpp:446::cloudFromDepthRGB() Condition ((model.imageHeight() == 0 && model.imageWidth() == 0) || (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols)) not met! [model=640x400 rgb=384x240]                                                                                                                                                           
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2019-08-16 14:21:54.219) util3d.cpp:446::cloudFromDepthRGB() Condition ((model.imageHeight() == 0 && model.imageWidth() == 0) || (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols)) not met! [model=640x400 rgb=384x240]

[rtabmap/rtabmap-5] process has died [pid 8562, exit code -6, cmd /opt/ros/melodic/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/rm_mapping_node/cameras/stern/rectified_right depth/image:=/rm_mapping_node/cameras/stern/3d_z_coords rgb/camera_info:=/rm_mapping_node/cameras/stern/right/camera_info rgbd_image:=rgbd_image left/image_rect:=/stereo_camera/left/image_rect_color right/image_rect:=/stereo_camera/right/image_rect left/camera_info:=/stereo_camera/left/camera_info right/camera_info:=/stereo_camera/right/camera_info scan:=/scan scan_cloud:=/scan_cloud user_data:=/user_data user_data_async:=/user_data_async odom:=odom __name:=rtabmap __log:=/home/avnerm/.ros/log/71f75a64-c063-11e9-b1ee-74d02b912ce1/rtabmap-rtabmap-5.log].                         

                                         

After changing the resolution of streamed images to 640x400 the topic /rtabmap/mapData does publish data, and Rviz does show the accumulated cloud (the quality is not good but this is unrelated issue)


Originally posted by Avner with karma: 96 on 2019-08-16

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.