I have a trajectory of a quadcopter in 3D. Out of all the poses in the trajectory, I want to correct one of the poses. I then want to adapt the rest of the poses based on the newly corrected pose. I understand that this is a common problem in robotics and there would be a number of solutions. I want a tried and tested c++ library that does this as I want to integrate this feature in my system but not spend time on building it myself. What are the possible c++ libraries I can use?

  • $\begingroup$ How do you get the trajectory? Is your trajectory generator taking into account the physics of the system ? Is you new pose feasible ? $\endgroup$
    – N. Staub
    Jun 13 '18 at 6:43
  • $\begingroup$ Trajectory is obtained using a stereo camera by tracking features. So I have the location and orientation of the camera for every point in the trajectory. The corrected trajectory point comes from another source. It is also just the pose information. All I have is a chain of poses. $\endgroup$
    – Vinmean
    Jun 13 '18 at 6:47
  • $\begingroup$ So you manually fly and record the trajectory and then 'correct' on point ? $\endgroup$
    – N. Staub
    Jun 13 '18 at 7:32
  • $\begingroup$ Yes, I fly manually and I get the trajectory information real time. I just want to correct the pose for one of those trajectory points. $\endgroup$
    – Vinmean
    Jun 13 '18 at 7:34
  • $\begingroup$ But you want to refly the corrected trajectory ? $\endgroup$
    – N. Staub
    Jun 13 '18 at 8:21

You can use ceres-solver library which is nonlinear optimization framework implemented for problems like pose optimization and/or bundle adjustment. It supports ICP , bundle adjustment , pose graph optimization and other techniques which is relative to your problem. I think you should glance at it first.

  • $\begingroup$ I found that ceres-solver has an example implementation called pose_graph_3d that does a optimization of the 3D trajectory. I got it working with sample .g2o datasets. It uses the g2o format for representing trajectory. The format captures poses of each node (VERTEX_SE3) and relative orientation between the nodes (EDGE_SE3). But how is the relative orientation usually found? All I have is a chain of poses (only vertices) How do you think I can estimate the relative poses of each node in my problem? I also have a node for which I know the corrected pose. How does that affect other nodes? $\endgroup$
    – Vinmean
    Jun 15 '18 at 8:55
  • $\begingroup$ As far as i understand you are building keyframe based map. I think one solution could be distributing the drift /error among the keyframes. $\endgroup$
    – MIRMIX
    Jun 15 '18 at 9:21

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.