registration.hpp

Go to the documentation of this file.

#pragma once
 
// 基础类和结果结构 / Base classes and result structures
#include <cpp-toolbox/pcl/registration/registration_result.hpp>
#include <cpp-toolbox/pcl/registration/base_coarse_registration.hpp>
#include <cpp-toolbox/pcl/registration/base_fine_registration.hpp>
 
// 粗配准算法 / Coarse registration algorithms
#include <cpp-toolbox/pcl/registration/ransac_registration.hpp>
#include <cpp-toolbox/pcl/registration/prosac_registration.hpp>
#include <cpp-toolbox/pcl/registration/four_pcs_registration.hpp>
#include <cpp-toolbox/pcl/registration/super_four_pcs_registration.hpp>
 
// 细配准算法 / Fine registration algorithms
#include <cpp-toolbox/pcl/registration/point_to_point_icp.hpp>
#include <cpp-toolbox/pcl/registration/point_to_plane_icp.hpp>
#include <cpp-toolbox/pcl/registration/generalized_icp.hpp>
#include <cpp-toolbox/pcl/registration/aa_icp.hpp>
#include <cpp-toolbox/pcl/registration/ndt.hpp>
 
// 必要的类型定义 / Required type definitions
#include <cpp-toolbox/types/point.hpp>
 
// 日志宏 / Logger macros
#include <cpp-toolbox/logger/thread_logger.hpp>
#define LOG_ERROR_S toolbox::logger::thread_logger_t::instance().error_s()
#define LOG_WARN_S toolbox::logger::thread_logger_t::instance().warn_s()
#define LOG_INFO_S toolbox::logger::thread_logger_t::instance().info_s()
 
// 便捷类型别名 / Convenient type aliases
namespace toolbox::pcl
{
 
// 粗配准单精度类型别名 / Coarse registration single precision aliases
using ransac_registration = ransac_registration_t<float>;
using prosac_registration = prosac_registration_t<float>;
using four_pcs_registration = four_pcs_registration_t<float>;
using super_four_pcs_registration = super_four_pcs_registration_t<float>;
using coarse_registration_result = registration_result_t<float>;
 
// 粗配准双精度类型别名 / Coarse registration double precision aliases
using ransac_registration_d = ransac_registration_t<double>;
using prosac_registration_d = prosac_registration_t<double>;
using four_pcs_registration_d = four_pcs_registration_t<double>;
using super_four_pcs_registration_d = super_four_pcs_registration_t<double>;
using coarse_registration_result_d = registration_result_t<double>;
 
// 细配准单精度类型别名 / Fine registration single precision aliases
using point_to_point_icp = point_to_point_icp_t<float>;
using point_to_plane_icp = point_to_plane_icp_t<float>;
using generalized_icp = generalized_icp_t<float>;
using aa_icp = aa_icp_t<float>;
using ndt = ndt_t<float>;
using fine_registration_result = fine_registration_result_t<float>;
 
// 细配准双精度类型别名 / Fine registration double precision aliases
using point_to_point_icp_d = point_to_point_icp_t<double>;
using point_to_plane_icp_d = point_to_plane_icp_t<double>;
using generalized_icp_d = generalized_icp_t<double>;
using aa_icp_d = aa_icp_t<double>;
using ndt_d = ndt_t<double>;
using fine_registration_result_d = fine_registration_result_t<double>;
 
template<typename DataType>
Eigen::Matrix<DataType, 4, 4> complete_registration(
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& source,
    const std::shared_ptr<toolbox::types::point_cloud_t<DataType>>& target,
    bool use_coarse = true,
    const std::string& fine_algorithm = "p2p")
{
  Eigen::Matrix<DataType, 4, 4> initial_guess = Eigen::Matrix<DataType, 4, 4>::Identity();
  
  // 粗配准 / Coarse registration
  if (use_coarse) {
    super_four_pcs_registration_t<DataType> coarse_reg;
    coarse_reg.set_source(source);
    coarse_reg.set_target(target);
    coarse_reg.set_delta(static_cast<DataType>(0.02));  // 2cm精度
    coarse_reg.set_overlap(static_cast<DataType>(0.5)); // 50%重叠
    
    registration_result_t<DataType> coarse_result;
    if (coarse_reg.align(coarse_result) && coarse_result.fitness_score > 0.3) {
      initial_guess = coarse_result.transformation;
      LOG_INFO_S << "粗配准成功，适应度分数 / Coarse registration successful, fitness score: " 
                 << coarse_result.fitness_score;
    }
  }
  
  // 细配准 / Fine registration
  fine_registration_result_t<DataType> fine_result;
  
  if (fine_algorithm == "p2p") {
    point_to_point_icp_t<DataType> reg;
    reg.set_source(source);
    reg.set_target(target);
    reg.set_max_iterations(50);
    reg.align(initial_guess, fine_result);
  } else if (fine_algorithm == "p2l") {
    if (!target->normals.empty()) {
      point_to_plane_icp_t<DataType> reg;
      reg.set_source(source);
      reg.set_target(target);
      reg.set_max_iterations(50);
      reg.align(initial_guess, fine_result);
    } else {
      LOG_WARN_S << "目标点云无法线，回退到Point-to-Point ICP / "
                    "Target cloud has no normals, falling back to Point-to-Point ICP";
      point_to_point_icp_t<DataType> reg;
      reg.set_source(source);
      reg.set_target(target);
      reg.set_max_iterations(50);
      reg.align(initial_guess, fine_result);
    }
  } else if (fine_algorithm == "gicp") {
    generalized_icp_t<DataType> reg;
    reg.set_source(source);
    reg.set_target(target);
    reg.set_max_iterations(50);
    reg.align(initial_guess, fine_result);
  } else if (fine_algorithm == "aa_icp") {
    aa_icp_t<DataType> reg;
    reg.set_source(source);
    reg.set_target(target);
    reg.set_max_iterations(50);
    reg.align(initial_guess, fine_result);
  } else if (fine_algorithm == "ndt") {
    ndt_t<DataType> reg;
    reg.set_source(source);
    reg.set_target(target);
    reg.set_resolution(static_cast<DataType>(0.5));  // 0.5m体素
    reg.set_max_iterations(50);
    reg.align(initial_guess, fine_result);
  } else {
    LOG_ERROR_S << "未知细配准算法 / Unknown fine registration algorithm: " << fine_algorithm;
    return initial_guess;
  }
  
  if (fine_result.converged) {
    LOG_INFO_S << "细配准收敛，迭代次数 / Fine registration converged, iterations: " 
               << fine_result.iterations_performed
               << "，最终误差 / , final error: " << fine_result.final_error;
  } else {
    LOG_WARN_S << "细配准未收敛，终止原因 / Fine registration did not converge, reason: " 
               << fine_result.termination_reason;
  }
  
  return fine_result.transformation;
}
 
 
}  // namespace toolbox::pcl