Skip to content

Why liftJacobian #91

Open
Open
@zhaozhongch

Description

Maybe there won't be any people answering this question since OKVIS is old but I am confused about the operation in the OKVIS jacobian recently.
In all of the XXError.cpp, the OKVIS inheritance the Ceres Solver's Evaluate function, which is used to provide the error and jacobian definition. I use ReprojectionError.hpp for example. In the code EvaluateWithMinimalJacobians function, it has

    Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J0_minimal;
...
      // pseudo inverse of the local parametrization Jacobian:
      Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
      PoseLocalParameterization::liftJacobian(parameters[0], J_lift.data());

      // hallucinate Jacobian w.r.t. state
      Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J0(
          jacobians[0]);
      J0 = J0_minimal * J_lift;
...

Let me assume the reprojection error is $\mathbf{e} \in \mathbb{R}^2$ , which is the pixel reprojection error defined in the paper equation 6. Clearly, from J0_minimal definition, it is the jacobian of the error w.r.t the minimal representation, i.e., the jacobian to the Lie Algebra $so(3)$. The J0_minimal has 6 DOF which are 3 (translation) + 3 (Lie algebra) shows that. But finally, OKVIS uses J_lift to move the jacobian from minimal representation to the over-parameterized space, which is 3 (translation) + 4 (quaternion).
I don't understand why OKVIS needs to move the jacobian from the minimal representation to the overparameterized representation. Because by using the Jacobian in minimal representation, we can update the incremental $\Delta x = (J^TJ)^{-1}J^Tf$ in a minimal representation. Then we can use something like $x_{k+1} = x_{k}\text{Exp}(\Delta x)$ to update the state such as camera pose, as stated in the ceres local parameterization part.
By using J_lift, then J_0 is back to overparameterized space, then the incremental $\Delta x = (J^TJ)^{-1}J^Tf$ has 7 DOF. Then in OKVIS directly add $x_{k+1} = x_{k} + \Delta x$ to update the state since they have the same DOF now? This doesn't sound reasonable.

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions