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
Both functions do not optimize the pose graph correctly.
This is the uncorrected pose graph, with loop closures in red:
and this is the pose graph after the optimization:
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.