Lidar_imu自动标定源码阅读（一）——registration部分

源码阅读，能力有限，如有某处理解错误，请指出，谢谢。

logging.hpp：

宏定义#define#ifndef#endif：

宏定义#define #ifndef #endif_马小超i的博客-CSDN博客_#ifndef 多个宏目录#define 宏定义一、无参宏定义二、带参宏定义#ifndef 条件编译#define 宏定义在C或C++语言源程序中允许用一个标识符来表示一个字符串，称为“宏”。“define”为宏定义命令。被定义为“宏”的标识符称为“宏名”。在编译预处理时，对程序中所有出现的“宏名”，都用宏定义中的字符串去代换，这称为“宏代换”或“宏展开”。宏定义是由源程序中的宏定义命令完成的。宏代换是由预处理程序自动完成的。优点：　(1) 方便程序的修改。这个就不多说了。（2...https://blog.csdn.net/qq_21989927/article/details/108619694?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165994503916782246467579%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165994503916782246467579&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-108619694-null-null.142%5Ev39%5Econtrol,185%5Ev2%5Econtrol&utm_term=ifndef%20define%20endif%E4%BD%9C%E7%94%A8&spm=1018.2226.3001.4187strrchr()函数：

strrchr函数_悦来客栈的老板的博客-CSDN博客/*函数名称： strrchr函数原型：char *strrchr(const char *str, int c);所属库： string.h函数功能：查找一个字符c在另一个字符串str中末次出现的位置（也就是从str的右侧开始查找字符c首次出现的位置），并返回从字符串中的这个位置起，一直到字符串结束的所有字符。如果未能找到指定字符，那么函数将返回NULL。这个函数和shttps://blog.csdn.net/qq523176585/article/details/11923695?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165994544416782350819884%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165994544416782350819884&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-11923695-null-null.142%5Ev39%5Econtrol,185%5Ev2%5Econtrol&utm_term=strrchr%E5%87%BD%E6%95%B0&spm=1018.2226.3001.4187

#ifndef LOGGING_HPP_   //先测试LOGGING_HPP_是否被定义过
#define LOGGING_HPP_

#define OUTPUT
#define __FILENAME__                                                           \
  (strrchr(__FILE__, '/') ? (strrchr(__FILE__, '/') + 1) : __FILE__)

#ifdef OUTPUT
#define LOGI(...)                                                              \
  (printf("[INFO] [%d@%s] ", __LINE__, __FILENAME__), printf(__VA_ARGS__),     \
   printf("\n"))
#define LOGW(...)                                                              \
  (printf("\33[33m[WARN] [%d@%s] ", __LINE__, __FILENAME__),                   \
   printf(__VA_ARGS__), printf("\033[0m\n"))
#define LOGE(...)                                                              \
  (printf("\33[31m[ERROR] [%d@%s] ", __LINE__, __FILENAME__),                  \
   printf(__VA_ARGS__), printf("\033[0m\n"))
#else
#define LOGI(...) ((void)0)
#define LOGW(...) ((void)0)
#define LOGE(...) ((void)0)
#endif

#ifdef DEBUG
#define LOGDEBUG(...) (printf(__VA_ARGS__), printf("\n"))
#else
#define LOGDEBUG(...) ((void)0)
#endif

#endif // LOGGING_HPP_

//#ifndef #define #endif联用

transform_util.hpp：

Eigen库块操作：
Eigen子矩阵操作_weixin_34297300的博客-CSDN博客1 子矩阵操作简介子矩阵操作又称块操作，在矩阵运算中，子矩阵的提取和操作应用也十分广泛。因此Eigen中也提供了相关操作的方法。提取的子矩阵在操作过程中既可以用作左值也可以用作右值。2 块操作的一般使用方法在Eigen中最基本的快操作运算是用.block()完成的。提取的子矩阵同样分为动态大小和固定大小。块操作构建动态大小子矩阵提取块大小为(p,q),起始于(i,j)m...https://blog.csdn.net/weixin_34297300/article/details/93329457?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165994680716781790787704%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165994680716781790787704&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-93329457-null-null.142%5Ev39%5Econtrol,185%5Ev2%5Econtrol&utm_term=%E7%9F%A9%E9%98%B5block%E5%87%BD%E6%95%B0&spm=1018.2226.3001.4187Eigen 中旋转向量、旋转矩阵、欧拉角、四元数的初始化及相互转换：

eigen 中旋转向量、旋转矩阵、欧拉角、四元数的初始化及相互转换_可即的博客-CSDN博客_eigen 欧拉角转旋转矩阵一、旋转向量1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z) Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z)) 1.1 旋转向量转旋转矩阵 Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.matrix(); Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.tohttps://blog.csdn.net/xiaojinger_123/article/details/124376199?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165997168316782248587203%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165997168316782248587203&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-124376199-null-null.142%5Ev39%5Econtrol,185%5Ev2%5Econtrol&utm_term=AngleAxisd&spm=1018.2226.3001.4187

#ifndef TRANSFORM_UTIL_HPP_
#define TRANSFORM_UTIL_HPP_

#include 
#include 

//定义内联函数，进行角度和弧度的相互转换
inline double rad2deg(double radians) { return radians * 180.0 / M_PI; }
inline double deg2rad(double degrees) { return degrees * M_PI / 180.0; }

class TransformUtil {
public:
  //=default 来要求编译器生成默认构造函数，仅仅是因为我们既需要其他形式的构造函数，也需要默认的构造函数。析构函数同样如此。
  TransformUtil() = default;
  ~TransformUtil() = default;

  static Eigen::Matrix4d GetMatrix(const Eigen::Vector3d &translation,
                                   const Eigen::Matrix3d &rotation) {
    Eigen::Matrix4d ret = Eigen::Matrix4d::Identity();
    //block(i,j)提取块大小为(p,q),起始于(i,j)
    ret.block<3, 1>(0, 3) = translation;
    ret.block<3, 3>(0, 0) = rotation;
    return ret;
  }

  static Eigen::Matrix4d Matrix4FloatToDouble(const Eigen::Matrix4f &matrix) {
    Eigen::Matrix4d ret = Eigen::Matrix4d::Identity();
    ret << matrix(0), matrix(4), matrix(8), matrix(12), matrix(1), matrix(5),
        matrix(9), matrix(13), matrix(2), matrix(6), matrix(10), matrix(14),
        matrix(3), matrix(7), matrix(11), matrix(15);
    return ret;
  }

  static Eigen::Matrix4d GetDeltaT(const float var[6]) {
    //欧拉角转换为旋转矩阵，按照内旋（绕自身的旋转轴进行旋转，按照z、y、x进行相乘）得到旋转矩阵
    auto deltaR = Eigen::Matrix3d(
        Eigen::AngleAxisd(deg2rad(var[2]), Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(deg2rad(var[1]), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(deg2rad(var[0]), Eigen::Vector3d::UnitX()));
    Eigen::Matrix4d deltaT = Eigen::Matrix4d::Identity();
    deltaT.block<3, 3>(0, 0) = deltaR;
    deltaT(0, 3) = var[3];
    deltaT(1, 3) = var[4];
    deltaT(2, 3) = var[5];
    return deltaT;
  }
};

#endif // TRANSFORM_UTIL_HPP_

registration.hpp：

#pragma once一般由编译器提供保证：同一个文件不会被包含多次。这里所说的”同一个文件”是指物理上的一个文件，而不是指内容相同的两个文件。无法对一个头文件中的一段代码作#pragma once声明，而只能针对文件。

Eigen库：

Core #include，包含Matrix和Array类，基础的线性代数运算和数组操作。

Geometry #include，包含旋转，平移，缩放，2维和3维的各种变换。

LU #include，包含求逆，行列式，LU分解。

Cholesky #include，包含LLT和LDLT Cholesky分解。

SVD `#include，包含SVD分解。

QR `#include，包含QR分解。

Eigenvalues #include，包含特征值，特征向量分解。

Sparse #include，包含稀疏矩阵的存储和运算。

Dense #include，包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块。

Eigen #include，包含Dense和Sparse

#include                         //PCD文件的读写
#include                     //点类型定义
#include                      //点云类定义
#include         //KD-Tree搜索
pcl头文件介绍：

PCL：头文件汇总及说明_孙 悟 空的博客-CSDN博客_pcl头文件PCL官网上给出的功能模块(modules）如下滤波器 Filters特征 Features关键点 Keypoints配准 RegistrationKd(K-dimensional)树 KdTree八叉树 Octree分割 Segmentation采样一致性 Sample Consesus表面 Surface范围图像 Range Image输入输出 I/O可视化 Visualization常用 Common搜索 Searchpcl头文件汇总及说明#include <https://blog.csdn.net/weixin_46098577/article/details/110951471?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165992690316780366551203%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=165992690316780366551203&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-110951471-null-null.142%5Ev39%5Econtrol,185%5Ev2%5Econtrol&utm_term=pcl%E5%A4%B4%E6%96%87%E4%BB%B6&spm=1018.2226.3001.4187

pcl 点云对象的两种定义方式的区别与转换:

PCL学习（4.5）——点云对象的两种定义方式的区别与转换_菜鸟知识搬运工的博客-CSDN博客创建与访问第一种，是一种vector的赋值方式，将point数据push_back到pcl::PointXYZ类型的模板中。pcl::PointCloud<pcl::PointXYZ> pointCloud;pcl::PointXYZ point; point.x = 2.0f - y; point.y = y; point.z = z;...https://blog.csdn.net/qq_30815237/article/details/86509741?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2~default~CTRLIST~default-1-86509741-blog-110189154.pc_relevant_sortByAnswer&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2~default~CTRLIST~default-1-86509741-blog-110189154.pc_relevant_sortByAnswer&utm_relevant_index=1

#pragma once

#include 
#include 
#include 
#include 
#include 
#include 

struct NDTParameter {
  double ndt_maxIterations = 50;
  double ndt_transformation_epsilon = 0.05;
  double ndt_step_size = 0.1; // need to adjust
  double resolution = 10;     // nedd to adjust
};

struct PlaneParam {
  PlaneParam() {}
  PlaneParam(const Eigen::Vector3d &n, double i) : normal(n), intercept(i) {}
  Eigen::Vector3d normal;
  double intercept;
};

struct PlaneParamError {
  PlaneParamError() {}
  PlaneParamError(double n, double i) : normal_error(n), intercept_error(i) {}
  double normal_error;
  double intercept_error;
};

struct PointCloudBbox {
  int min_x = 0;
  int min_y = 0;
  int min_z = 0;

  int max_x = 0;
  int max_y = 0;
  int max_z = 0;
};

class Registrator {
public:
  Registrator();
  ~Registrator();

  bool
  GroundPlaneExtraction(const pcl::PointCloud::Ptr &in_cloud,
                        pcl::PointCloud::Ptr g_cloud,
                        pcl::PointCloud::Ptr ng_cloud,
                        PlaneParam &plane);
  bool
  GroundPlaneExtraction(const pcl::PointCloud::Ptr &in_cloud,
                        PlaneParam &g_param);

  void
  PointCloudDownSampling(const pcl::PointCloud::Ptr &in_cloud,
                         double voxel_size,
                         pcl::PointCloud::Ptr &out_cloud);

  void
  PointCloudFilterByROI(const pcl::PointCloud::Ptr &in_cloud,
                        const PointCloudBbox &roi,
                        pcl::PointCloud::Ptr &out_cloud);

  //计算体素
  size_t ComputeVoxelOccupancy(float var[6]);

  double ComputeGroundRegistrationError(float var[6]);

  // registration method
  bool RegistrationByGroundPlane(Eigen::Matrix4d &transform);
  bool RegistrationByVoxelOccupancy(Eigen::Matrix4d &transform);

  // load data
  void LoadOdometerData(const std::string odometer_file,
                        const Eigen::Matrix4d &initial_extrinsic);
  void LoadLidarPCDs(const std::string &pcds_dir);

  void SaveStitching(const std::string &stitching_path);

private:
  //利用指针类模板，创建点云类对象，采用“->points[i]”方式赋值。
  pcl::PointCloud::Ptr all_cloud_;
  pcl::octree::OctreePointCloudSearch::Ptr all_octree_;

  //创建数组用于存放不同的点云数据
  std::vector> pcds_;
  std::vector> pcds_g_cloud_;
  std::vector> pcds_ng_cloud_;
  std::vector timestamp_;
  //创建数组4*4矩阵用于存放lidar位姿
  std::vector lidar_poses_;

  float curr_best_extrinsic_[6] = {0};
  Eigen::Matrix4d lidar2imu_initial_ = Eigen::Matrix4d::Identity();

  int intensity_threshold_ = 35;
};

registration.cpp：

#include "registration.hpp"

#include    //点云坐标变化
#include    //数据类型的转换
#include    //法线特征估计
#include    //索引提取
#include    //kdtree头文件
#include    //octree头文件
#include    //点类型定义
#include   //GICP配准
#include    //ICP配准
#include    //非线性ICP配准
#include    //NDT配准
#include    //随机参数估计方法
#include    //模型定义
#include    //基于采样一致性分割

#include "logging.hpp"
#include "omp.h"
#include "transform_util.hpp"

Registrator::Registrator() {
  all_cloud_.reset(new pcl::PointCloud());
  all_octree_.reset(
      new pcl::octree::OctreePointCloudSearch(0.1));
  all_octree_->setInputCloud(all_cloud_);
};

Registrator::~Registrator() {
  //shrink_to_fit()函数，可以在执行erase之后让vector的size()等于capacity()，使删除数据后的数组的capacity（）大小等于现在的现在的size()大小
  pcds_.clear();
  pcds_.shrink_to_fit();
  pcds_g_cloud_.clear();
  pcds_g_cloud_.shrink_to_fit();
  pcds_ng_cloud_.clear();
  pcds_ng_cloud_.shrink_to_fit();
};

//加载里程计数据，利用lidar2imu的初始外参标定，得到里程计位姿数据对应时间戳lidar的位姿
void Registrator::LoadOdometerData(const std::string odometer_file,
                                   const Eigen::Matrix4d &initial_extrinsic) {
  lidar2imu_initial_ = initial_extrinsic;
  std::ifstream file(odometer_file);
  if (!file.is_open()) {
    LOGW("can not open %s", odometer_file);
    return;
  }
  // load pose and timestamp
  std::string line;
  while (getline(file, line)) {
    std::stringstream ss(line);
    std::string timeStr;
    ss >> timeStr;
    timestamp_.emplace_back(timeStr);
    Eigen::Matrix4d Ti = Eigen::Matrix4d::Identity();
    ss >> Ti(0, 0) >> Ti(0, 1) >> Ti(0, 2) >> Ti(0, 3) >> Ti(1, 0) >>
        Ti(1, 1) >> Ti(1, 2) >> Ti(1, 3) >> Ti(2, 0) >> Ti(2, 1) >> Ti(2, 2) >>
        Ti(2, 3);
    Ti *= initial_extrinsic;
    lidar_poses_.emplace_back(Ti);
  }
  file.close();
}

//加载点云数据，对点云数据进行地面点和非地面点提取（分割和滤波），并利用体素网格法对提取后的点云数据进行下采样
void Registrator::LoadLidarPCDs(const std::string &pcds_dir) {
  pcds_g_cloud_.reserve(timestamp_.size());
  pcds_g_cloud_.resize(timestamp_.size());
  pcds_ng_cloud_.reserve(timestamp_.size());
  pcds_ng_cloud_.resize(timestamp_.size());
  pcds_.resize(timestamp_.size());
  pcl::PointCloud::Ptr cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr filter_cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr g_cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr ng_cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr g_filter_cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr ng_filter_cloud(
      new pcl::PointCloud);
  pcl::PointCloud::Ptr ng_filter_cloud_roi(
      new pcl::PointCloud);
  PointCloudBbox roi;
  roi.max_x = 100;
  roi.min_x = -100;
  roi.max_y = 50;
  roi.min_y = -50;
  roi.max_z = 5;
  roi.min_z = -5;
  PlaneParam plane;
  for (size_t i = 0; i < timestamp_.size(); ++i) {
    std::string lidar_file_name = pcds_dir + "/" + timestamp_[i] + ".pcd";
    if (pcl::io::loadPCDFile(lidar_file_name, *cloud) < 0) {
      LOGW("can not open %s", lidar_file_name);
      return;
    }
    pcds_[i] = *cloud;
    GroundPlaneExtraction(cloud, g_cloud, ng_cloud, plane);

    PointCloudDownSampling(g_cloud, 1, g_filter_cloud);
    pcds_g_cloud_[i] = *g_filter_cloud;
    PointCloudDownSampling(ng_cloud, 0.5, ng_filter_cloud);
    // PointCloudFilterByROI(ng_filter_cloud, roi, ng_filter_cloud_roi);
    pcds_ng_cloud_[i] = *ng_filter_cloud_roi;
    printf("\rload: %lu/%lu, %s", i, timestamp_.size() - 1,
           lidar_file_name.c_str());
  }
  LOGI("load done!");
  LOGI("the number of pcd is %d", pcds_g_cloud_.size());
}

//根据lidar的位姿信息得到点云的位置信息，并将有效的点云信息进行保存
void Registrator::SaveStitching(const std::string &stitching_path) {
  int interval = 10;
  Eigen::Matrix4d delta_pose = TransformUtil::GetDeltaT(curr_best_extrinsic_);
  //size_t 类型表示C中任何对象所能达到的最大长度，它是无符号整数
  for (size_t i = 0; i < pcds_.size(); i++) {
    if (i % interval != 0)
      continue;

    //lidar_poses_为由里程计数据利用初始化的外参标定数据得到的lidar位姿
    Eigen::Matrix4d lidar_pose = lidar_poses_[i];
    //得到lidar姿态发生后的位置
    lidar_pose *= delta_pose;

    //auto的原理就是根据后面的值，来自己推测前面的类型是什么。auto的作用就是为了简化变量初始化，如果这个变量有一个很长很长的初始化类型，就可以用auto代替。
    //利用pcl_isfinite()检查点是否属于无效点，返回值为bool类型，如果属于无效点，则跳过该点
    for (const auto &src_pt : pcds_[i].points) {
      if (!pcl_isfinite(src_pt.x) || !pcl_isfinite(src_pt.y) ||
          !pcl_isfinite(src_pt.z))
        continue;
      Eigen::Vector3d p(src_pt.x, src_pt.y, src_pt.z);
      Eigen::Vector3d p_res;
      //得到点云相对于激光雷达位置的位置信息p_res，点云X/Y/Z方向的偏移量是以激光雷达的安装位置作为原点
      p_res = lidar_pose.block<3, 3>(0, 0) * p + lidar_pose.block<3, 1>(0, 3);
      pcl::PointXYZI dst_pt;
      dst_pt.x = p_res(0);
      dst_pt.y = p_res(1);
      dst_pt.z = p_res(2);
      dst_pt.intensity = src_pt.intensity;
      if (dst_pt.intensity >= intensity_threshold_ &&
          !all_octree_->isVoxelOccupiedAtPoint(dst_pt)) {
        all_octree_->addPointToCloud(dst_pt, all_cloud_);
      }
    }
  }
  all_cloud_->width = all_cloud_->points.size();//设置点云宽度
  all_cloud_->height = 1; //高度为1，说明为无序点云
  pcl::io::savePCDFileASCII(stitching_path, *all_cloud_);

  all_cloud_->clear();
  all_octree_->deleteTree();
}

//通过计算体素，得到yaw、tx、ty数据的标定误差
bool Registrator::RegistrationByVoxelOccupancy(Eigen::Matrix4d &transform) {
  //
  const float rpy_resolution = 0.05;
  const float xyz_resolution = 0.01;
  float var[6] = {0}, bestVal[6] = {0};
  std::string varName[6] = {"roll", "pitch", "yaw", "tx", "ty", "tz"};
  int direction[2] = {1, -1};
  for (size_t i = 0; i < 6; i++) {
    var[i] = curr_best_extrinsic_[i];
    bestVal[i] = curr_best_extrinsic_[i];
  }

  size_t min_voxel_occupancy = ComputeVoxelOccupancy(var);
  int max_iteration = 60;
  // yaw
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {
      var[2] = iter * direction[j] * rpy_resolution;
      std::cout << varName[2] << ": " << var[2] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[2] = var[2];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[2] = bestVal[2];
  // tx
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[3] = iter * direction[j] * xyz_resolution;
      std::cout << varName[3] << ": " << var[3] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[3] = var[3];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[3] = bestVal[3];
  // ty
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[4] = iter * direction[j] * xyz_resolution;
      std::cout << varName[4] << ": " << var[4] << std::endl;
      size_t cnt = ComputeVoxelOccupancy(var);
      if (cnt < min_voxel_occupancy * (1 - 1e-4)) {
        min_voxel_occupancy = cnt;
        bestVal[4] = var[4];
        std::cout << "points decrease to: " << min_voxel_occupancy << std::endl;

      } else {
        std::cout << "points increase to: " << cnt << std::endl;
        break;
      }
    }
  }
  var[4] = bestVal[4];
  for (size_t i = 0; i < 6; i++) {
    curr_best_extrinsic_[i] = bestVal[i];
  }
  std::cout << "roll: " << bestVal[0] << ", pitch: " << bestVal[1]
            << ", yaw: " << bestVal[2] << ", tx: " << bestVal[3]
            << ", ty: " << bestVal[4] << ", tz: " << bestVal[5] << std::endl;
  std::cout << "points: " << min_voxel_occupancy << std::endl;

  // calib result
  Eigen::Matrix4d deltaT = TransformUtil::GetDeltaT(bestVal);
  transform = lidar2imu_initial_ * deltaT;
  transform = transform.inverse().eval();

  return true;
}

//调用ComputeGroundRegistrationError()函数，计算得到的误差可以看作标定过程中的roll、pitch角误差
bool Registrator::RegistrationByGroundPlane(Eigen::Matrix4d &transform) {

  const float rpy_resolution = 0.02;
  float var[6] = {0}, bestVal[6] = {0};
  std::string varName[6] = {"roll", "pitch", "yaw", "tx", "ty", "tz"};
  int direction[2] = {1, -1};
  int max_iteration = 300;
  double min_error = ComputeGroundRegistrationError(var);
  // roll
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[0] = iter * direction[j] * rpy_resolution;
      std::cout << varName[0] << ": " << var[0] << std::endl;
      double error = ComputeGroundRegistrationError(var);
      if (error < min_error) {
        min_error = error;
        bestVal[0] = var[0];
        std::cout << "error decrease to: " << min_error << std::endl;

      } else {
        std::cout << "error increase to: " << error << std::endl;
        break;
      }
    }
  }
  var[0] = bestVal[0];
  // pitch
  for (int j = 0; j <= 1; j++) {
    for (int iter = 1; iter < max_iteration; iter++) {

      var[1] = iter * direction[j] * rpy_resolution;
      std::cout << varName[1] << ": " << var[1] << std::endl;
      double error = ComputeGroundRegistrationError(var);
      if (error < min_error) {
        min_error = error;
        bestVal[1] = var[1];
        std::cout << "error decrease to: " << min_error << std::endl;

      } else {
        std::cout << "error increase to: " << error << std::endl;
        break;
      }
    }
  }
  var[1] = bestVal[1];
  for (size_t i = 0; i < 6; i++) {
    curr_best_extrinsic_[i] = bestVal[i];
  }
  // calib result
  Eigen::Matrix4d deltaT = TransformUtil::GetDeltaT(bestVal);
  transform = lidar2imu_initial_ * deltaT;
  transform = transform.inverse().eval();
  return true;
}

//使用VoxelGrid滤波器（体素化网格方法）对点云进行下采样
void Registrator::PointCloudDownSampling(
    const pcl::PointCloud::Ptr &in_cloud, double voxel_size,
    pcl::PointCloud::Ptr &out_cloud) {
  // 创建滤波器对象
  pcl::VoxelGrid sor;
  sor.setInputCloud(in_cloud);
  sor.setLeafSize(voxel_size, voxel_size, voxel_size);设置三维栅格的大小，数值越大，栅格越大，采样后的点云数据越小
  sor.filter(*out_cloud);
}

void Registrator::PointCloudFilterByROI(
    const pcl::PointCloud::Ptr &in_cloud,
    const PointCloudBbox &roi,
    pcl::PointCloud::Ptr &out_cloud) {
  out_cloud->clear();
  for (const auto &src_pt : in_cloud->points) {
    if (src_pt.x > roi.min_x && src_pt.x < roi.max_x) {
      if (src_pt.y > roi.min_y && src_pt.y < roi.max_y) {
        if (src_pt.z > roi.min_z && src_pt.z < roi.max_z) {
          out_cloud->points.push_back(src_pt);
        }
      }
    }
  }
}

//对输入的点云数据进行分割和滤波，提取地面点和非地面点，并将得到的系数coefficient赋值给plane
bool Registrator::GroundPlaneExtraction(
    const pcl::PointCloud::Ptr &in_cloud,
    pcl::PointCloud::Ptr g_cloud,
    pcl::PointCloud::Ptr ng_cloud, PlaneParam &plane) {
  //对系数和内点进行实例化
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  //创建分割对象
  pcl::SACSegmentation seg;
  //可选设置
  seg.setOptimizeCoefficients(true);//设置对估计的模型做优化处理
  //必选设置
  seg.setModelType(pcl::SACMODEL_PLANE);//设置分割模型类别
  seg.setMethodType(pcl::SAC_RANSAC);//设置使用那个随机参数估计方法
  seg.setDistanceThreshold(0.2);//设置是否为模型内点的距离阈值
  seg.setInputCloud(in_cloud);
  seg.segment(*inliers, *coefficients);
  //判断分割是否成功
  if (inliers->indices.size() == 0) {
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    return (-1);
  }
  // 创建滤波器对象
  pcl::ExtractIndices extract;
  // 分离内层
  extract.setInputCloud(in_cloud);
  extract.setIndices(inliers);//设置分割后的内点为需要提取的点集
  extract.setNegative (false);//设置提取内点而非外点
  extract.filter(*g_cloud);//提取输出存储到g_clound
  extract.setNegative(true);/提取出其余的点，也就是外点
  extract.filter(*ng_cloud);//提取输出存储到ng_clound
  plane.normal(0) = coefficients->values[0];
  plane.normal(1) = coefficients->values[1];
  plane.normal(2) = coefficients->values[2];
  plane.intercept = coefficients->values[3];
  return true;
}

bool Registrator::GroundPlaneExtraction(
    const pcl::PointCloud::Ptr &in_cloud, PlaneParam &plane) {
  //分割结果-系数因子coeffcients：比如一个平面的方程式为：aX + bY + cZ + d = 0；在此次实验中得出的系数因子为：0 0 1 -1；即 a=0 , b=0 , c=1, d=-1；将平面上的点代入该方程即可验证结果为正确。分割结果中包括在平面模型内的点的下标（在原始点云中），通过这个下标，就可以将平面内和平面外的点云分割开，单独显示。
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::SACSegmentation seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.2);
  seg.setInputCloud(in_cloud);
  seg.segment(*inliers, *coefficients);
  if (inliers->indices.size() == 0) {
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    exit(1);
  }

  plane.normal(0) = coefficients->values[0];
  plane.normal(1) = coefficients->values[1];
  plane.normal(2) = coefficients->values[2];
  plane.intercept = coefficients->values[3];
  return true;
}

//计算体素，先求出点云，然后点云数据进行搜索，得到体素
size_t Registrator::ComputeVoxelOccupancy(float var[6]) {
  int interval = 10;
  Eigen::Matrix4d delta_pose = TransformUtil::GetDeltaT(var);
  for (size_t i = 0; i < pcds_.size(); i++) {
    if (i % interval != 0)
      continue;

    Eigen::Matrix4d lidar_pose = lidar_poses_[i];
    lidar_pose *= delta_pose;

#pragma omp parallel for
    for (const auto &src_pt : pcds_[i].points) {
      if (!pcl_isfinite(src_pt.x) || !pcl_isfinite(src_pt.y) ||
          !pcl_isfinite(src_pt.z))
        continue;

      Eigen::Vector3d p(src_pt.x, src_pt.y, src_pt.z);
      Eigen::Vector3d p_res;
      p_res = lidar_pose.block<3, 3>(0, 0) * p + lidar_pose.block<3, 1>(0, 3);
      pcl::PointXYZI dst_pt;
      dst_pt.x = p_res(0);
      dst_pt.y = p_res(1);
      dst_pt.z = p_res(2);
      dst_pt.intensity = src_pt.intensity;
      if (dst_pt.intensity >= intensity_threshold_ &&
          !all_octree_->isVoxelOccupiedAtPoint(dst_pt)) {

#pragma omp critical
        all_octree_->addPointToCloud(dst_pt, all_cloud_);
      }
    }
    printf("\rprocessing: %lu/%lu", i, pcds_.size() - 1);
    std::fflush(stdout);
  }
  size_t pcdCnt = all_cloud_->size();
  all_cloud_->clear();
  all_octree_->deleteTree();
  return pcdCnt;
}

//计算通过通过地面登记的误差
double Registrator::ComputeGroundRegistrationError(float var[6]) {
  pcl::PointCloud::Ptr transformed_cloud(
      new pcl::PointCloud);
  std::vector normals_list;
  std::vector intercepts_list;
  Eigen::Vector3d normals_mean(0, 0, 0);
  double intercepts_mean = 0;
  PlaneParam plane_param;

  Eigen::Matrix4d delta_pose = TransformUtil::GetDeltaT(var);
  //对每帧点云进行分割，将分割后得到的系数进行求和
  for (size_t i = 0; i < pcds_g_cloud_.size(); i++) {
    Eigen::Matrix4d lidar_pose = lidar_poses_[i];
    lidar_pose *= delta_pose;
    //pcl::transformPointCloud(*source_cloud, *target_cloud, transform)，其中source_cloud, target_cloud的类型为pcl::PointCloud::Ptr，函数的作用是通过转换矩阵transform将source_cloud转换后存到target_cloud中保存。
    pcl::transformPointCloud(pcds_g_cloud_[i], *transformed_cloud, lidar_pose);
    GroundPlaneExtraction(transformed_cloud, plane_param);
    normals_list.push_back(plane_param.normal);
    intercepts_list.push_back(plane_param.intercept);

    normals_mean += plane_param.normal;
    intercepts_mean += plane_param.intercept;
  }

  //所求的和取平均值
  normals_mean = normals_mean / pcds_g_cloud_.size();
  intercepts_mean = intercepts_mean / pcds_g_cloud_.size();
  double normal_error = 0;
  double intercept_error = 0;
  double min = 0, max = 0;
  //计算每帧点云对应平面和所求平均平面之间的角度误差
  for (int i = 0; i < normals_list.size(); i++) {
    //acos（）为取反余弦，
    double alpha = std::acos(normals_list[i].dot(normals_mean));
    normal_error += std::abs(alpha);
    intercept_error += std::abs(intercepts_list[i] - intercepts_mean);
  }
  return normal_error;
}