chengwei0427/ct-lio

Question about the analytic derivatives of ct-icp

bzdfzfer opened this issue · 5 comments

bzdfzfer commented

Hi, buddy, excellent work! Would you share the process of the derivation of the derivatives of the slerped quaternions with reference to the begin and end rotation?

chengwei0427 commented

Hi， @bzdfzfer
Thanks for interesting in our work!
Maybe you can refer to this issue

bzdfzfer commented

Thanks for your quick reply, I will try that.

bzdfzfer commented

Hi, one more question about your implementation of the derivatives with reference to the begin and end rotation,

                Eigen::Matrix<double, 3, 3> jacobian_slerp_begin = (rot_delta_slerp.toRotationMatrix()).transpose() * (Eigen::Matrix3d::Identity() - alpha_time * zjloc::numType::Qleft(rot_delta_slerp).bottomRightCorner<3, 3>() * (zjloc::numType::Qleft(rot_delta).bottomRightCorner<3, 3>()).inverse());

                Eigen::Matrix<double, 3, 3> jacobian_slerp_end = alpha_time * zjloc::numType::Qright(rot_delta_slerp).bottomRightCorner<3, 3>() * (zjloc::numType::Qright(rot_delta).bottomRightCorner<3, 3>()).inverse();

https://github.com/chengwei0427/ct-lio/blob/6fdc51dcd4d2fb5cb1f8a8a2b30c03938790005b/src/liw/lidarFactor.cpp#L115

where you use the multiply of the "Qleft" or "Qright" function, instead of SO3Jr, could you explain the reason? Why those functions work as the right jacobians of SO(3)? (If so, is the former with "Qleft or Qright muliplication" seems faster than the latter implementation with SO3Jr?)

chengwei0427 commented

Hi, @bzdfzfer
The right perturbation is used here.
I suggest you take a look at Intermittent GPS-aided VIO_ Online Initialization and Calibration.
Quaternion derivation part, I suggest you refer to Quaternion kinematics for the error-state Kalman filter and Vins-mono.

bzdfzfer commented

Thanks, those papers help a lot!