Loop closure issues

[dorecasan]

Hi,

I know that the graph will be optimized when loop closure occurs, but I still don't understand why loop closure can reduce the uncertainty. Is there any formular that can help me? When optimizing, it need the initial values for each pose, so how you initialize those values in rtabmap? And the last one is that how to calculate the information matrix in each link of graph when I have odometry from encoder or from visual odometry.

Thank you so much

[sumitsarkar1]

Loop closure does not reduce "uncertainty" (not clear about what exactly you mean by uncertainty). It just reduces the accumulated error by putting extra constraints in the pose graph.

[dorecasan]

Thanks for your reply sumitsarkar1, The uncertainty i mentioned is the covariance of each pose of the graph. For example, when robot travel from x1,...,xn and have loop closure between x1 and xn. So x1 is put one more constraint. But for others x2,...,xn. Are those nodes affected by this extra constraint? Is this extra constraint affected greatly to our optimization results as we try to detect loop closure? If i misunderstood something, please explain for me. Thank you so much.

[sumitsarkar1]

Yes....other nodes are effected because its a joint optimisation problem over ALL posses (R,t) and the whole map I guess

[DuaneNielsen]

Understanding SLAM Using Pose Graph Optimization

[matlabbe]

For the original question:

1. You can think about the pose-graph like a network of springs attached each other by some nodes. Each spring has different stiffness (covariance or informative matrix), so when you add a new spring (loop closure constraint) between two nodes, the whole graph will balance to some equilibrium. See the videos on this page, this one or this one. For mathematics, I would refer you to g2o, toro or gtsam papers.
2. In rtabmap, initial values are taken from odometry poses. After a loop closure is detected, the graph is optimized, then those optimized poses are used as initial values for next graph optimizations when new loop closures are detected.
3. Good question, for wheel odometry, see something like this. However, rtabmap's use odometry's twist covariance for edges in the graph (which is assumed bounded). I saw that for some visual odometry packages, the covariance is fixed to an arbitrary value. In rtabmap visual odometry's covariance is derived from matching visual features error. For ARCore or ARKit, as we don't have access to this info, we can set arbitrary to a eye(6,6)*0.0001. In practice, we don't have always access to how the covariance is computed, so we can use those parameters for rtabmap node and approximate the drift:

~odom_frame_id (string, default: "")

The frame attached to odometry. If empty, rtabmap will subscribe to odom topic to get odometry. If set, odometry is got from tf (in this case, the covariance value is fixed by odom_tf_angular_variance and odom_tf_linear_variance). 

~odom_tf_linear_variance (double, default: 0.001)

When odom_frame_id is used, the first 3 values of the diagonal of the 6x6 covariance matrix are set to this value. 

~odom_tf_angular_variance (double, default: 0.001)

When odom_frame_id is used, the last 3 values of the diagonal of the 6x6 covariance matrix are set to this value. 

Thx @DuaneNielsen for the video link!