Stereo Odometry Error with DUO3D

[g.bartoli]

Hi Mathieu, I'm back :)

I updated to RTAB-Map 0.11.13 and now I'm using a DUO3D Camera with the official duo3d-driver ROS node together with RTAB-Map Stereo Odometry.

Since this camera has a really short baseline, I know it gives not so good results with depths greater than 2m, however, I'm trying indoor with close objects, and it is not so bad.

This is my test launch file:

<launch>

    <!-- ARG DEFAULT VALUES ARE OMITTED FOR CLARITY -->

    <!-- Telecamera stereo -->
    <node name="stereo_camera" pkg="duo3d_driver" type="duo3d_driver" output="screen">
        <remap from="duo3d/left/image_rect"       to="$(arg left_image)"/>
        <remap from="duo3d/right/image_rect"      to="$(arg right_image)"/>
        <remap from="duo3d/left/camera_info"      to="$(arg left_info)"/>
        <remap from="duo3d/right/camera_info"     to="$(arg right_info)"/>
        <remap from="duo3d/depth/image_raw"       to="$(arg disparity_img)"/>
        <remap from="duo3d/point_cloud/image_raw" to="$(arg point_cloud)"/>

        <rosparam param="image_size">[640,480]</rosparam>
        <param name="frame_rate"      value="30.0"/>
        <param name="auto_exposure"   value="true"/>
        <param name="vertical_flip"   value="false"/>
        <param name="exposure"        value="5"/>
        <param name="gain"            value="0"/>
    </node>

    <!-- Camera position -->
    <node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="$(arg camera_pose) $(arg base_frame) $(arg camera_frame) $(arg static_rate)"/>

    <!-- Stereo odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="odom" to="$(arg local_odom)"/>
        <remap from="left/image_rect"   to="$(arg left_image)"/>
        <remap from="left/camera_info"  to="$(arg left_info)"/>
        <remap from="right/image_rect"  to="$(arg right_image)"/>
        <remap from="right/camera_info" to="$(arg right_info)"/>

        <param name="frame_id"       type="string" value="$(arg base_frame)"/>
        <param name="odom_frame_id"  type="string" value="$(arg odom_frame)"/>
        <param name="Vis/MinInliers" type="string" value="10"/>
    </node>

    <!-- Visual SLAM -->
    <group ns="rtabmap">
        <node pkg="rtabmap_ros" type="rtabmap" name="visual_slam" output="screen" args="--delete_db_on_start">
            <remap from="odom" to="$(arg local_odom)"/>
            <remap from="left/image_rect"   to="$(arg left_image)"/>
            <remap from="left/camera_info"  to="$(arg left_info)"/>
            <remap from="right/image_rect"  to="$(arg right_image)"/>
            <remap from="right/camera_info" to="$(arg right_info)"/>

            <param name="subscribe_depth"  type="bool"   value="false"/>
            <param name="subscribe_stereo" type="bool"   value="true"/>
            <param name="frame_id"         type="string" value="$(arg base_frame)"/>
        </node>
    </group>

</launch>

When I launch it, for some seconds odometry goes fine (low quality, but it does not stop), then it always gives this error:

[ INFO] [1492589285.211382436]: Odom: quality=30, std dev=0.000174m|0.000245rad, update time=0.031231s
[ INFO] [1492589285.244175387]: Odom: quality=24, std dev=0.000422m|0.000537rad, update time=0.032190s
[ INFO] [1492589285.306793582]: Odom: quality=36, std dev=0.000226m|0.000254rad, update time=0.031595s
OpenCV Error: Assertion failed ((npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0) in calcOpticalFlowPyrLKStereo, file /tmp/binarydeb/ros-kinetic-rtabmap-0.11.13/corelib/src/util2d.cpp, line 372
terminate called after throwing an instance of 'cv::Exception'
  what():  /tmp/binarydeb/ros-kinetic-rtabmap-0.11.13/corelib/src/util2d.cpp:372: error: (-215) (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 in function calcOpticalFlowPyrLKStereo
[ INFO] [1492589285.359705489]: Odom: quality=34, std dev=0.000126m|0.010000rad, update time=0.052121s
[ INFO] [1492589285.426188707]: Odom: quality=37, std dev=0.000432m|0.000355rad, update time=0.045506s
[ INFO] [1492589285.464574679]: Odom: quality=22, std dev=0.001073m|0.001912rad, update time=0.037781s
[rtabmap/visual_slam-5] process has died [pid 30168, exit code -6, cmd /opt/ros/kinetic/lib/rtabmap_ros/rtabmap --delete_db_on_start odom:=/odometry/local left/image_rect:=/duo3d/left/image_rect left/camera_info:=/duo3d/left/camera_info right/image_rect:=/duo3d/right/image_rect right/camera_info:=/duo3d/right/camera_info __name:=visual_slam __log:=/home/agrirobot/.ros/log/42aedffe-24d7-11e7-b84a-f816548c315b/rtabmap-visual_slam-5.log].
log file: /home/agrirobot/.ros/log/42aedffe-24d7-11e7-b84a-f816548c315b/rtabmap-visual_slam-5*.log

Is this a bug, or am I setting some parameters wrong?

Thanks,
Best regards

[matlabbe]

Hi,

Welcome back! This is indeed a bug when no keypoints are extracted, and it's now fixed by this commit. For binaries 0.11.3, set parameter "Stereo/OpticalFlow" to false as a workaround, or reduce response/hessian threshold of the selected feature type to extract at least one feature per image.

cheers,
Mathieu

[g.bartoli]

Ok Mathieu, thanks for the informations! :)