Lidar_imu自动标定源码阅读（二）——calibration部分

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

Lidar_parser_base.h：激光雷达分析器基础

#pragma once

#include 
#include 

// constexpr float deg2rad = M_PI / 180.0f;

//定义激光校正类
struct LaserCorrection {
  //rot代表旋转、vert代表垂直
  float rot_correction = 0.f; //0.f代表浮点数0
  float vert_correction = 0.f;
  float dist_correction = 0.f;
  bool two_pt_correction_available = false;
  float dist_correction_x = 0.f;
  float dist_correction_y = 0.f;
  float vert_offset_correction = 0.f;
  float horiz_offset_correction = 0.f;
  int max_intensity = 255;
  int min_intensity = 0;
  float focal_distance = 0.f;
  float focal_slope = 0.f;

  /* cached values calculated when the calibration file is read */
  float cos_rot_correction = 1.0f;  ///< cosine of rot_correction
  float sin_rot_correction = 0.f;   ///< sine of rot_correction
  float cos_vert_correction = 0.f;  ///< cosine of vert_correction
  float sin_vert_correction = 1.0f; ///< sine of vert_correction

  int laser_ring = 0; ///< ring number for this laser
};

struct LidarPointXYZIRT {
  PCL_ADD_POINT4D;
  float intensity;
  uint16_t ring;
  double timestamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(
    LidarPointXYZIRT,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
        uint16_t, ring, ring)(double, timestamp, timestamp))

calibration.hpp：对一些函数进行声明，具体实现在cpp中
#include //包含Matrix和Array类，基础的线性代数运算和数组操作
#include //包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块
#include //包含旋转，平移，缩放，2维和3维的各种变换
#include //基于体素网格化的滤波
#include //PCD文件的读写
#include //八叉树搜索
#include //点云类定义
#include //点类型定义

#pragma once

#include  //包含Matrix和Array类，基础的线性代数运算和数组操作
#include  //包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块
#include  //包含旋转，平移，缩放，2维和3维的各种变换
#include  //基于体素网格化的滤波
#include  //PCD文件的读写
#include //八叉树搜索
#include //点云类定义
#include //点类型定义

#include "common/Lidar_parser_base.h"

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

  // load data Tl2i代表lidar到imu的外参变换矩阵
  void LoadTimeAndPoes(const std::string &filename, const Eigen::Matrix4d &Tl2i,
                       std::vector &lidarTimes,
                       std::vector &lidarPoses);

  Eigen::Matrix4d GetDeltaTrans(double R[3], double t[3]);

  void Calibration(const std::string lidar_path, const std::string odom_path,
                   const Eigen::Matrix4d init_Tl2i);
  void SaveStitching(const Eigen::Matrix4d transform,
                     const std::string pcd_name);

private:
  int turn_ = 35;
  int window_ = 10;
  std::vector lidar_files_;
  std::vector lidar_poses_;
  // std::vector> pcd_seq_;
  double degree_2_radian = 0.017453293;
  std::string lidar_path_;
};

calibration.cpp：在其中用到了genPcdFeature（）函数和optimizeDeltaTrans（）,会在下篇博客中介绍。

std::unordered_map:

C++ std::unordered_map_肥喵王得福_ฅ・ω・ฅ的博客-CSDN博客_std unordered_mapunordered_map底层基于哈希表实现，拥有快速检索的功能。unordered_map是STL中的一种关联容器。容器中元素element成对出现（std::pair），element.first是该元素的键-key，容器element.second是该元素的键的值-value。unordered_map中每个key是唯一的，插入和查询速度接近于O(1)（在没有冲突的情况下），但是其内部元素的排列顺序是无序的。 1. unordered_map 底层原理2. 功能函数2.1 构造函数2.2..https://blog.csdn.net/u013271656/article/details/113810084?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166018243516781683963863%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166018243516781683963863&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-2-113810084-null-null.142%5Ev40%5Econtrol,185%5Ev2%5Econtrol&utm_term=%20std%3A%3Aunordered_map%3CVOXEL_LOC%2C%20OCTO_TREE%20*%3E%20surf_map%2C%20corn_map%3B&spm=1018.2226.3001.4187

#include "calibration.hpp"
#include "BALM.hpp"
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "gen_BALM_feature.hpp"
#include "logging.hpp"

