util3d_features.cpp generateKeypoints3dDepth() uassert fails

[scott-eddy]

I am trying to run rtabmap.launch with an intel realsense R200. I've changed the rgb_topic, depth-topic, and camera_info_topic to the proper names but am running into a fatal error:
[FATAL] (2016-03-25 15:26:20.417) util3d_features.cpp:89::generateKeypoints3DDepth() Condition (cameraIndex >= 0 && cameraIndex < (int)cameraModels.size()) not met! [cameraIndex=1, models=1, kpt.x=546.000000, subImageWidth=480.000000 (Camera model image width=0)]

Obviously it is failing because cameraIndex = cameraModels.size() == 1, but I don't understand why that is a problem. I was wondering i anyone can provide me with some clarity as to what this assertion is demanding. Why would one camera model for one color camera be a problem?

Thanks!

[matlabbe]

Are your RGB and depth images are the same size? It seems not. Hopefully, the rtabmap version (0.11) you are using supports RGB and depth images with not the same size (so you would not have to resize the RGB or the depth image). The error above tells that image size of the camera model is null (Camera model image width=0). Make sure the camera_info msg contains the image size set. The conversion between ROS camera model to RTAB-Map camera model is done here. Looking at sensor_msgs/CameraInfo message, the fields width and height should be set.

$ rostopic echo /rgb/camera_info

If they are null, you would have to update the R200 package publishing the camera_info to include them or create a node republishing the same camera_info but with the resolution set.

cheers

[scott-eddy]

@matlabbe thanks for the prompt reply. My depth and RGB images are not the same size, but I cam using rtab map version 0.11.2. Unfortunately, the image size of the model is reported correctly in camera info. The output of echoing the topic is:

seq: 383
  stamp:
    secs: 1459076138
    nsecs: 252774570
  frame_id: camera_rgb_optical_frame
height: 480
width: 640
distortion_model: plumb_bob
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [616.394287109375, 0.0, 323.9136657714844, 0.0, 622.3211669921875, 242.3827362060547, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [616.394287109375, 0.0, 323.9136657714844, 0.0, 0.0, 622.3211669921875, 242.3827362060547, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

Any reason why the ROI width/height would be confused with the actual width/height?

[scott-eddy]

Tracking this down further, updating ROI values doesn't have any effect. Additionally, every so often rtabmap_ros will execute with no error but results in no apparent functionality. When executing with no error the visualization comes up, but nothing is happening:

Notice the high reported fps (varies between 1k - 3k). This behavior is intermittent and generally the node still fails to launch. Inserting ROS_ERROR messages in the RGBDOdometryNode to extract model width and height parameters the abridged output of launching my rtabmap.launch is:

/rtabmap/rgbd_odometry subscribed to:
   /camera/color/image_raw,
   /camera/depth/image_raw,
   /camera/color/camera_info
[ WARN] (2016-03-27 08:47:57.077) PreferencesDialog.cpp:2875::setParameter() Trying to set "Kp/NNStrategy" to KdTree but RTAB-Map isn't built with the nonfree module from OpenCV and kdTree cannot be used with binary descriptors. Keeping default combo value: FLANN Linear.
[ WARN] (2016-03-27 08:47:57.084) PreferencesDialog.cpp:2875::setParameter() Trying to set "Vis/CorNNType" to KdTree but RTAB-Map isn't built with the nonfree module from OpenCV and kdTree cannot be used with binary descriptors. Keeping default combo value: Brute Force.
[ INFO] [1459082877.085466158]: Reading parameters from the ROS server...
[ERROR] [1459082877.152795298]: ODOM NODE: MODEL WIDTH 640
[ERROR] [1459082877.152844044]: ODOM NODE: MODEL HEIGHT 480
[ WARN] (2016-03-27 08:47:57.204) Features2d.cpp:439::create() BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.
[FATAL] (2016-03-27 08:47:57.224) util3d_features.cpp:89::generateKeypoints3DDepth() Condition (cameraIndex >= 0 && cameraIndex < (int)cameraModels.size()) not met! [cameraIndex=1, models=1, kpt.x=541.000000, subImageWidth=480.000000 (Camera model image width=0)]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2016-03-27 08:47:57.224) util3d_features.cpp:89::generateKeypoints3DDepth() Condition (cameraIndex >= 0 && cameraIndex < (int)cameraModels.size()) not met! [cameraIndex=1, models=1, kpt.x=541.000000, subImageWidth=480.000000 (Camera model image width=0)]

The full output of the launchfile can be seen here, and the altered launchfile itself can be seen here

[matlabbe]

Thx, I missed that the error was coming from the odometry node. There was a camera_info conversion bug for odometry. The commit above should now copy the image width/height correctly.

[scott-eddy]

@matlabbe I came to the same conclusion and the above commit does fix this issue.

Rtabmap with a real sense is still not working out of the box but I'll post more questions only after pouring some debug time into my setup.

Thanks

[matlabbe]

Note that rtabmap requires a registered depth image to color image. If they don't have the same size, make sure that pixel correspondences are still one to one (up to a scale factor) between RGB and depth.