graduated_non_convexity.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2025  Kenji Koide (k.koide@aist.go.jp)
#pragma once
 
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/ann/nearest_neighbor_search.hpp>
#include <gtsam_points/registration/registration_result.hpp>
 
namespace gtsam_points {
 
struct GNCParams {
public:
  // Feature matching parameters
  int max_init_samples = 5000;   
  bool reciprocal_check = true;  
  bool tuple_check = false;      
  double tuple_thresh = 0.9;     
  int max_num_tuples = 1000;     
 
  // Estimation praameters
  double div_factor = 1.4;     
  double max_corr_dist = 0.25; 
  int innter_iterations = 3;   
  int max_iterations = 64;     
  int dof = 6;                 
 
  // Misc
  bool verbose = false;        
  std::uint64_t seed = 5489u;  
  int num_threads = 4;         
};
 
template <typename PointCloud, typename Features>
RegistrationResult estimate_pose_gnc_(
  const PointCloud& target,
  const PointCloud& source,
  const Features& target_features,
  const Features& source_features,
  const NearestNeighborSearch& target_tree,
  const NearestNeighborSearch& target_features_tree,
  const NearestNeighborSearch& source_features_tree,
  const GNCParams& params = GNCParams());
 
//
RegistrationResult estimate_pose_gnc(
  const PointCloud& target,
  const PointCloud& source,
  const Eigen::Matrix<double, 33, 1>* target_features,
  const Eigen::Matrix<double, 33, 1>* source_features,
  const NearestNeighborSearch& target_tree,
  const NearestNeighborSearch& target_features_tree,
  const NearestNeighborSearch& source_features_tree,
  const GNCParams& params = GNCParams()) {
  using ConstFeaturePtr = const Eigen::Matrix<double, 33, 1>*;
  return estimate_pose_gnc_<PointCloud, ConstFeaturePtr>(
    target, source, target_features, source_features, target_tree, target_features_tree, source_features_tree, params);
}
 
RegistrationResult estimate_pose_gnc(
  const PointCloud& target,
  const PointCloud& source,
  const Eigen::Matrix<double, 125, 1>* target_features,
  const Eigen::Matrix<double, 125, 1>* source_features,
  const NearestNeighborSearch& target_tree,
  const NearestNeighborSearch& target_features_tree,
  const NearestNeighborSearch& source_features_tree,
  const GNCParams& params = GNCParams()) {
  using ConstFeaturePtr = const Eigen::Matrix<double, 125, 1>*;
  return estimate_pose_gnc_<PointCloud, ConstFeaturePtr>(
    target, source, target_features, source_features, target_tree, target_features_tree, source_features_tree, params);
}
 
RegistrationResult estimate_pose_gnc(
  const PointCloud& target,
  const PointCloud& source,
  const Eigen::VectorXd* target_features,
  const Eigen::VectorXd* source_features,
  const NearestNeighborSearch& target_tree,
  const NearestNeighborSearch& target_features_tree,
  const NearestNeighborSearch& source_features_tree,
  const GNCParams& params = GNCParams()) {
  using ConstFeaturePtr = const Eigen::VectorXd*;
  return estimate_pose_gnc_<PointCloud, ConstFeaturePtr>(
    target, source, target_features, source_features, target_tree, target_features_tree, source_features_tree, params);
}
 
}  // namespace gtsam_points