Calibrator::Calibrator(){

};

Calibrator::~Calibrator(){

};

//利用输入的文件，得到lidar的时间戳和对应的位姿
void Calibrator::LoadTimeAndPoes(const std::string &filename,
                                 const Eigen::Matrix4d &Tl2i,
                                 std::vector &lidarTimes,
                                 std::vector &lidarPoses) {
  std::ifstream file(filename);
  if (!file.is_open()) {
    std::cout << "ERROR--->>> cannot open: " << filename << std::endl;
    exit(1);
  }
  // load pose and lidar timestamp(filename)
  std::string line;
  double max_x, max_y, max_z, min_x, min_y, min_z;
 //INT_MAX表示最大的整数值
  max_x = max_y = max_z = -INT_MAX;
  min_x = min_y = min_z = INT_MAX;
  while (getline(file, line)) {
    std::stringstream ss(line);
    std::string timeStr;
    ss >> timeStr;
    // lidarTimes.emplace_back(timeStr);
    lidar_files_.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 *= Tl2i;
    max_x = std::max(max_x, Ti(0, 3));
    max_y = std::max(max_y, Ti(1, 3));
    max_z = std::max(max_z, Ti(2, 3));
    min_x = std::min(min_x, Ti(0, 3));
    min_y = std::min(min_y, Ti(1, 3));
    min_z = std::min(min_z, Ti(2, 3));
    lidarPoses.emplace_back(Ti);
  }
  file.close();
}

//将传入的角度和平移变换变为用4*4的矩阵表示，得到deltaT
Eigen::Matrix4d Calibrator::GetDeltaTrans(double R[3], double t[3]) {
  Eigen::Matrix3d deltaR;
  double mat[9];
  // ceres::EulerAnglesToRotationMatrix(R, mat);
  ceres::AngleAxisToRotationMatrix(R, mat);
  deltaR << mat[0], mat[3], mat[6], mat[1], mat[4], mat[7], mat[2], mat[5],
      mat[8];
  // auto deltaR = Eigen::Matrix3d(
  //     Eigen::AngleAxisd(R[2], Eigen::Vector3d::UnitZ()) *
  //     Eigen::AngleAxisd(R[1], Eigen::Vector3d::UnitY()) *
  //     Eigen::AngleAxisd(R[0], Eigen::Vector3d::UnitX()));
  Eigen::Matrix4d deltaT = Eigen::Matrix4d::Identity();
  deltaT.block<3, 3>(0, 0) = deltaR;
  deltaT(0, 3) = t[0];
  deltaT(1, 3) = t[1];
  deltaT(2, 3) = t[2];
  return deltaT;
}

