Bundle adjustment demo using Ceres Solver
Also read a blog post.
- Ceres Solver
This project implements a simple bundle adjustment problem using Cere Solver to solve it.
The constraints are defined with customized cost functions and self-defined Jacobians.
Two versions of on-manifold local parameterization of SE3 poses (both of which using 6-dof Lie algebra for updating) are used, including
- quaternion + translation vector (7d), and
- rotation vector + translation vector (6d)
parametersse3.cpp for the local parameterization implementation.
For convenience in expression, we denote the Jacobian matrix of the error function w.r.t. the Lie group by
J_err_grp, the Jacobian matrix of the Lie group w.r.t. the local Lie algebra increment by
J_grp_alg, and the Jacobian matrix of the error function w.r.t. the local Lie algebra increment by
In many on-manifold graph optimization problems, only
J_err_alg are required. However, in Ceres Solver, one can only seperately assign
J_grp_alg, but cannot directly define
J_err_alg. This may be redundant and cost extra computational resources:
- One has to derive the explicit Jacobians equations of both
- The solver may spend extra time in computing
J_grp_alg, and multiplying
Therefore, for convenience, we use a not-that-elegant but effective trick. Let's say that the error function term is of dimension
m, the Lie group
N, and the Lie algebra
n. We define the leading
m*n block in
J_err_grp to be the actual
J_err_alg, with other elements to be zero; and define the leading
n*n block in
J_grp_alg to be identity matrix, with other elements to be zero. Thus, we are free of deriving the two extra Jacobians, and the computational burden of the solver is reduced -- although Ceres still has to multiply the two Jacobians, the overall computational process gets simpler.
One advice for self-defined Jacobians: do not access jacobians and jacobians in the meantime, which may cause seg-fault.