pcl学习
1、点云读写
pcl官方教程真的比较详细，可惜是英文的，不过耐心看下去，也能看懂。
Reading Point Cloud data from PCD files — Point Cloud Library 0.0 documentation
读取pcd文件
#include
#include
#include
int main (int argc, char** argv)
{
pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
if (pcl::io::loadPCDFile ("test_pcd.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
return (-1);
}
//Loaded 个数 data points from 文件 with the following fields: x y z
std::cout << "Loaded "
<< cloud->width * cloud->height //点的个数
<< " data points from test_pcd.pcd with the following fields: "
<< std::endl;
for (std::size_t i = 0; i < cloud->points.size (); ++i)
std::cout << "' " << cloud->points[i].x //点云里点的值
<< " " << cloud->points[i].y
<< " " << cloud->points[i].z << std::endl;
return (0);
}
解释
//创建一个PointCloud
//从堆中分配了一个PointCloud
pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 加载点云；换乘自己的pcd文件路径
pcl::io::loadPCDFile ("test_pcd.pcd", *cloud) 宽*高
cloud->width * cloud->height 读取点的x坐标
cloud->points[i].xCmake
cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
project(pcd_read)
find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (pcd_read pcd_read.cpp)
target_link_libraries (pcd_read ${PCL_LIBRARIES})除了最后两行，上面的那几行都是调用pcl的库。使环境运行时能找到正确的地方。
使我们创建的pcd_read.cpp代码生成pcd_read可执行程序
add_executable (pcd_read pcd_read.cpp)生成的可执行程序pcd_read连接的pcl库
target_link_libraries (pcd_read ${PCL_LIBRARIES})运行
//在cpp当前文件夹下运行;
mkdir build
cd build
cmake ..
make
//运行可执行文件
./pcd_read结果
写PCD文件
#include
#include
#include
int main (int argc, char** argv)
{
pcl::PointCloud cloud;
// 写入点云数据
cloud.width = 5;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize (cloud.width * cloud.height);
for (std::size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);//储存在pcd文件中
std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;
for (std::size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
return (0);
}
解释
包含PCD/IO操作库和点云结构库
#include
#include 点云类型为 pcl::PointXYZ
pcl::PointCloud cloud; 前两个参数好理解，第三个参数表示是否所有数据在点云中都是好的(true)或者是否xyz的值可能存在INF/NAN值(false)
// 写入点云数据
cloud.width = 5;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize (cloud.width * cloud.height);
for (std::size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
将这些点保存在test_pcd.pcd文件中
pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);cmake
只需要在上面的cmake文件中加入这两句即可
add_executable (pcd_write pcd_write.cpp)
target_link_libraries (pcd_write ${PCL_LIBRARIES})运行方法同上
./pcd_write结果
2、创建自己的的点云类型
在LIO-SAM中用到的点云类型为PointXYZIRPYT；包括最基本的x,y,z和强度I；以及rol，pitch，yaw三个旋转角和时间T(time);
在PCL的库中没有这样的点云类型，但是PCL给我们提供了创建自己类型点云的方法。
完整代码
#include
#include
#include
struct MyPointType //定义点类型结构
{
PCL_ADD_POINT4D; //该点类型有4个元素
/*尝试新增一个自定义*/
float test; //定义自己新增的类型名称
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //确保new操作符对齐操作
}EIGEN_ALIGN16; //强制SSE 对齐
POINT_CLOUD_REGISTER_POINT_STRUCT(MyPointType, //注册点类型宏
(float ,x,x)
(float ,y,y)
(float ,z,z)
(float ,test,test)
)
int main(int argc, char const *argv[])
{
pcl::PointCloud cloud;
cloud.points.resize(2);
cloud.width=2;
cloud.height=1;
cloud.points[0].x=cloud.points[0].y=cloud.points[0].z=1;
cloud.points[1].x=cloud.points[1].y=cloud.points[1].z=3;
cloud.points[0].test = 1.2;
cloud.points[1].test = 3.4;
pcl::io::savePCDFile("test.pcd",cloud);
for (std::size_t i = 0; i < cloud.points.size (); ++i)
for (std::size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << " " < 解释
定义自己的点云类型其实也比较简单，有固定的套路；
第一步先创建自己类型的结构体
struct MyPointType //定义点类型结构
{
PCL_ADD_POINT4D; //该点类型有4个元素
/*尝试新增一个自定义*/
float test; //定义自己新增的类型名称
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //确保new操作符对齐操作
}EIGEN_ALIGN16; //强制SSE 对齐定义的点云类型有4个元素x,y,z+padding(坠语)似乎padding只是一个代称代表所有其他元素，你可以在x,y,z的基础上加一个，两个，三个都行。此处是只加了一个float类型的test；
PCL_ADD_POINT4D; //该点类型有4个元素其他强制对齐似乎都要加
第二步注册
POINT_CLOUD_REGISTER_POINT_STRUCT(MyPointType, //注册点类型宏
(float ,x,x)
(float ,y,y)
(float ,z,z)
(float ,test,test)
)就是固定的格式；x,y,z是基本的类型，test是我们自己加的；MyPointType是新点云类型的名字
使用
int main(int argc, char const *argv[])
{
//点云定义
pcl::PointCloud cloud;
//确定点云大小
cloud.points.resize(2);
cloud.width=2;
cloud.height=1;
//为点云中的点的各个元素赋值(x,y,z,test)
cloud.points[0].x=cloud.points[0].y=cloud.points[0].z=1;
cloud.points[1].x=cloud.points[1].y=cloud.points[1].z=3;
cloud.points[0].test = 1.2;
cloud.points[1].test = 3.4;
//写入test.pcd
pcl::io::savePCDFile("test.pcd",cloud);
//输出测试
for (std::size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << " " < cmake
cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
project(pcd_read)
find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# add_executable (pcd_read pcd_read.cpp)
# target_link_libraries (pcd_read ${PCL_LIBRARIES})
# add_executable (pcd_write pcd_write.cpp)
# target_link_libraries (pcd_write ${PCL_LIBRARIES})
add_executable (my_point my_point.cpp)
target_link_libraries (my_point ${PCL_LIBRARIES})运行
//在cpp当前文件夹下运行;
mkdir build
cd build
cmake ..
make
//运行可执行文件
./my_point结果
LIO-SAM中定义的点云类型
定义成立x,y,z三个位置信息；roll，pitch，yaw三个旋转信息；以及time和intensiy强度信息；
并且利用typedef可以使用PointTypePose对该类型点云进行定义
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
(double, time, time))
typedef PointXYZIRPYT PointTypePose;使用
PointTypePose thisPose6D;
thisPose6D.x = transformIn[3];
thisPose6D.y = transformIn[4];
thisPose6D.z = transformIn[5];
thisPose6D.roll = transformIn[0];
thisPose6D.pitch = transformIn[1];
thisPose6D.yaw = transformIn[2];3、KD-Tree最近邻搜索
首先要理解KD-tree最近邻搜索的算法原理可以参考这篇博客
K近邻法(KNN)原理小结 - 刘建平Pinard - 博客园
对于使用PCL库进行比较简单直接调用一个函数即可下面先看完整代码
下面利用PCL提供的函数完成一个任务：
给定一个点云test.pcd;找出该点云中某个点(比如说第三个点)的10个最近邻点；并输出他们的坐标以及距离
#include
#include //也有这个文件 但是会报错
#include
#include
#include
#include
#include
#include
int main(int argc, char**argv)
{
srand (time (NULL));//用系统时间初始化 随机种子
/*创建KdTreeFLANN 对象*/
pcl::KdTreeFLANN kdtree;
//创建点云对象
pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
//
if (pcl::io::loadPCDFile ("/home/du/work/study/pcl/test_pcd.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
return (-1);
}
/*把上面建立的点云 设置为 KDTREE的输入*/
kdtree.setInputCloud(cloud);
//创建一个查询点 同样分配随机坐标
pcl::PointXYZ searchPoint=cloud->points[2];
//创建两个向量 存储搜索到的k紧邻 一个存点的索引 一个存点的距离平方
int K = 10;
std::vector pointIdxNKNSearch(K);
std::vector pointNKNSquareDistance(K);
//终端输出 相关 信息
std::cout<<"寻找该点"<points[ pointIdxNKNSearch[i] ].x
<<" "<< cloud->points[pointIdxNKNSearch[i] ].y
<<" "<< cloud->points[pointIdxNKNSearch[i] ].z
<<" (平方距离为: "< 解释
计算某点最近邻主要分为三个步骤类似于把大象装冰箱
首先构造kdtree对象
/*创建KdTreeFLANN 对象*/
pcl::KdTreeFLANN kdtree; 第二确定搜索点和待搜索的点云集合装到kdtree对象
//创建点云对象
pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
//
if (pcl::io::loadPCDFile ("/home/du/work/study/pcl/test_pcd.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
return (-1);
}
//创建一个查询点
pcl::PointXYZ searchPoint=cloud->points[2];/*把上面建立的点云 设置为 KDTREE的输入*/
kdtree.setInputCloud(cloud);第三nearestKSearch函数开始搜索
//searchPoint要搜索的点；K要找几个最近邻；pointIdxNKNSearch近邻点的索引；pointNKNSquareDistance近邻点对应的距离
kdtree.nearestKSearch(searchPoint,K,pointIdxNKNSearch,pointNKNSquareDistance)>0结果
4、创建RGB点云+用八叉树压缩点云
在学习创建RGB点云时突然想到，似乎可以将点云投影到深度图后，利用语义分割的结果确定了点云属于那个类别，然后提取出范围坐标，将此范围内的点云RGB赋值为特定的颜色从而生成语义地图；--毕设相关与本文内容无关，突然有了想法，防止自己忘了记录一下；
一、创建RGB点云
#include
#include
#include
#include
#include
#include
#include
#include
#include
int main (int argc, char** argv)
{
pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud());;
uint8_t r(255), g(15), b(15);
for (float z(-1.0); z <= 1.0; z += 0.1)
{
for (float angle(0.0); angle <= 360.0; angle += 60.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
basic_point.y = sinf (float(angle/180*M_PI));
basic_point.z = z;
pcl::PointXYZRGB point;
point.x = basic_point.x;
point.y = basic_point.y;
point.z = basic_point.z;
uint32_t rgb = (static_cast(r) << 16 |
static_cast(g) << 8 | static_cast(b));
point.rgb = *reinterpret_cast(&rgb);
point_cloud_ptr->points.push_back (point);
}
if (z < 0.0)
{
r -= 12;
g += 12;
}
else
{
g -= 12;
b += 12;
}
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (point_cloud_ptr);
while (!viewer.wasStopped ())
{
}
return 0;
} 解释
创建基本的xyz点云；xy平面旋转成圆形创建，z方向不断升高
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
basic_point.y = sinf (float(angle/180*M_PI));
basic_point.z = z;在z<0时颜色都是相同的z颜色变化规律如下
if (z < 0.0)
{
r -= 12;
g += 12;
}z>0时变化规律为
else
{
g -= 12;
b += 12;
}保存显示点云
pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (point_cloud_ptr);
while (!viewer.wasStopped ())
{
}cmake
add_executable (creat_rgb creat_rgb.cpp)
target_link_libraries (creat_rgb ${PCL_LIBRARIES})结果
二、点云压缩
#include
#include
#include
#include
#include
#include
#include
#include
#include
class SimpleOpenNIViewer
{
public:
SimpleOpenNIViewer()
{
}
void run(const pcl::PointCloud::ConstPtr &cloud)
{
/* 设置压缩 和解压 时 的结果信息是否打印显示出来 */
bool showStatistics = true ;
//bool showStatistics = false;
/* 压缩选项 可以参考 usr/include/pcl/compression/compression_profiles.h */
pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;//设定为允许为高级参数化进行手工配置
/*声明八叉树的压缩 变量 指针*/
pcl::io::OctreePointCloudCompression * PointCloudEncoder;
/* 进行 八叉树 的 压缩 配置 */
PointCloudEncoder = new pcl::io::OctreePointCloudCompression(compressionProfile,showStatistics,0.001,0.01,true,100,true,8);//输入参数
//压缩的字节流
std::stringstream compressedData;
/* 实现压缩 的 函数 第一个参数要求时只能指针， .makeShared()即将cloud转成了 ptr */
PointCloudEncoder->encodePointCloud(cloud,compressedData);
//声明 解压 缩 后的 点云 指针
pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud());
/* 实现 解压缩 的 地方 第一个参数字节流 第二个参数 点云指针*/
PointCloudEncoder->decodePointCloud(compressedData,cloudOut);
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloudOut);
while (!viewer.wasStopped ())
{
}
}
};
int main (int argc, char** argv)
{
pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud());;
uint8_t r(255), g(15), b(15);
for (float z(-1.0); z <= 1.0; z += 0.02)
{
for (float angle(0.0); angle <= 360.0; angle += 60.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
basic_point.y = sinf (float(angle/180*M_PI));
basic_point.z = z;
pcl::PointXYZRGB point;
point.x = basic_point.x;
point.y = basic_point.y;
point.z = basic_point.z;
uint32_t rgb = (static_cast(r) << 16 |
static_cast(g) << 8 | static_cast(b));
point.rgb = *reinterpret_cast(&rgb);
point_cloud_ptr->points.push_back (point);
}
if (z < 0.0)
{
r -= 12;
g += 12;
}
else
{
g -= 12;
b += 12;
}
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;
pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);
//pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
// viewer.showCloud (point_cloud_ptr);
// while (!viewer.wasStopped ())
// {
// }
SimpleOpenNIViewer v;
v.run(point_cloud_ptr);
return 0;
} 创建一个点云压缩的class
通过实例化出对象，调用函数直接运行即可
5、体素滤波网格降采样
概述
通过体素网格实现降采样，可以减少点数量的同时，保证点云的形状特征，可以提高配准、曲面重建、形状识别等算法的速度，并保证准确性
原理
PCL实现的VoxelGrid类通过输入的点云数据创建一系列三维的体素栅格（可以想象为一个个微小的空间三维立方体的集合），然后在每一个体素中（就是每一个三维体素栅格）内，用这个体素栅格内所有点的重心来近似的显示体素中的其他点（用一个点代表栅格内的所有点）。
经过体素网格处理过的点云的密度将会显著减小，并且能够很好的保留原始的曲面特征,这也就是降采样（或称下采样的原因）。
代码
#include
#include
#include
#include
int main ()
{
//定义输入点云和滤波后的点云
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
// 创建点云读取对象
pcl::PCDReader reader;
// 读取点云到cloud
reader.read ("/home/du/work/study/pcl/table_scene_lms400.pcd", *cloud);
std::cerr << "点云滤波前大小: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
// 创建滤波对象
pcl::VoxelGrid sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
std::cerr << "滤波后点云大小: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
pcl::PCDWriter writer;
//将滤波后的点云写入pcd文件
writer.write ("/home/du/work/study/pcl/table_scene_lms400_downsampled.pcd", *cloud_filtered,
Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
//delete[] cloud;
return (0);
}
cmake
add_executable (voxel_grid voxel_grid.cpp)
target_link_libraries (voxel_grid ${PCL_LIBRARIES})运行
查看结果
pcl_viewer -multiview 1 table_scene_lms400.pcd table_scene_lms400_downsampled.pcd
LIO-SAM中的点云降采样滤波
//创建滤波对象 其中PointType为PointXYZI
pcl::VoxelGrid downSizeFilterCorner;
//将降采样点云输入到滤波对象
downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
//讲采样输出到laserCloudCornerStack
downSizeFilterCorner.filter(*laserCloudCornerStack);
//查看输出后的大小
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
6、点云配准
ICP理论部分可以参考我的另一篇博客
手写ICP-SVD_111111111112454545的博客-CSDN博客
用pcl库调用icp比较简单；
使用上一节降采样的点云
#include
#include
#include
#include
#include
int main ()
{
pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud);
pcl::PCDReader reader;
reader.read ("/home/du/work/study/pcl/table_scene_lms400_downsampled.pcd", *cloud_in);
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->size() << std::endl;
for (auto& point : *cloud_out)
point.x += 0.07f;
// for(auto& point : *cloud_out)
// {
// point.x=point.x*(-0.2)+0*point.y+0*point.z;
// point.y=point.x*0+0*point.y+0*point.z;
// point.z=point.x*0+0*point.y+1*point.z;
// }
std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
// for (auto& point : *cloud_out)
// std::cout << point << std::endl;
pcl::IterativeClosestPoint icp;
/*有4个参数可以设置 */
// 设置最大的对应关系距离为 多少 单位m， （对应距离大于此距离将被忽略）
icp.setMaxCorrespondenceDistance (0.8);
// 设置最大迭代次数(默认 1)
icp.setMaximumIterations (50);
// 设置 两次变换矩阵之间的差值 (默认 2)
icp.setTransformationEpsilon (1e-8);
// 设置 均方误差(默认 3)
icp.setEuclideanFitnessEpsilon (1);
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
} 利用pcl使用icp有以下几步
第一：确定原始点云与目标点云
pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud); 第二：构造icp对象
pcl::IterativeClosestPoint icp; 第三：配置icp参数(默认也可以)
/*有4个参数可以设置 */
// 设置最大的对应关系距离为 多少 单位m， （对应距离大于此距离将被忽略）
icp.setMaxCorrespondenceDistance (0.8);
// 设置最大迭代次数(默认 1)
icp.setMaximumIterations (50);
// 设置 两次变换矩阵之间的差值 (默认 2)
icp.setTransformationEpsilon (1e-8);
// 设置 均方误差(默认 3)
icp.setEuclideanFitnessEpsilon (1);
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);第四查看结果
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;结果
