Graph optimization with G2O

edit

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.

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 ... (more)