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)