PCL库学习
c_cpp_properties.json
{
"configurations": [
{
"name": "Linux",
"includePath": [
"${workspaceFolder}/**",
"/usr/include",
"/usr/local/include"
],
"defines": [],
"compilerPath": "/usr/bin/gcc",
"cStandard": "c11",
"cppStandard": "c++17",
"intelliSenseMode": "clang-x64"
// "compileCommands": "${workspaceFolder}/build/compile_commands.json"
}
],
"version": 4
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(pcl_demo)
find_package(PCL 1.14 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl_read pcl_read.cpp)
target_link_libraries (pcl_read ${PCL_LIBRARIES})
# install(TARGETS pcl_read RUNTIME DESTINATION bin)
点云数据类型
- pcl::PointXYZ:最简单也可能是最常用到的点类型;它只储存了3Dxyz的信息。
- pcl::PointXYZI:除上面XYZ内容外还包含了一个描述点亮度(intensity)的字段。当想要获取传感器返回的亮度高于一定级别的点时非常有用。还有与此相似的其他两种标准的点数据类型:一是pcl::nterestPoint,它有一个字段储存强度(strength);二是pcl::PointWithRange;它有一个字段储存距离(视点到采样点)。 pcl::PointXYZRGBA:这种点类型储存3D信息,也储存颜色(RGB=Red,Green, Blue)和透明度(A=Alpha)。
- pcl::PointXYZRGB:这种点类型与前面的点类型相似,但是它没有透明度字段。
- pcl::Normal:这是最常用的点类型,表示曲面上给定点处的法线以及测量的曲率。
- pcl::PointNormal:这种点类型跟前一个点类型一样;它包含了给定点所在曲面法线以及曲率信息,但是它也包含了点的3Dxyz坐标。这种点类型的变异类型是PointXYZRGBNormal和 PointXYZINormal,正如名字所提示,它们包含了颜色(前者)和亮度(后者)。
公共字段
- header:这个字段是pcl::PCLHeader类型,指定了点云的获取时间。
- points:这个字段是std::vector<PointT,…>类型,它是储存所有点的容器。vector定 义中的PointT对应于类的模板参数,即点的类型。
- width:这个字段指定了点云组织成一种图像时的宽度,否则它包含的是云中点的数量。
- height:这个字段指定了点云组织成一种图像时的高度,否则它总是1。
- is_dense:这个字段指定了点云中是否有无效值(无穷大或NaN值)。
- sensor_origin_:这个字段是Eigen::Vector4f类型,并且定义了传感器根据相对于原点 的平移所得到的位姿。
- sensor_orientation_:这个字段是Eigen::Quaternionf类型,并且定义了传感器旋转所 得到的位姿。
创建并保存点云
方式1
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
int main()
{
pcl::PointCloud<pcl::PointXYZ> cloud;
// Fill in the cloud data
cloud.width = 5;
cloud.height = 1;
cloud.is_dense = false;
cloud.resize(cloud.width * cloud.height);
for (auto &point : cloud)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
pcl::io::savePCDFileASCII("test_pcd.pcd", cloud);
std::cerr << "Saved " << cloud.size() << " data points to test_pcd.pcd." << std::endl;
for (const auto &point : cloud)
std::cerr << " " << point.x << " " << point.y << " " << point.z << std::endl;
return 0;
}
/*************
Saved 5 data points to test_pcd.pcd.
0.352222 -0.151883 -0.106395
-0.397406 -0.473106 0.292602
-0.731898 0.667105 0.441304
-0.734766 0.854581 -0.0361733
-0.4607 -0.277468 -0.916762
***************/
方式2
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
int main(int argc, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
uint8_t r(255), g(15), b(15);
for(float z(-1.0); z < 1.0; z += 0.05)
{
for(float angle(0.0); angle <= 360.0; angle += 5.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf(pcl::deg2rad(angle));
basic_point.y = sinf(pcl::deg2rad(angle));
basic_point.z = z;
basic_cloud_ptr->points.push_back(basic_point);
pcl::PointXYZRGB point;
point.x = basic_point.x;
point.y = basic_point.y;
point.z = basic_point.z;
uint32_t rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 |static_cast<uint32_t>(b));
point.rgb = *reinterpret_cast<float *> (&rgb);
point_cloud_ptr->points.push_back(point);
}
if(z<0.0)
{
r -= 12;
g += 12;
}
else
{
g -= 12;
b += 12;
}
}
basic_cloud_ptr->width = (int)basic_cloud_ptr->points.size();
basic_cloud_ptr->height = 1;
basic_cloud_ptr->is_dense = false;
pcl::io::savePCDFileASCII("test_pcd_basic_2.pcd", *basic_cloud_ptr);
for(const auto &point:basic_cloud_ptr->points)
std::cerr<<" "<<point.x<<" "<<point.y<<" "<<point.z<<std::endl;
point_cloud_ptr->width = (int)point_cloud_ptr->points.size();
point_cloud_ptr->height = 1;
point_cloud_ptr->is_dense = false;
pcl::io::savePCDFileASCII("test_pcd_point_2.pcd", *point_cloud_ptr);
for(const auto &point:point_cloud_ptr->points)
std::cerr<<" "<<point.x<<" "<<point.y<<" "<<point.z<<std::endl;
return 0;
}
test_pcd.pcd
# .PCD v.7 - Point Cloud Data file format
VERSION .7 # 版本号
FIELDS x y z # 指定一个点可以有的每一个维度和字段的名字
SIZE 4 4 4 # 用字节数指定每一个维度的大小。例如:
TYPE F F F # 用一个字符指定每一个维度的类型 int uint folat
COUNT 1 1 1 1 # 指定每一个维度包含的元素数目
WIDTH 5 # 像图像一样的有序结构,有5行和1列,
HEIGHT 1 # 这样该数据集中共有5*1=5个点
VIEWPOINT 0 0 0 1 0 0 0 # 指定数据集中点云的获取视点 视点信息被指定为平移(txtytz)+四元数(qwqxqyqz)
POINTS 5 # 指定点云中点的总数。从0.7版本开始,该字段就有点多余了
DATA ascii # 指定存储点云数据的数据类型。支持两种数据类型:ascii和二进制
0.35222197 -0.15188313 -0.10639524
-0.3974061 -0.47310591 0.29260206
-0.73189831 0.66710472 0.44130373
-0.73476553 0.85458088 -0.036173344
-0.46070004 -0.2774682 -0.91676188
读取点云
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
int main(int argc, char ** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ>("/home/ghigher/CPuls_Project/test01/build/test_pcd.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn`t read file test_pcd.pcd \n");
return -1;
}
std::cout<<"Load " << cloud->width * cloud->height << "data points from test_pcd.pcd width the following fields:" <<std::endl;
for(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;
}
/*********************************
Load 5data points from test_pcd.pcd width the following fields:
0.352222 -0.151883 -0.106395
-0.397406 -0.473106 0.292602
-0.731898 0.667105 0.441304
-0.734766 0.854581 -0.0361733
-0.4607 -0.277468 -0.916762
*********************************/
显示点云
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
int main()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile("/home/ghigher/CPuls_Project/test01/build/test_pcd_point_2.pcd", *cloud);
//创建CloudViewer点云显示对象
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud);
while(!viewer.wasStopped())
{
}
return 0;
}
KDTree
#include <pcl/point_cloud.h> //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
srand (time (NULL)); //用系统时间初始化随机种子
//创建一个PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// 随机点云生成
cloud->width = 1000; //此处点云数量
cloud->height = 1; //表示点云为无序点云
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i) //循环填充点云数据
{
cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); // // 产生数值为0-1024的浮点数
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
//创建KdTreeFLANN对象,并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
//设置搜索空间
kdtree.setInputCloud (cloud);
//设置查询点并赋随机值
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// K 临近搜索
//创建一个整数(设置为10)和两个向量来存储搜索到的K近邻,两个向量中,一个存储搜索到查询点近邻的索引,另一个存储对应近邻的距离平方
int K = 10;
std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引
std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方
//打印相关信息
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索
{
//打印所有近邻坐标
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// 半径 R内近邻搜索方法
std::vector<int> pointIdxRadiusSearch; //存储近邻索引
std::vector<float> pointRadiusSquaredDistance; //存储近邻对应距离的平方
float radius = 256.0f * rand () / (RAND_MAX + 1.0f); //随机的生成某一半径
//打印输出
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
// 假设我们的kdtree返回了大于0个近邻。那么它将打印出在我们"searchPoint"附近的10个最近的邻居并把它们存到先前创立的向量中。
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
return 0;
}
/**************************
K nearest neighbor search at (763.71 771.792 499.333) with K=10
743.497 707.763 466.9 (squared distance: 5560.2)
743.305 814.092 581.355 (squared distance: 8933.23)
831.035 753.543 578.683 (squared distance: 11162.2)
706.388 734.124 580.101 (squared distance: 11228.1)
811.653 670.148 524.748 (squared distance: 13276)
819.709 753.288 599.12 (squared distance: 13435.7)
852.38 697.685 530.509 (squared distance: 14326.1)
817.204 665.183 481.782 (squared distance: 14535)
694 796.036 396.447 (squared distance: 16032.8)
788.24 794.644 621.827 (squared distance: 16128.8)
Neighbors within radius search at (763.71 771.792 499.333) with radius=118.164
743.497 707.763 466.9 (squared distance: 5560.2)
743.305 814.092 581.355 (squared distance: 8933.23)
831.035 753.543 578.683 (squared distance: 11162.2)
706.388 734.124 580.101 (squared distance: 11228.1)
811.653 670.148 524.748 (squared distance: 13276)
819.709 753.288 599.12 (squared distance: 13435.7)
**************************/
ROS中使用
PCL接口
- std_msgs::Header:通常是每一个ROS消息的一部分。它包含消息发送时间、序列号和坐标系名称等信息。等价于pcl::Headertype。
- sensor_msgs::PointCloud2:用来转换pcl::PointCloud类型
- pcl_msgs::PointIndices:这个消息类型储存了一个点云中点的索引,等价PCL类型是pcl::PointIndices。
- pcl_msgs::PolygonMesh:这个消息类型保存了描绘网格、顶点和多边形的信息,等价PCL类型是pcl::PolygonMesh。
- pcl_msgs::Vertices:这个消息类型将一组顶点的索引保存在一个数组中,例如描述一个多边形。等价PCL类型是pcl::Vertices。
- pcl_msgs::ModelCoefficients:这个消息类型储存了一个模型的不同系数,例如描述一 个平面需要的四个参数。等价的PCL类型是pcl:ModelCoefficients。
void toROSMsg(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &);
void fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T> &);
void moveFromROSMsg(sensor_msgs::PointCloud2 &, pcl::PointCloud<T> &);
创建并发布
依赖
message_filters roscpp rosbag rospy sensor_msgs std_msgs pcl_ros pcl_conversions message_generation
#include <iostream>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "point_cloud_node");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("pointcloud_pub_topic", 1000);
ros::Rate rate(3);
unsigned int num_points = 4;
pcl::PointCloud<pcl::PointXYZRGB> cloud;
cloud.points.resize(num_points);
sensor_msgs::PointCloud2 output_msg;
while(ros::ok())
{
output_msg.header.stamp = ros::Time::now();
for(int i=0; i<num_points; i++)
{
cloud.points[i].x = i;
cloud.points[i].y = i;
cloud.points[i].z = i;
cloud.points[i].r = 0;
cloud.points[i].g = 255;
cloud.points[i].b = 0;
}
pcl::toROSMsg(cloud, output_msg);
output_msg.header.frame_id = "point_cloud_frame_id";
pub.publish(output_msg);
rate.sleep();
}
ros::spin();
return 0;
}
接收并进行位姿转换
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
class pcl_sub
{
private:
ros::NodeHandle nh;
ros::Subscriber subCloud;
ros::Publisher pubCloud;
sensor_msgs::PointCloud2 msg;
sensor_msgs::PointCloud2 adjust_msg;
pcl::PointCloud<pcl::PointXYZRGB> adjust_pcl;
public:
pcl_sub():nh("~")
{
subCloud = nh.subscribe<sensor_msgs::PointCloud2>("/point_cloud_topic", 1, &pcl_sub::getcloud, this);
pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/adjust_msgs", 1);
}
void getcloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr adjust_pcl_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
//将ROS消息转换为PCL点云
pcl::fromROSMsg(*laserCloudMsg, *adjust_pcl_ptr);
//定义一个旋转矩阵 虽然为3D 实质上是4x4矩阵(旋转+平移)
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
Eigen::AngleAxisd rotationVector(M_PI/4, Eigen::Vector3d(0, 0, 1));
Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity();
rotationMatrix = rotationVector.toRotationMatrix();
//旋转部分赋值
T.linear() = rotationMatrix;
//平移部分赋值
T.translation() = Eigen::Vector3d(0, 0, 0);
//执行变换 保存结果
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud (*adjust_pcl_ptr, *transform_cloud, T.matrix());
adjust_pcl = *transform_cloud;
for(int i = 0; i<adjust_pcl.points.size();i++)
{
adjust_pcl.points[i].r = 255;
adjust_pcl.points[i].g = 0;
adjust_pcl.points[i].b = 0;
}
//将PCL点云转换为ROS消息
pcl::toROSMsg(adjust_pcl, adjust_msg);
pubCloud.publish(adjust_msg);
}
~pcl_sub(){}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "point_cloud_transform");
pcl_sub ps;
ros::spin();
return 0;
}
参考视频:https://space.bilibili.com/222855903