solver.hpp

// Copyright 2021 Yu-Kai Lin. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.
 
#pragma once
 
#include "kcp/common.hpp"
 
#include <teaser/registration.h>
 
namespace kcp {
 
class AbstractSolver {
 protected:
  Eigen::Matrix4d solution;
 
 public:
  AbstractSolver() : solution(Eigen::Matrix4d::Identity()){};
 
  virtual void solve(const Eigen::MatrixX3d& src,
                     const Eigen::MatrixX3d& dst,
                     const Eigen::MatrixXd& src_feature,
                     const Eigen::MatrixXd& dst_feature) = 0;
 
  virtual const Eigen::Matrix4d& get_solution() const { return this->solution; }
};
 
class KCP : public AbstractSolver {
 public:
  using TEASER = teaser::RobustRegistrationSolver;
 
  struct Params {
    TEASER::Params teaser;
 
    size_t k;
 
    bool verbose;
 
    Params() {
      k                                    = 2;
      verbose                              = false;
      teaser.noise_bound                   = 0.06;
      teaser.cbar2                         = 1;
      teaser.estimate_scaling              = false;
      teaser.rotation_gnc_factor           = 1.4;
      teaser.rotation_estimation_algorithm = TEASER::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
      teaser.rotation_max_iterations       = 100;
      teaser.rotation_cost_threshold       = 1e-6;
      teaser.kcore_heuristic_threshold     = 0.5;
      teaser.use_max_clique                = true;
      teaser.max_clique_exact_solution     = true;
      teaser.max_clique_time_limit         = 3600;
    }
  };
 
 protected:
  TEASER solver;
 
  KCP::Params params;
 
  Correspondences initial_correspondences;
 
  std::vector<int> inlier_correspondence_indices;
 
 public:
  KCP(KCP::Params params) : params(params), solver(params.teaser) {}
 
  KCP::Params& get_params() { return this->params; }
 
  const Correspondences& get_initial_correspondences() const { return this->initial_correspondences; }
 
  const std::vector<int>& get_inlier_correspondence_indices() const { return this->inlier_correspondence_indices; }
 
  virtual void solve(const Eigen::MatrixX3d& src,
                     const Eigen::MatrixX3d& dst,
                     const Eigen::MatrixXd& src_feature,
                     const Eigen::MatrixXd& dst_feature) override;
};
 
};  // namespace kcp