search

koide3 / gtsam_points

A collection of GTSAM factors and optimizers for point cloud SLAM
MIT License
211 stars 26 forks source link

some question about code and paper difference in LsqBundleAdjustmentFactor #30

Open gyarnet opened 1 month ago

gyarnet commented 1 month ago

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();

Screenshot from 2024-10-29 15-55-19

koide3 commented 1 month ago

How did you correct it?

gyarnet commented 1 month ago

How did you correct it?

remove the R_k in the 171 line

koide3 commented 1 month ago

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.

gyarnet commented 1 month ago

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.