General questions about mapping and odometry

[adr_arroyo]

Hi, I am opening this other thread as these questions are more general and not only related to mapping with two cameras, sorry for the confusion in the other thread as I am already making too many questions .
I have other bunch of questions, which I tried to answer by myself but I guess I could not find proper literature or I did not understand the functioning of the sw. They are mostly concerning about the consequences of the odometry in the creation of the map. I mainly do not understand if for the slam, the odometry of the robot is used or also visual odometry is used to correct the robots one, is this how the slam is done? Questions related:
- So there is a parameter to select the odometry strategy (Odom, Strategy); how the odometry is registered I guess depends on it. But how does it happen when I have an odometry coming from the robot? How is it registered? I mean, is the information on the robot used to know the movement of the robot and then it uses the odom strategy (which I understood all of them are visual) to correct that robots odometry and thus refine the registered map (with ICP?, is the registration parameter related?) If there exists this map correction, is there any other parameter related to this? Is it also affected the links refining (are the links refined when the map is corrected?)?
- Regarding the odom strategy types, is there any literature about frame-to-map or frame-to-frame? I understand that the correction of the odometry is either related to the already registered map or to the previous frame, is it correct? The rest of the strategies seem related to visual odometry correction, is it activated even with the info coming from the odometry of the robot?
- In frame-to-map strategy, there is a parameter regarding the "local map of X words", are they visual words or odometry words? the parameters related to visual words have influence here?

Other types of questions are:
- when the graph optimization occurs, what is it changed? I am correct saying that the links (which means transformations) are refined with other transforms? anything else is changed? About links redefinition, what are the differences between visual and ICP, and how does it work when we are using both (=2)? Are there other parameters affected when we use one or the other?

- About ICP, if the update is set to 0.1, does it make sense to have a ICP/Traslation>0.1? Are the Rtabmap/update (angular update, linear update) parameters related or have influence here? If we set a higher correspondence ratio, will ICP work better (even though less times)?

Thanks and regards

[adr_arroyo]

Forgot to ask also about the filtering strategy
The odometry filtering strategy occurs before the graph optimization, when a node is added, am I right?
How does this affect the graph optimization strategy? Are there parameters that are influenced when using one strategy or another?

Also, what is the difference between using Force3DOF or not? Does it influence other parameters?

lastly, the approx_sync parameter, saves the incoming messages into a queue waiting for the rest of messages, when it receives all the necessary messages, it publishes the output. Is this correct or does it do something different?

Cheers

[matlabbe]

Depending on the SLAM approach, odometry can be loosely or tightly coupled with the SLAM approach. In RTAB-Map, odometry is loosely coupled, as it acts like a black box at the input of the SLAM system. There is no feedback from the mapping algorithm to odometry (only odometry -> mapping). The advantage is that mapping node is independant of the odometry, so you can use either wheel/robot odometry, laser scan odometry, visual odometry or a fusion of them. The one used by rtabmap is the topic "odom" connected at its input. The "odom" topic can be a single odometry approach, or a fusion of multiple odometry (e.g., robot_localization)

The "odom/" parameters affect only rgbd_odometry node, not rtabmap node. If you don't use rgbd_odometry node, these parameters are not used. If you want to use robot odometry, don't start visual odometry node, and just connect odometry message from your robot odometry node to rtabmap directly. For odometry correction, this is done by the loop closure approach, which is visual and/or scan based. Parameter groups are "Reg/", "Vis/", "Icp/". Is "RGBD/NeighborLinkRefining" is enabled, the odometry is refined between consecutive nodes added to graph using the same parameters of the same groups.

"Odom/" parameters don't affect odometry from the robot. You can see visual odometry node as an independent node publishing its own odometry, like robot odometry. Frame-to-Frame (or Frame-to-Keyframe) is a well known visual odometry approach. Frame-to-Map in RTAB-Map doesn't mean that the map of rtabmap is used for odometry, it means that odometry manages its own local map of features to match against and track the current frame. For frame-to-map odometry (and for frame-to-frame), the words are defined by "Vis/" parameters, while in rtabmap they are defined by "Kp/" parameters.

See this paper about graph optimization: Online Global Loop Closure Detection for Large-Scale Multi-Session
Graph-Based SLAM. All links are modified to minimize the global error in the graph. Visual registration is done using visual features, as ICP registration is done using point clouds or laser scans. When both are done, ICP is done after visual registration.