void Calibrator::Calibration(const std::string lidar_path,
                             const std::string odom_path,
                             const Eigen::Matrix4d init_Tl2i) {
  lidar_path_ = lidar_path;
  auto time_begin = std::chrono::steady_clock::now();
  int turn = 35;
  int window = 10;
  //   Eigen::Matrix4d init_Tl2i = Eigen::Matrix4d::Identity();
  Eigen::Matrix last_deltaT;
  //调用LoadTimeAndPoses()函数，传入里程计数据，得到lidar的位姿
  LoadTimeAndPoes(odom_path, init_Tl2i, lidar_files_, lidar_poses_);

  //利用滑动窗口选取关键帧数据，进行外参标定的校正
  std::vector frm_start_box;//定义frm起始框
  std::vector frm_step_box;//定义frm步进盒
  std::vector frm_num_box;//定义frm数字框
  int upper_bound = std::min(int(lidar_files_.size()), 1000);//设置上界
  int start_step = (upper_bound / 2) / turn_ - 1;//设置起始步骤
  //选取turn_(35)个窗口，可以理解为进行35次迭代
  for (int i = 0; i < turn_; i++) {
    int a = upper_bound / 2 - i * start_step - 1;
    frm_start_box.push_back(a);
    frm_step_box.push_back((upper_bound - a) / window_ - 1);
    frm_num_box.push_back(window_);
  }
  double deltaRPY[3] = {0, 0, 0};//RPYci表示欧拉角
  double deltaT[3] = {0, 0, 0};
  for (int i = 0; i < frm_start_box.size(); i++) {
    std::cout << "\n==>ROUND " << i << std::endl;
    int step = frm_step_box[i];
    int start = frm_start_box[i];
    int frmnum = frm_num_box[i];
    // The hash table of voxel map 体素映射的哈希表
    std::unordered_map surf_map, corn_map;
    // build voxel_map
    OCTO_TREE::imu_transmat.clear();
    Eigen::Matrix4d deltaTrans = GetDeltaTrans(deltaRPY, deltaT);
    OCTO_TREE::voxel_windowsize = frmnum;
    int window_size = frmnum; 
    //每个窗口内选取frmnum（10）次数据
    for (size_t frmIdx = 0; frmIdx < frmnum; frmIdx++) {
      int real_frmIdx = start + frmIdx * step;
      std::string lidar_file_name =
          lidar_path + lidar_files_[real_frmIdx] + ".pcd";
      pcl::PointCloud::Ptr cloud(
          new pcl::PointCloud);
      if (pcl::io::loadPCDFile(lidar_file_name, *cloud) < 0) {
        std::cout << "cannot open pcd_file: " << lidar_file_name << "\n";
        exit(1);//非正常运行导致退出程序
      }
      pcl::PointCloud::Ptr pl_corn(
          new pcl::PointCloud);
      pcl::PointCloud::Ptr pl_surf(
          new pcl::PointCloud);
      pcl::PointCloud::Ptr pl_surf_sharp(
          new pcl::PointCloud);
      // generate feature points
      // GenFeature feature;
      genPcdFeature(cloud, pl_surf, pl_surf_sharp, pl_corn);
      Eigen::Matrix4d imu_T = lidar_poses_[real_frmIdx];
      Eigen::Matrix4d refined_T = imu_T * deltaTrans;
      OCTO_TREE::imu_transmat.push_back(imu_T);
      if (i < turn / 2) {
        cut_voxel(surf_map, pl_surf_sharp, refined_T, 0, frmIdx,
                  window_size + 5);
      } else {
        cut_voxel(surf_map, pl_surf, refined_T, 0, frmIdx, window_size + 5);
      }
      // if (i > turn / 2)
      //     cut_voxel(corn_map, pl_corn, refined_T, 1, frmIdx, window_size +
      //     5);

      // Points in new frame have been distributed in corresponding root node
      // voxel
      // Then continue to cut the root voxel until right size
      for (auto iter = surf_map.begin(); iter != surf_map.end(); ++iter) {
        if (iter->second->is2opt) // Sliding window of root voxel should
                                  // have points
        {
          iter->second->root_centors.clear();
          iter->second->recut(0, frmIdx, iter->second->root_centors);
        }
      }

      for (auto iter = corn_map.begin(); iter != corn_map.end(); ++iter) {
        if (iter->second->is2opt) {
          iter->second->root_centors.clear();
          iter->second->recut(0, frmIdx, iter->second->root_centors);
        }
      }
    }
    // display
    // displayVoxelMap(surf_map);
    // optimize delta R, t1, t2
    if (i < turn / 2) {
      optimizeDeltaTrans(surf_map, corn_map, 4, deltaRPY, deltaT);
    } else {
      optimizeDeltaTrans(surf_map, corn_map, 2, deltaRPY, deltaT);
    }
    std::cout << "delta rpy: " << deltaRPY[0] / degree_2_radian << " "
              << deltaRPY[1] / degree_2_radian << " "
              << deltaRPY[2] / degree_2_radian << std::endl;
    std::cout << "delta T: " << deltaT[0] << " " << deltaT[1] << " "
              << deltaT[2] << std::endl;

    //  clear tree
    for (auto iter = corn_map.begin(); iter != corn_map.end(); ++iter) {
      clear_tree(iter->second);
    }
    for (auto iter = surf_map.begin(); iter != surf_map.end(); ++iter) {
      clear_tree(iter->second);
    }
    std::cout << "Round Finish!\n";
  }
  //将得到的结果放到bestVal中
  double bestVal[6];
  bestVal[0] = deltaRPY[0];
  bestVal[1] = deltaRPY[1];
  bestVal[2] = deltaRPY[2];
  bestVal[3] = deltaT[0];
  bestVal[4] = deltaT[1];
  bestVal[5] = deltaT[2];
  auto time_end = std::chrono::steady_clock::now();
  std::cout << "calib cost "
            << std::chrono::duration(time_end - time_begin).count()
            << "s" << std::endl;
  // save refined calib 保存得到的外参标定参数，并将数据输出到屏幕上
  std::string refine_calib_file = "./refined_calib_imu_to_lidar.txt";
  Eigen::Matrix4d deltaTrans = Eigen::Matrix4d::Identity();
  SaveStitching(deltaTrans,"before.pcd");
  deltaTrans = GetDeltaTrans(deltaRPY, deltaT);
  SaveStitching(deltaTrans,"after.pcd");
  std::cout << "delta T is:" << std::endl;
  std::cout << deltaTrans << std::endl;
  auto refined_Tl2i = init_Tl2i * deltaTrans;
  auto refined_Ti2l = refined_Tl2i.inverse().eval();
  std::cout << "refined T(imu 2 lidar): " << std::endl;
  std::cout << refined_Ti2l << std::endl;
  std::ofstream fCalib(refine_calib_file);
  if (!fCalib.is_open()) {
    std::cerr << "open file " << refine_calib_file << "failed." << std::endl;
    // return 1;
  }

  fCalib << "refined calib:" << std::endl;
  fCalib << "R: " << refined_Ti2l(0, 0) << " " << refined_Ti2l(0, 1) << " "
         << refined_Ti2l(0, 2) << " " << refined_Ti2l(1, 0) << " "
         << refined_Ti2l(1, 1) << " " << refined_Ti2l(1, 2) << " "
         << refined_Ti2l(2, 0) << " " << refined_Ti2l(2, 1) << " "
         << refined_Ti2l(2, 2) << std::endl;
  fCalib << "t: " << refined_Ti2l(0, 3) << " " << refined_Ti2l(1, 3) << " "
         << refined_Ti2l(2, 3) << std::endl;
  fCalib << "deltaTrans:" << std::endl;
  fCalib << deltaTrans << std::endl;
  fCalib << "delta roll, pitch, yaw, tx, ty, tz:" << std::endl;
  fCalib << bestVal[0] << " " << bestVal[1] << " " << bestVal[2] << " "
         << bestVal[3] << " " << bestVal[4] << " " << bestVal[5] << std::endl;
  fCalib << "delta roll, pitch, yaw, tx, ty, tz from begin:" << std::endl;
  fCalib << bestVal[0] + last_deltaT[0] << " " << bestVal[1] + last_deltaT[1]
         << " " << bestVal[2] + last_deltaT[2] << " "
         << bestVal[3] + last_deltaT[3] << " " << bestVal[4] + last_deltaT[4]
         << " " << bestVal[5] + last_deltaT[5] << std::endl;
  std::cout << "save refined calib to " << refine_calib_file << std::endl;
}

