RGBD Mapping issue

[Dalalos]

Hello all,

I'm new to ROS and SLAM and I'm trying to implement 3D mapping using rtabmap and realsense r200 camera.

The two launch files I'm using (attached) are:

realsense_r200_nodelet_standalone_manual.launch
rtabmap.launch

Now I'm in a point that I can see all the images from the camera, I can see all the axes and their transformation and I also can see a different point-clouds with the right 3D location.

The problem is that I cant see a map (no map received). rtabmap/rtabmap fails every time I'm running the launch file.
I think it something to do with the resolution of the camera because it's demanding me to use the same resolution for the color and depth image and when I do it's fails with opencv error in reasize.cpp file.

Different resolution:

[FATAL] [1514359403.865104643]: ASSERTION FAILED
    file = /home/gal/catkin_ws/src/rtabmap_ros/src/nodelets/point_cloud_xyzrgb.cpp
    line = 238
    cond = imageDepthPtr->image.cols == imagePtr->image.cols
[points_xyzrgb-5] process has died

[FATAL] (2017-12-27 09:24:34.764) CoreWrapper.cpp:870::commonDepthCallback() Condition (depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight) not met! [imageWidth=640 vs 480 imageHeight=480 vs 360]a
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2017-12-27 09:24:34.764) CoreWrapper.cpp:870::commonDepthCallback() Condition (depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight) not met! [imageWidth=640 vs 480 imageHeight=480 vs 360]
[rtabmap/rtabmap-3] process has died [pid 8715, exit code -6, cmd /home/gal/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/camera/color/image_raw depth/image:=/camera/depth/image_raw rgb/camera_info:=/camera/color/camera_info scan:=/scan scan_cloud:=/scan_cloud __name:=rtabmap __log:=/home/gal/.ros/log/df1514f0-ead2-11e7-9fc2-001e06302d57/rtabmap-rtabmap-3.log].

Same resolution:

/rtabmap/rtabmap subscribed to (approx sync):
   /camera/color/image_raw,
   /camera/depth/image_raw,
   /camera/color/camera_info,
   /rtabmap/odom
[ INFO] [1514359767.896568156]: rtabmap 0.11.8 started...
[ INFO] [1514359768.020182663]: Odom: quality=141, std dev=0.007423m, update time=0.056269s
[ INFO] [1514359768.147217906]: Odom: quality=150, std dev=0.006155m, update time=0.064382s
OpenCV Error: Bad argument (Unknown interpolation method) in resize, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/resize.cpp, line 3164
terminate called after throwing an instance of 'cv::Exception'
  what():  /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/resize.cpp:3164: error: (-5) Unknown interpolation method in function resize

[ INFO] [1514359768.262800384]: Odom: quality=172, std dev=0.007795m, update time=0.066269s
[ INFO] [1514359768.352276111]: Odom: quality=174, std dev=0.007170m, update time=0.051930s
[ INFO] [1514359768.484443269]: Odom: quality=159, std dev=0.007719m, update time=0.074119s
[rtabmap/rtabmap-3] process has died [pid 8862, exit code -6, cmd /home/gal/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/camera/color/image_raw depth/image:=/camera/depth/image_raw rgb/camera_info:=/camera/color/camera_info scan:=/scan scan_cloud:=/scan_cloud __name:=rtabmap __log:=/home/gal/.ros/log/78b0e0fe-ead7-11e7-90c6-001e06302d57/rtabmap-rtabmap-3.log].
log file: /home/gal/.ros/log/78b0e0fe-ead7-11e7-90c6-001e06302d57/rtabmap-rtabmap-3*.log

What am I doing wrong? I can really use your help in solving this,
Thanks, Gal.

rtabmap1.launch
realsense_r200_nodelet_standalone_manual.launch

[matlabbe]

The error says that RGB and depth image are not the same size, as it should. rtabmap requires that we have a registered depth image to color camera. Are R200 instructions from this page work?

[Dalalos]

When i try to run rtabmap without register the depth image it's going well and then it's got stuck after rtabmap node comes up without any error.
I didne't think registering the depth image is necessary since I saw someone runing the same process without registering the depth image:
https://www.youtube.com/watch?v=Xc1WhmFCjcc



.
.
.
[ INFO] [1514574281.566786401]: Odom: quality=190, std dev=0.072548m, update time=0.049244s
[ INFO] [1514574291.060535729]: Odom: quality=159, std dev=0.079305m, update time=0.055836s
[ INFO] [1514574295.219242956]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1514574295.219337012]: rtabmap: Using database from "/home/gal/.ros/rtabmap.db".
[ INFO] [1514574295.225569958]: rtabmap: Database version = "0.11.8".
[ INFO] [1514574298.319736424]:
/rtabmap/rtabmap subscribed to (approx sync):
   /camera/color/image_raw,
   /camera/depth/image_raw,
   /camera/color/camera_info,
   /rtabmap/odom
