I am trying to optimize a pose graph I created in MATLAB using either the optimizePoses(viewSet)-function on the imageViewSet that I have or the optimizePoseGraph(poseGraph)-function after converting that imageViewSet to a poseGraph using createPoseGraph(vSet). Both functions do not optimize the pose graph correctly.
This is the uncorrected pose graph, with loop closures in red: uncorrected pose graph

and this is the pose graph after the optimization:

optimized pose graph

Clearly, the optimization is not correct. I used a dataset from Sensefly, and you can look at the "ground-truth" here: https://senseflycom.s3.amazonaws.com/datasets/small-village-merlishachen/postflight-report/merlischachen_report.pdf
As you see, the UAV flew some kind of a grid-trajectory.
I don't have a real clue where to start looking for the problem. There seems to be an issue with the distribution of the error over all poses. The graph was created using visual odometry, i. e. matching features and calculating and decomposing homographies between consecutive images.

Can anyone point me in the right direction?
If you need more information, feel free to ask.

  • $\begingroup$ Have you tried to optimize a simulated simple graph? e.g. 4 nodes and 1 edges and 1 loop closure constraint. $\endgroup$ Apr 29 at 1:23
  • $\begingroup$ your result looks like the smoothness constraint is missing. $\endgroup$ Apr 29 at 1:29
  • $\begingroup$ Thanks for the suggestion. I tried a simple graph with four nodes, four edges and 1 loop closure and that works perfectly fine. What do you mean by smoothness constraint, and how can I add that in MATLAB? I thought the smoothness of the graph should already come from the fact, that the overall error of the poses is minimized. $\endgroup$
    – DocRobson
    Apr 29 at 7:11
  • $\begingroup$ If you look at the optimized graph, loops are closed ad intended but their relative location to the neighbour nods are severely distorted. Relative transformation between close neighbor nodes shouldn't be too different after the optimization. This is sometimes called as smoothness constraint or it has many other names. Just see what are given as edges. $\endgroup$ Apr 29 at 8:09
  • $\begingroup$ If you have a trajectory with n1 n2 n3 n4 nodes, your edge constraint between (n1,n2) and (n2,n3) and (n3,n4) should be given to the optimizer. I think your optimization missed it but if there are edges see what happened to the residuals after the optimization. $\endgroup$ Apr 29 at 8:12

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.