No, if odometry is good, ICP translation will be likely to be well under the displacement. It is more related to covariance of odometry, how much the odometry drifts. For example, if you move 1 meter between two nodes added to graph and odometry error is 1 cm, then ICP should correct only 1 cm. If it corrects 10 cm, the ICP transformation may resulted in a wrong transformation, or the odometry drifted a lot more than expected. If the odometry error is 1 meter, then ICP could correct up to 1 meter (if it is possible). The correspondence ratio can help to ignore transformations for which there not a lot of correspondences comparing to maximum size of laser scans. When a low ratio of correspondences is found, the transformation is likely to be bad.

For the second post. Odometry happens well before graph optimization. Odometry affects graph optimization by the covariance set in the odometry messages. Graph optimization uses the covariance to know how much a link can be corrected when minimizing the global error of the graph.

EDIT
3DoF force visual odometry/graph optimization to be done in 2D. If your robot is moving on a 2D plane (e.g., indoor), set it to true to avoid errors in "z", "roll" and "pitch". If your robot is moving on a 3D terrain or in the air, set it to false so that 6DoF odometry/localization is done.

For approx_sync, see this page: http://wiki.ros.org/message_filters/ApproximateTime

cheers,
Mathieu

[adr_arroyo]

Hi, thanks for the reply.
About the odometry, we thought that the robots odom was taken and then was somehow refined with those Odom/Strategies so we misunderstood that part.
We consider our odometry to be pretty good, as it comes from IMU, so the errors that appear are minimal and that is why we were surprised when the results were not so accurate in terms of odometry. For example, the first graph in "Graph view" of one test is like this:

As you can see, somehow the sw thought that we moved in two different corridors, but then graph optimization corrected the map:


It is very well in most of the parts but at the right end of the corridor there are two walls placed. Trajetory seems more accurate from how we moved but only on the left part as the movement was first closer to the bottom wall and then closer to the upper one.
With an odom with this accurazy, how is it possible that the initial graph was so different?
Corrections are done when a loop closure is added,as I read in the paper, so my guess (and from the loop closures that I saw in the db) is that at the right part of the wall there where not detected any proximity or global loop closures so the ICP cloud not match both pcl and graph optimization placed two.
Launch
DB


Regards

----------

Edit:
My guess was right, adding more loop closure possiblities, I mean, rotating every certain meters of translating and thus having a better knowledge of the environment, helps with having more loop closures and then the graph optimization is quite better:

There are much more matches to help with the refining


I am still not sure about the causes of this initial drift

[matlabbe]

Looking at the database, some neighbor (odometry) links have very high variance set. Graph optimization will work very badly in that case. If you open the database in DatabaseViewer, then open Constraints View, you can scroll each neighbor link. Some links with really bad covariance are (IDs): 208->209, 213->214, 505->506, 545->547, 596->597. I don't know why, unless the odometry node gives these values. Can you show what looks like your odometry messages (too see if this high covariance error is coming from odometry node or rtabmap node)?

$ rostopic echo /my_odom_topic

 
Clicking on "Refine" button fixed the problem in the database:
 ->
However, the problem seems to be the covariance set in the odometry messages. So fixing these covariances, I get those results:

Top View point cloud:


Pure odometry (is it really just IMU in your case? maybe you could use robot_localization package to do fusion with wheel encoders):



cheers,
Mathieu

[adr_arroyo]

Hi,
we are not sure of the causes of this odometry variance. We consider our odometry's covariance matrix correct (it might not be given this). The odometry of the robot is divided in: wheels encoders for traslation and IMU for rotation. It might be better to filter it with the robot_localization package as you said given this unsual drifts.
 The odom message looks like:
header:
  seq: 6065
  stamp:
    secs: 1499943999
    nsecs: 403572456
  frame_id: odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.000118427855769
      w: 0.999999992987
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]

About the error of the covariance in the links, it is weird that it appears but also that every time it appears its the same value. In other db ive noticed that the values are different (only ~2.0 of variance) but the same over all of this high covariances.

Cheers and thanks

[matlabbe]

I tried your covariance matrix (assuming they have always the same values), and I cannot reproduce the high covariance problem on my side. I tested with demo_mapping.bag by republishing the odometry messages with your covariance. Is the covariance fixed or changing over time? I refactored a little how covariance is copied from the odometry messages, though I don't think it would fix the problem.

Make sure the IMU is far from the motors (or magnetic parts) to avoid some strange rotation errors.

cheers,
Mathieu