源码阅读，能力有限，如有某处理解错误，请指出，谢谢。
logging.hpp：
宏定义#define#ifndef#endif：
#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：
#ifndef TRANSFORM_UTIL_HPP_
#define TRANSFORM_UTIL_HPP_
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
//定义内联函数，进行角度和弧度的相互转换
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<p,q>(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<Eigen/Core>，包含Matrix和Array类，基础的线性代数运算和数组操作。
Geometry #include<Eigen/Geometry>，包含旋转，平移，缩放，2维和3维的各种变换。
LU #include<Eigen/LU>，包含求逆，行列式，LU分解。
Cholesky #include<Eigen/Cholesky>，包含LLT和LDLT Cholesky分解。
SVD `#include<Eigen/SVD>，包含SVD分解。
QR `#include<Eigen/QR>，包含QR分解。
Eigenvalues #include<Eigen/Eigenvalues>，包含特征值，特征向量分解。
Sparse #include<Eigen/Sparse>，包含稀疏矩阵的存储和运算。
Dense #include<Eigen/Dense>，包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块。
Eigen #include<Eigen/Eigen>，包含Dense和Sparse
#include <pcl/io/pcd_io.h> //PCD文件的读写
#include <pcl/point_types.h> //点类型定义
#include <pcl/point_cloud.h> //点云类定义
#include <pcl/kdtree/kdtree_flann.h> //KD-Tree搜索
pcl头文件介绍：
pcl 点云对象的两种定义方式的区别与转换:
#pragma once
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree_search.h>
#include <pcl/point_cloud.h>
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<pcl::PointXYZI>::Ptr &in_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr g_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr ng_cloud,
PlaneParam &plane);
bool
GroundPlaneExtraction(const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud,
PlaneParam &g_param);
void
PointCloudDownSampling(const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud,
double voxel_size,
pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud);
void
PointCloudFilterByROI(const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud,
const PointCloudBbox &roi,
pcl::PointCloud<pcl::PointXYZI>::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<pcl::PointXYZI>::Ptr all_cloud_;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr all_octree_;
//创建数组用于存放不同的点云数据
std::vector<pcl::PointCloud<pcl::PointXYZI>> pcds_;
std::vector<pcl::PointCloud<pcl::PointXYZI>> pcds_g_cloud_;
std::vector<pcl::PointCloud<pcl::PointXYZI>> pcds_ng_cloud_;
std::vector<std::string> timestamp_;
//创建数组4*4矩阵用于存放lidar位姿
std::vector<Eigen::Matrix4d> 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 <pcl/common/transforms.h> //点云坐标变化
#include <pcl/conversions.h> //数据类型的转换
#include <pcl/features/normal_3d.h> //法线特征估计
#include <pcl/filters/extract_indices.h> //索引提取
#include <pcl/kdtree/kdtree_flann.h> //kdtree头文件
#include <pcl/octree/octree_search.h> //octree头文件
#include <pcl/point_types.h> //点类型定义
#include <pcl/registration/gicp.h> //GICP配准
#include <pcl/registration/icp.h> //ICP配准
#include <pcl/registration/icp_nl.h> //非线性ICP配准
#include <pcl/registration/ndt.h> //NDT配准
#include <pcl/sample_consensus/method_types.h> //随机参数估计方法
#include <pcl/sample_consensus/model_types.h> //模型定义
#include <pcl/segmentation/sac_segmentation.h> //基于采样一致性分割
#include "logging.hpp"
#include "omp.h"
#include "transform_util.hpp"
Registrator::Registrator() {
all_cloud_.reset(new pcl::PointCloud<pcl::PointXYZI>());
all_octree_.reset(
new pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>(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<pcl::PointXYZI>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr filter_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr g_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr ng_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr g_filter_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr ng_filter_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr ng_filter_cloud_roi(
new pcl::PointCloud<pcl::PointXYZI>);
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<pcl::PointXYZI>::Ptr &in_cloud, double voxel_size,
pcl::PointCloud<pcl::PointXYZI>::Ptr &out_cloud) {
// 创建滤波器对象
pcl::VoxelGrid<pcl::PointXYZI> sor;
sor.setInputCloud(in_cloud);
sor.setLeafSize(voxel_size, voxel_size, voxel_size);设置三维栅格的大小，数值越大，栅格越大，采样后的点云数据越小
sor.filter(*out_cloud);
}
void Registrator::PointCloudFilterByROI(
const pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud,
const PointCloudBbox &roi,
pcl::PointCloud<pcl::PointXYZI>::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<pcl::PointXYZI>::Ptr &in_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr g_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr ng_cloud, PlaneParam &plane) {
//对系数和内点进行实例化
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//创建分割对象
pcl::SACSegmentation<pcl::PointXYZI> 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<pcl::PointXYZI> 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<pcl::PointXYZI>::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<pcl::PointXYZI> 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<pcl::PointXYZI>::Ptr transformed_cloud(
new pcl::PointCloud<pcl::PointXYZI>);
std::vector<Eigen::Vector3d> normals_list;
std::vector<double> 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<pcl::PointXYZ>::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;
}
652
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
