pcl中的一些常用函数记录笔记

1. 删除无效点(nan点)——pcl::removeNaNFromPointCloud

NaNs表明测量传感器距离到该点的距离值是有问题的，可能是因为传感器太近或太远，或者因为表面反射。那么当存在无效点云的NaNs值作为算法的输入的时候，可能会引起很多问题。

注意一定要设置cloud_ptr->is_dense = false，否则不能使用pcl方法删除无效点。

• 删除nan点的函数形式：

pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

函数有三个参数：分别为输入点云，输出点云及对应保留的索引。

• 使用方法：

void removeNan(pcl::PointCloud<pcl::PointXYZ>::Ptr in, pcl::PointCloud<pcl::PointXYZ>::Ptr out)
{
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*in, *out, indices);
}

• 或者使用遍历的方法：

for (size_t i = 0; i < scan_ptr->points.size(); ++i)
{
    const auto &pt = scan_ptr->points[i];
    if (std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z))
    {
        continue;
    }
}

• 代码举例

#include 
#include 

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 10;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    //加入nan点
    cloud_ptr->points[9].x = std::nan("1");//或者使用std::numeric_limits::quiet_NaN()
    cloud_ptr->points[9].y = std::nan("1");
    cloud_ptr->points[9].z = std::nan("1");
    cloud_ptr->is_dense = false;//如果不加这个，就不认为有nan点
    std::cout << "原始点云：" << cloud_ptr->points.size()<<std::endl;
    for (const auto &point : *cloud_ptr)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
    /*******************************过滤nan点*************************************/
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_ptr, *output_ptr, indices);
    std::cout << "过滤后的点云：" <<output_ptr->points.size()<< std::endl;
    for (const auto &point : *output_ptr)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
}

2. 判断单个点是否是无效点——pcl::isFinite

#include 
 
int main(int argc, char **argv)
{
	pcl::PointXYZ p_valid;
	p_valid.x = 0;
	p_valid.y = 0;
	p_valid.z = 0;
	std::cout << "Is p_valid valid? " << pcl::isFinite(p_valid) << std::endl;//1,true
 
	pcl::PointXYZ p_invalid;
	p_invalid.x = std::numeric_limits<float>::quiet_NaN();
	p_invalid.y = 0;
	p_invalid.z = 0;
	std::cout << "Is p_invalid valid? " << pcl::isFinite(p_invalid) << std::endl;//0,false
}

3. 求点的极值——pcl::getMinMax3D

#include 
#include 

//方法一：带索引
void pcl::getMinMax3D 	( 	const pcl::PointCloud< PointT > &  	cloud,
		const std::vector< int > &  	indices,
		Eigen::Vector4f &  	min_pt,
		Eigen::Vector4f &  	max_pt	 
	) 	
//方法二：不带索引
void pcl::getMinMax3D 	( 	const pcl::PointCloud< PointT > &  	cloud,
		Eigen::Vector4f &  	min_pt,
		Eigen::Vector4f &  	max_pt	 
	) 	

cloud为输入点云，输出min_pt为所有点中最小的x值，y值，z值，输出max_pt为为所有点中最大的x值，y值，z值。

#include 
#include 

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 5;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    for (const auto &point : *cloud_ptr)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
    /*******************************求点的极值*************************************/
    Eigen::Vector4f min_pt, max_pt;
    std::vector<int> indices = {0, 1, 2, 3, 4};
    //pcl::getMinMax3D(*cloud_ptr, indices, min_pt, max_pt);
    pcl::getMinMax3D(*cloud_ptr, min_pt, max_pt);
    std::cout << "min_pt: " << min_pt << std::endl;
    std::cout << "max_pt: " << max_pt << std::endl;
}

4. 点云与点云ptr类型互相转换

#include 
#include 

//1. pcl::PointIndices-->pcl::PointIndices::Ptr的转化
pcl::PointIndices inliers;
pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices(inliers));

//2. pcl::PointIndices::Ptr-->pcl::PointIndices的转化
pcl::PointIndices inliers;
pcl::PointIndices::Ptr inliers_ptr;
inliers=*inliers_ptr;

//3. PointCloud::Ptr-->PointCloud的转化
PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT>);
PointCloud<PointT> cloud;
cloud=*cloud_ptr;

