Pose graph 2d problem enforcing straight trajectory
Josep Bosch
I am working on a 2D pose graph optimization problem where each pose has , and yaw parameters. The problem includes global position constraints, incremental position constraints, and incremental rotation constraints, among others.
In some cases, I have additional information that the trajectory should be perfectly straight. My initial idea was to introduce two extra parameters: a line angle and a line offset, then reduce one degree of freedom by expressing as a function of , the line angle, and the line offset. Would this approach be feasible? I have explored local parameterization, but I don’t think it applies in this case.
Alternatively, I considered using a cost function to minimize the distance between the points and the estimated line, but this does not strictly enforce that all points lie on the line.
Another approach would be to define poses in terms of line coordinates (e.g., line origin, line vector, and distance to the origin). However, this would require rewriting my existing cost functions, which currently operate on and , to work with the new representation. This would make residuals and Jacobians more complex to compute and the code harder to maintain.
Additionally, I am constrained to using Ceres Solver v1.1.4.
What would be the best approach?
Thank you for your help!
Josep Bosch