[ INFO] [1514574298.319872056]: rtabmap 0.11.8 started...

When I do try registering the depth  image using different realsense launch file (similar to the one in the RBG-D tutorial) it does the same thing:


.
.
.
[ INFO] [1514574833.854113562]: Odom: quality=22, std dev=0.213201m, update time=0.028359s
[ INFO] [1514574846.165440958]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1514574846.165547201]: rtabmap: Using database from "/home/gal/.ros/rtabmap.db".
[ INFO] [1514574846.172617688]: rtabmap: Database version = "0.11.8".
[ INFO] [1514574849.554978793]:
/rtabmap/rtabmap subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/sw_registered/image_rect_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom
[ INFO] [1514574849.555129506]: rtabmap 0.11.8 started...

By the way, i'm running the Realsense r200 on Odroid XU4 running ros-indigo and rtabmap on mac-pro running ros-kinetic.
Could the different ROS versions could be an issue?

What else could be the issue?

I'm attaching the two launch files:
realsense_r200_rgbd.launch
rtabmap1.launch

 

[matlabbe]

Hi,

In the video, if you look at the top left terminal, the depth image is resized to 640/480 using an image_resize node. That depth image is passed to rtabmap so that rgb and depth images have the same size. I am not sure at 100% if the original depth image was registered to color camera, if so then just resizing the depth image can be another solution.

I don't see errors in outputs you have shown. Odometry seems working. Different ROS versions would work, as long as topic formats didn't have changed (you would be notified by an error if so).

As you are using two computers, make sure the clock is synchronized between them (with ntpdate), this will avoid topic synchronization problems on rtabmap side.

You may also compare the frame rates of /camera/rgb/image_rect_color, /camera/depth_registered/sw_registered/image_rect_raw and /camera/rgb/camera_info on mac-pro side (rostopic hz "topic_name").

cheers,
Mathieu

[Dalalos]

I think the clocks are synchronized, I used:
&sudo ntpdate -s time.nist.gov
in both computers.

This it the output of $rostopic hz "rostopic" for each topic[ INFO] [1514580405.484924168]: rtabmap 0.11.8 started...:

subscribed to [/camera/rgb/image_rect_color]
no new messages
average rate: 1.851
        min: 0.530s max: 0.551s std dev: 0.01054s window: 3
average rate: 1.827
        min: 0.530s max: 0.561s std dev: 0.01315s window: 4
average rate: 1.822
        min: 0.530s max: 0.562s std dev: 0.01265s window: 6
average rate: 1.819
        min: 0.530s max: 0.562s std dev: 0.01171s window: 8
average rate: 1.811
        min: 0.530s max: 0.562s std dev: 0.01132s window: 10
average rate: 1.817
        min: 0.530s max: 0.562s std dev: 0.01144s window: 12
average rate: 1.814
        min: 0.530s max: 0.562s std dev: 0.01126s window: 13
average rate: 1.809
        min: 0.530s max: 0.562s std dev: 0.01101s window: 15
average rate: 1.812
        min: 0.530s max: 0.562s std dev: 0.01104s window: 17
^Caverage rate: 1.812
        min: 0.530s max: 0.562s std dev: 0.01071s window: 18


subscribed to [/camera/depth_registered/sw_registered/image_rect_raw]
no new messages
average rate: 2.684
        min: 0.362s max: 0.380s std dev: 0.00799s window: 4
average rate: 2.699
        min: 0.362s max: 0.381s std dev: 0.00845s window: 7
average rate: 2.701
        min: 0.361s max: 0.381s std dev: 0.00833s window: 9
average rate: 2.714
        min: 0.360s max: 0.381s std dev: 0.00784s window: 12
average rate: 2.709
        min: 0.360s max: 0.381s std dev: 0.00808s window: 15
average rate: 2.706
        min: 0.360s max: 0.389s std dev: 0.00900s window: 18
average rate: 2.708
        min: 0.360s max: 0.389s std dev: 0.00862s window: 20
average rate: 2.706
        min: 0.360s max: 0.389s std dev: 0.00841s window: 23
^Caverage rate: 2.709
        min: 0.360s max: 0.389s std dev: 0.00845s window: 24


subscribed to [/camera/rgb/camera_info]
average rate: 64.113
        min: 0.000s max: 0.046s std dev: 0.01917s window: 47
average rate: 59.665
        min: 0.000s max: 0.047s std dev: 0.02025s window: 103
