some question about code and paper difference in LsqBundleAdjustmentFactor

[hsiaochuan]

the jacobi in line 171 is wrong, but if i correct it, the optimization is worse.

    gtsam::Matrix36 H2_;
    H2_.block<3, 3>(0, 0) = R_k * -gtsam::SO3::Hat(mean_k);
    H2_.block<3, 3>(0, 3) = R_k * gtsam::Matrix3::Identity();

[koide3]

How did you correct it?

[hsiaochuan]

How did you correct it?

remove the R_k in the 171 line

[koide3]

I think the implementation is correct. You can confirm the SE3 derivatives by seeing the GTSAM code: https://github.com/borglab/gtsam/blob/d48b1fc8402e28d5b6af8f59cce8dbe87e66cf0c/gtsam/geometry/Pose3.cpp#L353.

[hsiaochuan]

thanks for your answer, I mistakenly take the gtsam::Pose3 as SO(3) + R3 instead of SE(3). I learn a lot from your code.