Map Distorting when closing loop

[kiprock]

Hello,

When closing the loop in simulation, the rtab generated map becomes very distorted. I am unsure if this is an intrinsics issue, or odometry, or some other tuning problem?

Here is a video demonstrating the issue:

Thank you Kip

[matlabbe]

If you can share the database, that would be great. Maybe the covariance in the odometry topic is too large. How is the odometry computed?

[kiprock]

Hi @matlabbe Check out this latest video: Here is the associated database

I have tried a few ways of computing the odometry, presently I am sending the rtab calculated rgbd_odometry to PX4 via the /mavros/odometry/out topic. The PX4 fuses it's IMU with this external odometry source and publishes out to the /mavros/local_position/odom topic. I re-broadcast that topic to /odom, which rtab_map subscribes to. In my opinion, this gives a smoother and more accurate pose.

Here is what my rtab_map launch looks like:

<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
            <arg name="rtabmap_args"       value="--delete_db_on_start"/>
            <!-- <arg name="rtabmap_args"       value="-delete_db_on_start -Optimizer/GravitySigma 0.3" make it double dashes in args-->
            <arg name="frame_id"           value="base_link"/>
            <arg name="visual_odometry"    value="true"/>
            <arg name="odom_topic"         value="/mavros/odometry/out"/> <!--this is output topic of rtabmap vo-->
            <arg name="rgb_topic"          value="/camera/camera/color/image_raw"/>
            <arg name="depth_topic"        value="/camera/camera/depth/image_raw"/>
            <arg name="camera_info_topic"  value="/camera/camera/color/camera_info"/>
            <arg name="approx_sync"        value="true"/>
            <arg name="queue_size"         value="50"/>
            <arg name="rviz"               value="false"/>
            <arg name="rtabmapviz"         value="true"/>
            <arg name="publish_tf_odom"    value="false"/>
              <arg name="odom_frame_id"    value="odom"/>
            <arg name="odom_sensor_sync" value="true"/>
            <!--arg name="vo_odom_topic" value="/mavros/odometry/out"/-->

             <!-- use actionlib to send goals to move_base --> 
            <param name="use_action_for_goal" type="bool" value="true"/>
            <remap from="move_base"            to="/move_base"/>

    </include>

As you can see from the video, everything is fine until rtab_map closes a loop, I thought this might be a NED/ENU issue, but then, wouldn't the entire map jump around, not just the section that the camera can see?

Here is my tf_tree. You can ignore the map->map_ned tree

[matlabbe]

The linked database doesn't seem to match what we see in the video. In the database the drone never takes off. I think however the issue is coming from <arg name="odom_frame_id" value="odom"/>, you may try to remove this to force rtabmap to use covariance set in the odometry topic. Because when setting odom_frame_id, the covariance should be manually set with odom_tf_linear_variance and odom_tf_angular_variance arguments. Depending on the version of rtabmap you have, previously a covariance of 1 was set by default with rtabmap.launch if odom_frame_id is set. I fixed this with a lower covariance by default (now 0.001 for angular and linear variances) not so long ago in this commit https://github.com/introlab/rtabmap_ros/commit/06c29333bd6f683e0a497c499a171e8f405ab025.

Regards, Mathieu

[kiprock]

Thanks Mathieu,

I incorporated your changes, (Commented out , and set linear/angular velocity to 0.001) the mapping process seems much better, but after some time, I am still getting the distortion. This is the video and here is the database (sorry of the other was not correct).

[matlabbe]

I feel that odometry covariance on some links is still high. Here is an example:

[0.01298195155207068, 0, 0, 0, 0, 0;
 0, 0.01298195155207068, 0, 0, 0, 0;
 0, 0, 0.01298195155207068, 0, 0, 0;
 0, 0, 0, 0.005927445176616312, 0, 0;
 0, 0, 0, 0, 0.005927445176616312, 0;
 0, 0, 0, 0, 0, 0.005927445176616312]

which means a linear error of 11 cm for two consecutive frames, which is too high and loop closure rejection based on maximum graph deformation cannot work properly. With RGBD/OptimizeMaxError=3, it means that link can deform up do 33 cm.

If you commented "odom_frame_id", it means the covariance was taken from the odometry topic, not odom_tf_linear_variance or odom_tf_angular_variance arguments. Is the odometry the output of rgbd_odometry? I may try to test why it outputs such high covariance. If you can record a rosbag like this:

rosbag record /tf /tf_static /camera/camera/color/image_raw /camera/camera/depth/image_raw /camera/camera/color/camera_info

It will help me to debug rgbd_odometry.

Another way is to force the covariance at some fixed values, then those wrong loop closures will be rejected. Try with:

<arg name="odom_frame_id"          value="odom"/>
<arg name="odom_tf_linear_variance"    value="0.0001"/>
<arg name="odom_tf_angular_variance"   value="0.001"/>

Here a maximum of 3 cm deformation will be allowed on an odometry link.

For information about why a loop closure has been detected first, it is because of the repetitive brick texture on the wall, which can produce a lot of perfect matches: :