average rate: 59.088
        min: 0.000s max: 0.064s std dev: 0.02089s window: 161
average rate: 58.296
        min: 0.000s max: 0.091s std dev: 0.02160s window: 218
average rate: 57.538
        min: 0.000s max: 0.091s std dev: 0.02213s window: 273
average rate: 57.464
        min: 0.000s max: 0.093s std dev: 0.02223s window: 330
average rate: 57.139
        min: 0.000s max: 0.093s std dev: 0.02206s window: 385
^Caverage rate: 57.438
        min: 0.000s max: 0.093s std dev: 0.02197s window: 444

I do have a warning in the realsense cmd line after rtabmap.launch file has started but I don't think it's affect something:

[ WARN] [1514580683.544697686]: TF2 exception:
Lookup would require extrapolation into the future.  Requested time 1514580683.530019490 but the latest data is at time 1514580683.529112733, when looking up transform from frame [camera_depth_optical_frame] to frame [camera_rgb_optical_frame]

[matlabbe]

1.8 Hz is too slow for visual odometry. How do you transfer images between computers? By wifi?

To be able to syncrhonize between 1.8Hz and 60 Hz topics, increase queue_size to 60. However, the real problem is why you are not receiving images at 60 hz too. You may need to make rtabmap subscribing to compressed image topics instead of raw topics.

[Dalalos]

No, by cable.
This rates numbers changed after I launch rtabmap, before that the images - color and registered depth came at around 20Hz, the camera info stayed 60Hz.
I open my depth camera in 332x252 resolution it's really low.

[Dalalos]

I forgot to change queue_size to 60, now it won't stuck but it fails like my first message:

[ WARN] [1514624113.106971809]: rtabmapviz: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624111.489696)!
[ INFO] [1514624113.298395708]: Odom: quality=60, std dev=0.129099m, update time=0.028214s
[ INFO] [1514624113.612110067]: Odom: quality=57, std dev=0.132453m, update time=0.031802s
[ INFO] [1514624113.930270548]: Odom: quality=64, std dev=0.125000m, update time=0.030742s
[ INFO] [1514624114.253325916]: Odom: quality=63, std dev=0.125988m, update time=0.026296s
[ INFO] [1514624114.659521177]: Odom: quality=70, std dev=0.119523m, update time=0.029966s
[ WARN] [1514624114.687310421]: rtabmapviz: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624112.803140)!
[ INFO] [1514624115.143475180]: Odom: quality=70, std dev=0.119523m, update time=0.035584s
[ INFO] [1514624115.339404334]:
/rtabmap/rtabmap subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/sw_registered/image_rect_raw,
   /camera/rgb/camera_info,
   /rtabmap/odom
[ INFO] [1514624115.339556349]: rtabmap 0.11.8 started...
[ INFO] [1514624115.600067094]: Odom: quality=60, std dev=0.129099m, update time=0.025757s
[ WARN] [1514624115.699898795]: rtabmapviz: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624113.122915)!
[ INFO] [1514624116.076388202]: Odom: quality=66, std dev=0.123091m, update time=0.029299s
[ INFO] [1514624116.555891944]: Odom: quality=62, std dev=0.127000m, update time=0.038612s
[ WARN] [1514624116.724258233]: rtabmapviz: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624114.934681)!
[ INFO] [1514624117.031532117]: Odom: quality=75, std dev=0.115470m, update time=0.037920s
[ WARN] [1514624117.083361576]: rtabmap: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624114.934681)!
OpenCV Error: Bad argument (Unknown interpolation method) in resize, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/resize.cpp, line 3164
terminate called after throwing an instance of 'cv::Exception'
  what():  /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/resize.cpp:3164: error: (-5) Unknown interpolation method in function resize

[rtabmap/rtabmap-2] process has died [pid 13889, exit code -6, cmd /home/gal/catkin_ws/devel/lib/rtabmap_ros/rtabmap rgb/image:=/camera/rgb/image_rect_color depth/image:=/camera/depth_registered/sw_registered/image_rect_raw rgb/camera_info:=/camera/rgb/camera_info 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 __name:=rtabmap __log:=/home/gal/.ros/log/980a9c74-eb95-11e7-9af0-001e06302d57/rtabmap-rtabmap-2.log].
log file: /home/gal/.ros/log/980a9c74-eb95-11e7-9af0-001e06302d57/rtabmap-rtabmap-2*.log
[ INFO] [1514624117.766062934]: Odom: quality=76, std dev=0.114708m, update time=0.035344s
[ INFO] [1514624117.803406346]: Odom: quality=63, std dev=0.125988m, update time=0.035821s
[ INFO] [1514624118.077901692]: Odom: quality=65, std dev=0.124035m, update time=0.027739s
[ INFO] [1514624118.387913931]: Odom: quality=58, std dev=0.131306m, update time=0.031247s
[ WARN] [1514624118.773310295]: rtabmapviz: Could not get transform from odom to camera_link after 1.000000 seconds (for stamp=1514624116.509444)!