//4. PointCloud->PointCloud::Ptr-的转化
PointCloud<PointT>::Ptr cloud_ptr(new pcl::PointCloud<PointT>);
PointCloud<PointT> cloud;
cloud_ptr = cloud.makeShared();

5. 点云拷贝——pcl::copyPointCloud

//部分拷贝，索引拷贝
std::vector<int > indices = { 1, 2, 5 };
pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices,pcl::PointCloud &cloud_out)

//全部拷贝
pcl::copyPointCloud (const pcl::PointCloud &cloud_in,pcl::PointCloud &cloud_out)

6. 点云的插入与删除——insert、push_back和erase

#include 
#include 
#include 
int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 5;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        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::PointCloud<pcl::PointXYZ>::iterator index = cloud_ptr->begin();
    cloud_ptr->erase(index); //删除第1个
    index = cloud_ptr->begin() + 5;
    cloud_ptr->erase(index);//删除第5个

    /***************插入****************/
    pcl::PointXYZ point = {1, 1, 1};
    cloud_ptr->insert(cloud_ptr->begin() + 5, point); //在索引号为5的位置1上插入一点，原来的点后移一位
    cloud_ptr->push_back(point);                  //从点云最后面插入一点
    std::cout << cloud_ptr->points[5].x;              //输出1
}

7. 指定点云的颜色显示——PointCloudColorHandlerCustom

#include 
#include 
#include 

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 5;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        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::visualization::PCLVisualizer viewer("pointcloud viewer");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sig(cloud_ptr, 0, 234, 0);
    viewer.addPointCloud(cloud_ptr, sig, "cloud");
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
    return 1;
}

8. PointCloud和PCLPointCloud2类型互相转换——toPCLPointCloud2和fromPCLPointCloud2

#include 
#include 

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 5;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        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::PCLPointCloud2 cloud2;
    pcl::toPCLPointCloud2(*cloud_ptr, cloud2);//PointCloud==》PCLPointCloud2
    pcl::fromPCLPointCloud2(cloud2, *cloud_ptr);//PCLPointCloud2==》PointCloud
    return 1;
}

9. 提取已知索引之外的点云—— pcl::ExtractIndices

#include 
#include 
#include 

int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    /*******************************填充点云*************************************/
    cloud_ptr->width = 5;
    cloud_ptr->height = 1;
    cloud_ptr->points.resize(cloud_ptr->width * cloud_ptr->height);
    for (auto &point : *cloud_ptr) //填充点云
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
    std::cout << "原始点： " << std::endl;
    for (const auto &point : *cloud_ptr)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    inliers->indices = {1, 2};

    std::vector<int> indexes = {1, 2};
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud_ptr);
    extract.setIndices(inliers);
    /***************************内点*****************************/
    extract.filter(*cloud_filtered);
    std::cout << "内点： " << std::endl;
    for (const auto &point : *cloud_filtered)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
    /***************************外点*****************************/
    extract.setNegative(true); //false: 筛选Index对应的点，true：过滤获取Index之外的点
    extract.filter(*cloud_filtered);
    std::cout << "外点： " << std::endl;
    for (const auto &point : *cloud_filtered)
    {
        std::cout << "    " << point.x << " "
                  << point.y << " "
                  << point.z << std::endl;
    }
}

10. 计算质心——pcl::compute3DCentroid

     Eigen::Vector4f centroid;  //质心
     pcl::compute3DCentroid(*cloud_ptr, centroid); //估计质心的坐标

11. 计算两个向量之间的夹角——pcl::getAngle3D

• 函数原型

返回值为：[0, 180]

//in_degree默认为false，也就是默认输出弧度
pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree=false);
pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)

• 源码

pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
{
  // Compute the actual angle
  double rad = v1.normalized ().dot (v2.normalized ());
  if (rad < -1.0)
    rad = -1.0;
  else if (rad >  1.0)
    rad = 1.0;
  return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
}

• 代码举例

#include 
#include 

using namespace std;

int main()
{
	Eigen::Vector3f V1;
	V1 << 0, 0, 1;
	Eigen::Vector3f V2;
	V2 << 1, 0, 0;

	float angle;	//夹角
	angle = pcl::getAngle3D(V1, V2);
	cout << "->angle(弧度) = " << angle << endl;
	angle = pcl::getAngle3D(V1, V2, true);
	cout << "->angle(角度) = " << angle << "°" << endl;

	return 0;
}

---