ethz-asl / okvis

OKVIS: Open Keyframe-based Visual-Inertial SLAM.
Other
1.25k stars 538 forks source link

Why liftJacobian #91

Open zhaozhongch opened 1 year ago

zhaozhongch commented 1 year ago

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 J0 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.

zhaozhongch commented 1 year ago

I also noticed that in the ceres solver this page if you want to update the incremental $\Delta x$, you need to define something like MultiplyByJacobian to map the Jacobian from over-parameterized space to the minimal representation. In the OKVIS PoseLocalParameterization.cpp you define oplusJacobian to work as the MultiplyByJacobian because I see

Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor> > Jp(jacobian);

which shows that it goes from over-parameterized space 7 to 6. Maybe the OKVIS is using this to map the J_0 in the EvaluateWithMinimalJacobians from 7 DOF back to 6 DOF then we can get the $\Delta x$ in the minimal representation again? If so, this also feels very weird. Because we already have J_minimal in the EvaluateWithMinimalJacobians. If the above guess is correct, the OKVIS is doing

J_minimal -> J_over_param -> J_minimal

to finally get the $\Delta x$. This looks like wasting the computation resources.
Hope we can discuss those processes

NikolausDemmel commented 1 year ago

Without looking at the details, I think it's simply done to fit to Ceres' API, which expects Jacobians wrt the ambient space, i.e. 7DoF, and does the multiplication to get Jacobians wrt the local parameterization itself.

You cannot give minimal Jacobians directly to Ceres. I think this mostly for historic reasons and also has advantages (Residuals are independent of the local parameterization). But it also has disadvantages, as you point out here. Although the actual runtime overhead is probably not very noticable in this case.

cumtchenchang commented 6 months ago

@zhaozhongch Do you have change to right Jacobian and evaluate these actural results?