This is what the camera is publishing:

Frames:
Frame: camera_depth_frame published by unknown_publisher Average Delay: -0.0927805 Max Delay: 0
Frame: camera_depth_optical_frame published by unknown_publisher Average Delay: -0.0891017 Max Delay: 0
Frame: camera_rgb_frame published by unknown_publisher Average Delay: -0.0886092 Max Delay: 0
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.087947 Max Delay: 0

[matlabbe]

Hi,

To debug synchronization and network problems, can you try the R200 on the mac-pro computer?

Do /camera/rgb/image_rect_color and /camera/depth_registered/sw_registered/image_rect_raw have the same image size? Not sure why/when opencv "resize" is called.

Can you record about 10 seconds of topics and share the bag? Like start only the realsense launch and do:

$ roslaunch realsense_camera r200_nodelet_rgbd.launch
$ rosbag record /tf /camera/rgb/image_rect_color /camera/depth_registered/sw_registered/image_rect_raw /camera/rgb/camera_info

// if you record from a remote computer, do this instead:
$ rosbag record /tf /camera/rgb/image_rect_color/compressed /camera/depth_registered/sw_registered/image_rect_raw/compressedDepth /camera/rgb/camera_info

For the rtabmapviz tf error, you may need to change frame_id to root of your camera TF (as camera_link doesn't exist). Normally, the r200 launch file would publish camera_link though. You can do "$ rosrun tf view_frames" to see TF tree.

cheers,
Mathieu

[Dalalos]

Rosbag is attached.

I'v changed the frame_id to "camera_depth_frame" - one that the camera is published.

I can't understand either why rtabmap call opencv resize,  I open up the color and depth image at the same resolution - updated realsense launch file attached.

2017-12-31-20-41-07.bag
realsense_r200_rgbd.launch

[matlabbe]

rgb and depth images are missing in the bag:

$ rosbag info 2017-12-31-20-41-07.bag 
path:        2017-12-31-20-41-07.bag
version:     2.0
duration:    18.1s
start:       Dec 31 2017 13:41:07.75 (1514745667.75)
end:         Dec 31 2017 13:41:25.84 (1514745685.84)
size:        600.2 KB
messages:    1807
compression: none [1/1 chunks]
types:       sensor_msgs/CameraInfo [c9a58c1b0b154e0e6da7578cb991d214]
             tf2_msgs/TFMessage     [94810edda583a504dfda3829e70d7eec]
topics:      /camera/rgb/camera_info   1087 msgs    : sensor_msgs/CameraInfo
             /tf                        720 msgs    : tf2_msgs/TFMessage

Not sure you will be able to attach the file in the forum, if not, use a cloud service (dropbox/googledrive) and link the file here.

[Dalalos]

https://www.dropbox.com/s/6qn5oozjhs4tvao/2017-12-31-23-51-03.bag?dl=0

[matlabbe]

Hi,

it seems to work with queue_size set to 60...

$ roscore
$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth_registered/sw_registered/image_rect_raw queue_size:=60
$ rosbag play --clock 2017-12-31-23-51-03.bag

Well, this is on my indigo computer with rtabmap 0.15.0. I'll give another try on my kinetic computer and 0.11.8 binaries when it will have completed the current tests.

cheers,
Mathieu

PS: I guess from the timestamp of the bag that you are now in 2018, Happy New Year!

[matlabbe]

Tested on kinetic binaries and it works too. Note that current rtabmap binaries on kinetic are 0.11.13, not 0.11.8. Make sure to upgrade all kinetic packages. I know that opencv has been upgraded recently on kinetic, this may cause some issues if rtabmap binaries built with oldest opencv version are still there.

cheers,
Mathieu

[Dalalos]

Hi Mathieu, Happy New Year :)

After changing to rtabmap-0.11.13 and rtabmap-ros-0.11.13 on ROS-Kinetic it finally worked. Thanks a lot for your support.

By the way I noticed that I don't have to register the depth image in order for rtabmap will work, I think it's working the same with the raw images set to the same resolution.

In order to run on ROS-Indigo (I want to try runing on the Odroid itself), whice rtabmap and rtabmap-ros version can I use?

Another thing, when running rtabmap.launch file it takes around half a minute until rtabmap node comes up, why is that?

Thanks again, Gal.