Graph optimization with G2O

Question

I'm trying to do graph optimization with G2O, mainly in order to perform loop closure. However finding minimal working examples online is an issue (I've found this project, as well as this one. The second one though has the form of a library, so one cannot really see how the author uses things.)

In contrast to online loop closure, where people update and optimize a graph every time they detect a loop, I'm doing graph optimization only once, after pairwise incremental registration. So in my case, pairwise registration and global, graph-based optimization are two separate stages, where the result of the first is the input for the second.

I already have a working solution, but the way that works for me is quite different from the usual use of g2o:

• As nodes I have identity matrices (i.e. I consider that my pointclouds are already transformed with the poses of the pairwise reg. step) and
• as edges, I use the relative transformation based on the keypoints of the pointclouds (also the keypoints are transformed). So in this case I penalize deviations of the relative pose from the identity matrix.
• As Information matrix (inverse of covariance) I simply use a 6x6 identity matrix multiplied by the number of found correspondences (like this case).
• The result of the graph is an update matrix, i.e. I have to multiply with this the camera poses.

Although this works in many/most cases, it is a quite unusual approach, while one cannot draw the graph for debugging (all nodes are identities in the beginning, and the result after optimization is a 3d path), which means that if something goes wrong getting an intuition about this is not always easy.

So I'm trying to follow the classic approach:

• The vertices/nodes are the poses of the pairwise registration
• The edges are the relative transformations based on the keypoints/features of the raw pointclouds (i.e. in the camera frame, not transformed by the poses of the pairwise registration)
• The output are the new poses, i.e. one simply replaces the old poses with the new ones
• Drawing the graph in this case makes sense. For example in case of scanning an object with a turntable, the camera poses form a circle in 3d space.
• I'm trying to form all the edges and then optimize only at one stage (this doesn't mean only 1 LM iteration though).

However I cannot make things running nicely with the 2nd approach. I've experimented a lot with the direction of the edges and the relative transformation that is used as measurement in the edges, everything looks as expected, but still no luck. For simplicity I still use the information matrix as mentioned above, it is a 6x6 identity matrix multiplied with the number of correspondences. In theory the information matrix is the inverse of covariance, but I don't really do this for simplicity (plus, following this way to compute the covariance is not very easy).

Are there any minimal working examples that I'm not aware of? Is there something fundamentally wrong in what I describe above? Are any rules of thumb (e.g. the first node in both approaches above is fixed) that I should follow and I might not be aware of them?

Update: More specific questions

• The nodes hold the poses of the robot/camera. It is unclear though at which reference frame they are defined. If it is the world coordinate frame, is it defined according to the camera or according to the object, i.e. first acquired pointcloud? This would affect the accumulation of the pose matrices during incremental registration (before the g2o stage - I try to form and optimize the graph only once at the end, for all the frames/pointclouds).
• The edge (Src->Tgt) constraints hold the relative transformation from pointcloudSrc to pointcloudTgt. Is it just the transformation based on the features of the two in the local coordinate frame of pointcloudSrc? Is there and tricky point regarding the direction, or just consistency with the relative transformation is enough?
• The first node is always fixed. Does the fixed node affect the direction of the edge that departs/ends_up from/at the fixed node?
• Is there any other tricky point that could hinter implementation?
• I'm working in millimeter instead of meter units, I'm not sure if this will affect the solvers of g2o in any way. (I wouldn't expect so, but a naive use of g2o that was giving some usable results was influenced)

Answer 1

The nodes hold the poses of the robot/camera. It is unclear though at which reference frame they are defined. If it is the world coordinate frame, is it defined according to the camera or according to the object, i.e. first acquired pointcloud? This would affect the accumulation of the pose matrices during incremental registration (before the g2o stage - I try to form and optimize the graph only once at the end, for all the frames/pointclouds).

- As far as I can tell, they should be in world coordinate frame indeed. So we start with identity, and incrementally carry on moving as we compute the odometry transformations (if visual odometry is used). So basically Pose_1 = Identity, Pose_2 = Poise_1*T_12, ..., Pose_i = Pose_(i-1)*T_(i-1)i. So that's one way.

The edge (Src->Tgt) constraints hold the relative transformation from pointcloudSrc to pointcloudTgt. Is it just the transformation based on the features of the two in the local coordinate frame of pointcloudSrc? Is there and tricky point regarding the direction, or just consistency with the relative transformation is enough?

Say you were in Pose_(i-1) (your previous known pose), and you want to compute the relative transformation between Pose_(i-1) and the current unknown pose Pose_i. Then you get the previous point cloud in coordinate frame of Pose_(i-1), I will call it PC_(i-1), and you also get your current point cloud PC_i and put also with respect to Pose_(i-1). More specifically, for all points p belonging to PC_(i-1), you transform them using Pose_(i-1), making p' = Pose_(i-1)*p, same for the points q, obtaining q' = Pose_(i-1)*q.

Given these two point clouds we can compute ICP between them src_cloud = PC_(i-1) tgt_cloud = PC_i T_(i-1)i = ICP( src_cloud, tgt_cloud ).inverse();

--> Note that instead of ICP you could have some other algorithm to align two point clouds, this is just an example.

(we get the inverse of the transform that aligns src_cloud to tgt_cloud because we want the relative motion of the robot/camera, and not the relative motion of the cloud, one is the inverse of the other, just like if you move towards a tree, the tree moves towards you with the same transformation, but inverse).

The first node is always fixed. Does the fixed node affect the direction of the edge that departs/ends_up from/at the fixed node?

The first node is fixed because of the formulation of the linear system (which the whole graph optimisation equation becomes at the end, you can see g2o paper and also look for "A tutorial on Graph-Based SLAM")

Is there any other tricky point that could hinter implementation? If you read RGBDSLAM code, and RTABMAP code, you will see that they set the information matrix as the identity multiplied by something. This something can be a proxy that reflects the inverse variance, you can compute the variance itself (average distance between the aligned points of the point clouds aligned with T_(i-1)i). You can set it as the identity multiplied by the number of inliers in your aligned procedure of choice, the whole point is that your information matrix must say more or less how much certain you are about the correctness of the transformation T_(i-1)i, or how much information it's bringing to the system, that's the main semantic meaning.

I'm working in millimeter instead of meter units, I'm not sure if this will affect the solvers of g2o in any way. (I wouldn't expect so, but a naive use of g2o that was giving some usable results was influenced)

I don't think it should create problems either, although you should beware of numerical issues such as overflow/underflow, rounding, and such things.