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

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

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

#pragma once#include <pcl/point_cloud.h>
#include <pcl/point_types.h>// constexpr float deg2rad = M_PI / 180.0f;//定义激光校正类
struct LaserCorrection {//rot代表旋转、vert代表垂直float rot_correction = 0.f; //0.f代表浮点数0float 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_correctionfloat sin_rot_correction = 0.f;   ///< sine of rot_correctionfloat cos_vert_correction = 0.f;  ///< cosine of vert_correctionfloat sin_vert_correction = 1.0f; ///< sine of vert_correctionint 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 <eigen3/Eigen/Core> //包含Matrix和Array类，基础的线性代数运算和数组操作
#include <eigen3/Eigen/Dense> //包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块
#include <eigen3/Eigen/Geometry> //包含旋转，平移，缩放，2维和3维的各种变换
#include <pcl/filters/voxel_grid.h> //基于体素网格化的滤波
#include <pcl/io/pcd_io.h> //PCD文件的读写
#include <pcl/octree/octree_search.h>//八叉树搜索
#include <pcl/point_cloud.h>//点云类定义
#include <pcl/point_types.h>//点类型定义

#pragma once#include <eigen3/Eigen/Core> //包含Matrix和Array类，基础的线性代数运算和数组操作
#include <eigen3/Eigen/Dense> //包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块
#include <eigen3/Eigen/Geometry> //包含旋转，平移，缩放，2维和3维的各种变换
#include <pcl/filters/voxel_grid.h> //基于体素网格化的滤波
#include <pcl/io/pcd_io.h> //PCD文件的读写
#include <pcl/octree/octree_search.h>//八叉树搜索
#include <pcl/point_cloud.h>//点云类定义
#include <pcl/point_types.h>//点类型定义#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<std::string> &lidarTimes,std::vector<Eigen::Matrix4d> &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<std::string> lidar_files_;std::vector<Eigen::Matrix4d> lidar_poses_;// std::vector<pcl::PointCloud<LidarPointXYZIRT>> 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<std::string> &lidarTimes,std::vector<Eigen::Matrix4d> &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<double, 6, 1> last_deltaT;//调用LoadTimeAndPoses()函数，传入里程计数据，得到lidar的位姿LoadTimeAndPoes(odom_path, init_Tl2i, lidar_files_, lidar_poses_);//利用滑动窗口选取关键帧数据，进行外参标定的校正std::vector<int> frm_start_box;//定义frm起始框std::vector<int> frm_step_box;//定义frm步进盒std::vector<int> 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<VOXEL_LOC, OCTO_TREE *> surf_map, corn_map;// build voxel_mapOCTO_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<LidarPointXYZIRT>::Ptr cloud(new pcl::PointCloud<LidarPointXYZIRT>);if (pcl::io::loadPCDFile(lidar_file_name, *cloud) < 0) {std::cout << "cannot open pcd_file: " << lidar_file_name << "\n";exit(1);//非正常运行导致退出程序}pcl::PointCloud<pcl::PointXYZI>::Ptr pl_corn(new pcl::PointCloud<pcl::PointXYZI>);pcl::PointCloud<pcl::PointXYZI>::Ptr pl_surf(new pcl::PointCloud<pcl::PointXYZI>);pcl::PointCloud<pcl::PointXYZI>::Ptr pl_surf_sharp(new pcl::PointCloud<pcl::PointXYZI>);// 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 sizefor (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, t2if (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 treefor (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<double>(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<pcl::PointXYZI>::Ptr all_cloud(new pcl::PointCloud<pcl::PointXYZI>());//实例化all_octree，并将体素分辨率设置为0.3pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr all_octree(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>(0.3));all_octree->setInputCloud(all_cloud);pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);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();
}

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

Lidar_imu自动标定源码阅读（六）——run部分
源码阅读,能力有限,如有某处理解错误,请指出,谢谢. run_lidar2imu.cpp:输入文件所在路径,开始标定. #include <Eigen/Core> #include < ...
mybatis源码阅读(二)：mybatis初始化上
转载自 mybatis源码阅读(二):mybatis初始化上 1.初始化入口 //Mybatis 通过SqlSessionFactory获取SqlSession, 然后才能通过SqlSession与 ...
LeGo-LOAM激光雷达定位算法源码阅读(二)
文章目录 1.featureAssociation框架 1.1节点代码主体 1.2 FeatureAssociation构造函数 1.3 runFeatureAssociation()主体函数 2.重 ...
nginx源码阅读(二).初始化:main函数及ngx_init_cycle函数
前言在分析源码时,我们可以先把握主干,然后其他部分再挨个分析就行了.接下来我们先看看nginx的main函数干了些什么. main函数这里先介绍一些下面会遇到的变量类型: ngx_int_t: t ...
DBFace: 源码阅读(二)
上篇链接看LZ上篇博客的时间竟然是7月18日,着实是懈怠了,其实有很多东西需要总结归纳,这周末就补一下之前欠的债吧上篇主要介绍了DBFace的大体框架,这篇主要介绍数据的预处理部分 5. 数据预处 ...
Mybatis源码阅读(二)
本文主要介绍Java中,不使用XML和使用XML构建SqlSessionFactory,通过SqlSessionFactory 中获取SqlSession的方法,使用SqlsessionManager ...
【SwinTransformer源码阅读二】Window Attention和Shifted Window Attention部分
先放一下SwinTransformer的整体结构,图片源于原论文,可以发现,在Transformer的Block中 W-MSA(Window based multi-head self attenti ...
datax源码阅读二：Engine流程
一.根据前面python文件知道,java的main函数是com.alibaba.datax.core.Engine public static void main(String[] args) th ...
Struts2源码阅读(二)_ActionContext及CleanUP Filter
1. ActionContext ActionContext是被存放在当前线程中的,获取ActionContext也是从ThreadLocal中获取的.所以在执行拦截器. action和result的 ...

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

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

最新文章

热门文章