//实现SaveStihching()函数，利用标定后的外参变化矩阵，对点云数据进行校正
void Calibrator::SaveStitching(const Eigen::Matrix4d transform,
                               const std::string pcd_name) {

  pcl::PointCloud::Ptr all_cloud(
      new pcl::PointCloud());
  //实例化all_octree，并将体素分辨率设置为0.3
  pcl::octree::OctreePointCloudSearch::Ptr all_octree(
      new pcl::octree::OctreePointCloudSearch(0.3));

  all_octree->setInputCloud(all_cloud);
  pcl::PointCloud::Ptr cloud(
      new pcl::PointCloud);
  for (size_t i = 0; i < lidar_files_.size(); i++) {
    std::string lidar_file_name = lidar_path_ + lidar_files_[i] + ".pcd";
    if (pcl::io::loadPCDFile(lidar_file_name, *cloud) < 0) {
      LOGW("can not open %s", lidar_file_name);
      return;
    }
    Eigen::Matrix4d T = lidar_poses_[i] * transform;
    for (const auto &src_pt : cloud->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 = T.block<3, 3>(0, 0) * p + T.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 (!all_octree->isVoxelOccupiedAtPoint(dst_pt)) {
        all_octree->addPointToCloud(dst_pt, all_cloud);
      }
    }
  }
  pcl::io::savePCDFileASCII(pcd_name, *all_cloud);
  all_cloud->clear();
  all_octree->deleteTree();
}