How to combine the error function(GICP) with GTSAM? (on KITTI dataset)

[Kenzo2511]

@koide3 Hi author and everyone, Currently, I am doing the experiment on KITTI dataset. My approach is that I construct a pose graph with relative pose constraints between the consecutive frames. Then I use GTSAM to optimize the pose graph. Then, I again optimize the sensor poses using pose graph optimization and global registration error minimization (based on GICP scan matching for all frame pairs). Is my approach right? If it's right, please tell me how to apply the error of GICP error to GTSAM. (Can anyone explain in detail?) Thank you very much

[koide3]

Yes, the approach you described is correct. Specifically, you need to implement a GICP factor class derived from gtsam::NonlinearFactor that linearizes the GICP error function and returns the linearized function as a gtsam::HessianFactor. The following document would be a help to see how to implement a custom factor for GTSAM.

Creating new factor and variable types: https://gtsam.org/doxygen/

[Kenzo2511]

class CustomGICPFactor : public NoiseModelFactorN<Pose3, Pose3> {
   private:
    typedef BetweenFactor<Pose3> This;
    typedef NoiseModelFactorN<Pose3, Pose3> Base;
    Pose3 measured_;
    PointCloudT::Ptr cloud_source_;
    PointCloudT::Ptr cloud_target_;

   public:
    using Base::evaluateError;

    typedef std::shared_ptr<CustomGICPFactor> shared_ptr;

    ~CustomGICPFactor() override {}
    CustomGICPFactor(Key key1, Key key2, const Pose3& measured, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
                     const SharedNoiseModel& model = nullptr)
        : Base(model, key1, key2), measured_(measured), cloud_source_(cloud_source), cloud_target_(cloud_target) {}
    Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2, OptionalMatrixType H1, OptionalMatrixType H2) const override {

        gtsam::Vector error(6);
        Pose3 hx = p1.between(p2, H1, H2);
        error = hx.localCoordinates(measured_);
        return error;
    }
    gtsam::NonlinearFactor::shared_ptr clone() const override { return std::make_shared<CustomGICPFactor>(*this); }
};

This is the CustomGICPFactor class that I will custom to the pose graph in GTSAM. It calculated the sum-of-squares error between a measurement z and a measurement prediction function h(x). Could you please check it?

If it's right, and I want to combine the GICP score to the evaluateError. How can I calculate the h(x)? and how to calculate H1 and H2 in A, while in GICP code, I only see 1 unique H matrix?

[koide3]

In my opinion, you need to compute the error between points but not between poses. Because this is off topic for this repository, please take a look at the following paper and GTSAM forum for further details.

http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf https://groups.google.com/g/gtsam-users?pli=1

[Kenzo2511]

Yes, the approach you described is correct. Specifically, you need to implement a GICP factor class derived from gtsam::NonlinearFactor that linearizes the GICP error function and returns the linearized function as a gtsam::HessianFactor. The following document would be a help to see how to implement a custom factor for GTSAM.

Creating new factor and variable types: https://gtsam.org/doxygen/

maybe you misunderstood me. Currently, I want to import GICP's custom error function into GTSAM. Could you describe the input and output in more detail?