integrated_icp_factor.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <memory>
#include <gtsam_points/util/gtsam_migration.hpp>
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/factors/integrated_matching_cost_factor.hpp>
 
namespace gtsam_points {
 
struct NearestNeighborSearch;
 
template <typename TargetFrame = gtsam_points::PointCloud, typename SourceFrame = gtsam_points::PointCloud>
class IntegratedICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using shared_ptr = gtsam_points::shared_ptr<IntegratedICPFactor_<PointCloud>>;
 
  IntegratedICPFactor_(
    gtsam::Key target_key,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source,
    const std::shared_ptr<const NearestNeighborSearch>& target_tree,
    bool use_point_to_plane = false);
 
  IntegratedICPFactor_(
    gtsam::Key target_key,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source,
    bool use_point_to_plane = false);
 
  IntegratedICPFactor_(
    const gtsam::Pose3& fixed_target_pose,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source,
    const std::shared_ptr<const NearestNeighborSearch>& target_tree,
    bool use_point_to_plane = false);
 
  IntegratedICPFactor_(
    const gtsam::Pose3& fixed_target_pose,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source,
    bool use_point_to_plane = false);
 
  virtual ~IntegratedICPFactor_() override;
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  virtual size_t memory_usage() const override;
 
  void set_num_threads(int n) { num_threads = n; }
 
  void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; }
 
  void set_point_to_plane_distance(bool use) { use_point_to_plane = use; }
 
  void set_correspondence_update_tolerance(double angle, double trans) {
    correspondence_update_tolerance_rot = angle;
    correspondence_update_tolerance_trans = trans;
  }
 
private:
  virtual void update_correspondences(const Eigen::Isometry3d& delta) const override;
 
  virtual double evaluate(
    const Eigen::Isometry3d& delta,
    Eigen::Matrix<double, 6, 6>* H_target = nullptr,
    Eigen::Matrix<double, 6, 6>* H_source = nullptr,
    Eigen::Matrix<double, 6, 6>* H_target_source = nullptr,
    Eigen::Matrix<double, 6, 1>* b_target = nullptr,
    Eigen::Matrix<double, 6, 1>* b_source = nullptr) const override;
 
private:
  int num_threads;
  double max_correspondence_distance_sq;
  bool use_point_to_plane;
 
  std::shared_ptr<const NearestNeighborSearch> target_tree;
 
  // I'm unhappy to have mutable members...
  double correspondence_update_tolerance_rot;
  double correspondence_update_tolerance_trans;
  mutable Eigen::Isometry3d last_correspondence_point;
  mutable std::vector<long> correspondences;
 
  std::shared_ptr<const TargetFrame> target;
  std::shared_ptr<const SourceFrame> source;
};
 
template <typename TargetFrame = gtsam_points::PointCloud, typename SourceFrame = gtsam_points::PointCloud>
class IntegratedPointToPlaneICPFactor_ : public gtsam_points::IntegratedICPFactor_<TargetFrame, SourceFrame> {
public:
  using shared_ptr = gtsam_points::shared_ptr<IntegratedPointToPlaneICPFactor_<TargetFrame, SourceFrame>>;
 
  IntegratedPointToPlaneICPFactor_(
    gtsam::Key target_key,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source,
    const std::shared_ptr<const NearestNeighborSearch>& target_tree)
  : IntegratedICPFactor_<TargetFrame, SourceFrame>(target_key, source_key, target, source, target_tree, true) {}
 
  IntegratedPointToPlaneICPFactor_(
    gtsam::Key target_key,
    gtsam::Key source_key,
    const std::shared_ptr<const TargetFrame>& target,
    const std::shared_ptr<const SourceFrame>& source)
  : IntegratedICPFactor_<TargetFrame, SourceFrame>(target_key, source_key, target, source, true) {}
};
 
using IntegratedICPFactor = IntegratedICPFactor_<>;
using IntegratedPointToPlaneICPFactor = IntegratedPointToPlaneICPFactor_<>;
 
}  // namespace gtsam_points