Why is this pose-graph not optimized correctly when using optimizePoses?

I created a pose-graph from KITTI Odometry Evaluation Dataset 05 (first 1700 frames) using a Visual Odometry System with Loop Detection.

The graph looks very good to me and has multiple correct loop closures (red lines), so I want to optimize it making use of these loop closures to reduce the overall error in the graph, however, the optimization is not done as I expected it. The optimized graph ends up looking distorted and worse than the original one.

Here are images of the graph before and after optimizing it with the function optimizePoses(vSet):

Ground truth looks like this:

I'd be happy to share the imageviewset with you, but unfortunately the size is about 380 MB, so it exceeds the limit of what I can upload here.

Some words about the viewset: there is one connection between consecutive frames that originated from the visual odometry, and there are 45 additional connections for the loop closures. Every information matrix in all of these connections is an identity matrix - is that a problem? The relative poses are rigid3d-objects. I did not add matches to the connections, so the 'Matches'-column is empty.

Any help is highly appreciated! Thanks.

Answer 1

[Qu Cao]

The optimized pose graph looks correct to me as the nodes of red loop closure edges are merged. That been said, there are a couple of factors that can affect the performance:

• Accuracy of visual odometry. The pose graph before optimization looks very different from the ground truth. What visual odometry or visual SLAM method are you using? Have you considered adding more nodes into the graph so that the relative pose estimation is more accurate?
• Weight of pose graph edge in optimization. You can increase the value of minNumMatches to optimzie only strong edges and ignore weak connections including weak loop closures. Note that you need to fill in the Match column so that number of matches can be calculated.
• Type of relative transformation. You can perform similarity pose graph optimization if using a monocular camera. You just need to specify one of the relative poses (usually the loop closures) as a similarity transform.
• The information matrix can affect the performance but usually they are specified as identity matrices for all connections.

Answer 2

[Akshai Manchana]

Hi Robin,

As Qu suggested improving the loop closure detection and considering similarity graph optimization can improve the overall accuracy considerably.

However I understand that tuning the loop closure system is always difficult. We have a functionality called trimLoopClosures to identify false loop closures and remove them to improve the accuracy of the system.

d = vSet.createPoseGraph;
pg = poseGraph3D;
for k = 1:size(d.Edges,1)
pg.addRelativePose(se3(d.Edges.RelativePose{k}.A).xyzquat,[],d.Edges.EndNodes(k,1),d.Edges.EndNodes(k,2));
end
trimParams.MaxIterations = 100;
trimParams.TruncationThreshold = 25;
solverOptions = poseGraphSolverOptions;
[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);
removedLCs = trimInfo.LoopClosuresToRemove;

One you identify removed loop closures we can this data tune the loop detection parameters to reduce false positives and improve accuracy.