Map Optimized before Loop Closure detected

[KamalDSOberoi]

Hello,

I am working with rtabmap_ros package. As I understand, it optimizes the graph and 3D point cloud when loop closure has been detected. But in my case, it optimizes even if loop closure has not been detected.

1. In the paper about loop closure detection, I read that there are two types of links: neighbors and loop closure. Where in the code are these links added to the graph? Are we differentiating between these links?

2. Where in the code is loop closure being detected? I want to go through the steps.

3. What could be the reason for the graph to be optimized even before loop closure has been detected? Is MapOptimizerNode responsible for this optimization?

4. When the graph is optimized, the position of the odom frame will change, am I right? I think that in optimization the error is huge due to which the position of the odom varies drastically.

These might be very basic doubts but I think they are important to get a complete understanding of the package.
Any help is appreciated. Thanks in advance!!

     

[matlabbe]

Hi,

1. For graph optimization, there is no difference between neighbor or loop closure links. Neighbor links are added when a new node is created and added to graph (Memory::addSignatureToStm()), so to link with the previous node. Loop closure links are added in Rtabmap::process().

2. Loop closures are detected in the Bayes filter update.

3. The logic of map optimization is here. If the graph has changed (new loop closure links, new proximity links, nodes transferred to or retrieved from LTM), the map is re-optimized. This step is done at the end of the loop. Other graph optimizations may happen locally for proximity detections when laser scans are used (see here). map_optimizer node is not used by default (only for advanced usage), see  its description on ROS wiki.

4. Yes, if parameter "RGBD/OptimizeFromGraphEnd" is false (default is false), the robot has drifted and a loop closure is detected.

cheers

[KamalDSOberoi]

Hello,

Thanks for your guidance. The problem is in the last point.

Even if there is no loop closure, the code is detecting it and changes the pose of "odom" frame. Also it is not able to properly assemble the point cloud and when "odom" moves, a point cloud is assembled but it is not "integrated" as expected with the previously assembled clouds.

What could be the problem according to you? Where should I check for the solution to this problem in the code?  

Thanks!!  

[matlabbe]

Hi,

I'm not aware about your configuration, can you reproduce the problem with the basic Hand-held tutorial too?

Which topic are you looking for the assembled cloud? Is this /rtabmap/cloud_map? If you are using MapCloud plugin in RVIZ, make sure to set the fixed frame to /map (see general RVIZ's parameter) .

cheers

[KamalDSOberoi]

Hi,

Sorry for the late reply. I was trying to solve some other problem.

I checked the Hand-held tutorial with MapCloud plugin in RVIZ subscribed to /rtabmap/cloud_map. I got an error that
Client [/rviz] wants topic /rtabmap/cloud_map to have datatype/md5sum
[rtabmap_ros/MapData/150da8e574f437839a50106f27c0e364], but our version has [sensor_msgs/PointCloud2/1158d486dd51d683ce2f1be655c3c181]. Dropping connection.

If I use /rtabmap/mapData, then I get the assembled cloud in RVIZ. It works fine but when a loop closure is detected, the position of "odom" changes randomly. Also if after moving the robot for sometime, I detect a loop closure and dont move the robot again, it keeps on detecting the loop closures at the same position. Shouldn't it just detect the loop closure only once?

Also what if I have two sources for computing the robot pose and I want to visualize both in RVIZ and not combine them, say using EKF. So there will be two transformations for odom->base, one from kinect and one from other source. How can I do that? Any idea?


Thanks a lot for your help.

Best Reagards    
 

[matlabbe]

Hi,

Not sure why you are getting rviz datatype error for MapCloud topic as by default in the tutorial, the MapCloud is linked to /rtabmap/mapData (when launching rviz with the tutorial's launch file):

$ roslaunch rtabmap_ros rgbd_mapping.launch rviz:=true rtabmapviz:=false

On loop closures, /map -> /odom will change accordingly to map corrections (which may appear random). A loop closure can be detected for each image acquired. However, if you don't move while a loop closure is detected, it may not be added to graph if the latest node is not added. Even if we are not moving, the environment may change. For example, the first time we traversed an area it was sunny, then the second time it is cloudy: visual features may be very different between cloudy and sunny images, if we don't move and the cloud is passing by so that it is sunny again, a loop closure could be then detected.

TF doesn't like child transforms with two parents. You may have /odom -> /kinect and /odom -> /kinect_fake. I did something similar to compare the ground truth of TUM datasets with rtabmap, see rgbdslam_datasets.launch.

<node pkg="tf" type="static_transform_publisher" name="world_to_map" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />

<!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry">
      <remap from="odom" to="vis_odom"/>

      <param name="odom_frame_id" type="string" value="vis_odom"/>
      <param name="frame_id" type="string" value="kinect"/>
      <param name="publish_tf" type="bool" value="false"/>
      <param name="ground_truth_frame_id" type="string" value="world"/>
    </node>
    <!-- rename the child frame of odometry -->
    <node name="odom_msg_to_tf" pkg="rtabmap_ros" type="odom_msg_to_tf">
      <param name="frame_id" type="string" value="kinect_est"/>
    </node>

So odometry publishes "/vis_odom -> /kinect_est" instead of "/odom -> /kinect". Note that "/world -> /kinect" is the ground truth. The resulting TF tree was something like this:

/world -----> /map ----> /vis_odom ----> /kinect_est
        \
         ---> /kinect

cheers