Ubuntu18.04使用C++中的PCL点云库对不水平的点云进行校准

        我们在拿到用激光SLAM方法得到的全局地图时，往往由于安装传感器倾斜，地面不水平等等原因导致整体得到的点云也是倾斜的，这不方便我们对地图后续的工作如矢量地图标注及路径规划等，所以我们须将点云地图校准为水平状态。

        这里采用常见的点云处理库PCL（Point Cloud Library）,安装部分不再赘述，CSDN上有很多教程，博主采用的是Ubuntu18.04+PCL1.9+VScodeIDE的方式，其中VScode只能用CMAKE来进行编译。博主也是新人，大家相互交流。建议大家学完PCL的官方IO教程并学会打印出自己的点云文件数据后（此时你已经受过了配置环境的痛苦），再来看本篇博客

        校准思路：求得原始点云地图最下方的倾斜底面的平面方程，求其法向量，计算法向量与Z轴的角度，最后根据角度求得旋转矩阵随后变换即可。

        实践：建议哪里不懂直接问chatgpt就好，3.5就行，效率神器

        （1）PCL是一个模块化的库，拟合平面采用RANSAC(Random Sample Consensus）方法，包含文件如下：
 

#include <iostream>

#include <pcl/ModelCoefficients.h>

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/common/geometry.h>

#include <pcl/sample_consensus/method_types.h>

#include <pcl/sample_consensus/model_types.h>

#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/common/transforms.h>

#include <pcl/common/common.h> 

        （2）现在编写一个函数用来求拟合平面，主要的思路是对加载的点云数据进行循环遍历，筛选出满足特定条件的点，并将这些点存储到 in_cloud 中。在这个例子中，条件是点的 x 坐标小于 20 并且点的 y 坐标的绝对值小于 5：

void ransac_plane(const std::string& pcd_file_path, pcl::ModelCoefficients::Ptr coefficients, Eigen::Vector3f& plane_normal)

{

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

//如果读取失败，打印报错提示

if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file_path, *cloud) == -1)

{

PCL_ERROR("Couldn't read file %s\n", pcd_file_path.c_str());

return;

}

pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZ>);


for (int i = 0; i < cloud->size(); i++)

{

if (cloud->points[i].x < 20 && abs(cloud->points[i].y) < 5)

{

in_cloud->push_back(cloud->points[i]);

}

}

pcl::PointIndices::Ptr idx(new pcl::PointIndices);

pcl::SACSegmentation<pcl::PointXYZ> seg;

seg.setOptimizeCoefficients(true);

seg.setModelType(pcl::SACMODEL_PLANE);

seg.setMethodType(pcl::SAC_RANSAC);

seg.setDistanceThreshold(0.01);

seg.setInputCloud(in_cloud);

seg.segment(*idx, *coefficients);

//传入法向量

plane_normal << coefficients->values[0], coefficients->values[1], coefficients->values[2];

}

        （3）编写函数主入口：

int main(int argc, char **argv)

{

//首先将函数参数实例化，设定为自己的点云文件路径

std::string pcd_file_path = "你的文件路径（可采用终端中的pwd命令获取）";

//创建一个智能化指针，用来存放函数运行后产生的系数

pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

//主函数中仍须定义目标平面的法向量

Eigen::Vector3f plane_normal;

//调用函数

ransac_plane(pcd_file_path, coefficients,plane_normal);


if (coefficients->values.empty())

{

std::cerr << "Failed to estimate a planar model from the input point cloud." << std::endl;

return (-1);

}

// 计算平面法向量与Z轴之间的角度

double angle = atan2(plane_normal(0), plane_normal(2));

//以下有点小错误，因为我们只能绕Y轴和Z轴去转，所以应该通过立体几何的知识计算一下两次旋转的角度，懒得改了，大家可以去问chat
//创建一个变换矩阵

Eigen::Affine3f transform = Eigen::Affine3f::Identity();

transform.rotate(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()));


pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

pcl::io::loadPCDFile(pcd_file_path, *cloud);


// 对点云进行变换

pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>);

pcl::transformPointCloud(*cloud, *transform_cloud, transform);


// 将变换后的点云保存

std::string output_file_path = "你想保存的校准后的点云地址";

pcl::io::savePCDFileASCII(output_file_path, *transform_cloud);


std::cout << "变换后的点云已保存至：" << output_file_path << std::endl;


return (0);

}

        （4）最后编译运行就可以啦，附上CMakeLists代码

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)

project(MY_GRAND_PROJECT)

find_package(PCL 1.3 REQUIRED COMPONENTS common io)

include_directories(${PCL_INCLUDE_DIRS})

link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})

add_executable(你想生成的可执行文件 你的cpp文件)

target_link_libraries(你想生成的可执行文件 ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} pcl_segmentation -L/usr/lib/x86_64-linux-gnu/)

        最后，点赞收藏量较多的话我再出个可视化教程啥的，新人求支持！求SCUT校友支持！祝顺风顺水顺SCI！！！