Ask Your Question
0

[SOLVED] RTABMAP does not generate accumulated 3d points in rgbd mode

asked 2019-08-15 20:11:07 -0500

Avner gravatar image

updated 2019-08-16 17:18:01 -0500

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

<launch>
  <!-- Convenience launch file to launch ...
(more)
edit retag flag offensive close merge delete

Comments

Could you please post your launch structure to launch this? Can you be sure that everything is still running as expected? Although you have a stereo pair, as I understand, you are trying to use an RGB image and a depth image (presumably from stereo_image_proc)? Perhaps try using the RGBD launch file now?

PapaG gravatar imagePapaG ( 2019-08-15 22:39:38 -0500 )edit

is this a duplicate of #q330832 or a follow-up?

mgruhler gravatar imagemgruhler ( 2019-08-16 01:24:40 -0500 )edit
1

This is a followup. In #q330832, I had problems with the rgb_odometry that RTABMAP uses, which I was able to solve. Here the problem is that although rgbd_odometry works, I do not get a point cloud from RTABMAP.

Avner gravatar imageAvner ( 2019-08-16 11:47:58 -0500 )edit

@PapaG I posted the launch files, and the running processes

I don't know what you mean by "everything". Some things are still running in the sense, that: I can show the right image streaming in Rviz, and some rtabmap topics such as /rtabmap/odom are publishing data

The camera can deliver stereo pair, camera_info, and depth image, all synced.

In stereo mode, the camera sends stereo pair (left and right), and camera_info. That works OK.

In rgbd mode, the camera sends rgb image, depth image and camera_info. That does not work.

Avner gravatar imageAvner ( 2019-08-16 12:13:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-08-16 17:17:39 -0500

Avner gravatar image

updated 2019-08-16 17:18:40 -0500

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)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-08-15 20:11:07 -0500

Seen: 15 times

Last updated: Aug 16