PCL study note

pcl常用计算过程

在使用pcl时常用的一些算子用法

法向量计算

使用omp加速需要添加头文件

#include <pcl/features/normal_3d_omp.h>//使用omp需要添加的头文件

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bev(new pcl::PointCloud<pcl::PointXYZ>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  //创建一个快速k近邻查询,查询的时候若该点在点云中，则第一个近邻点是其本身
kdtree.setInputCloud(cloud_bev);
//pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率
//计算法向量
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst; //使用OMP加速
normEst.setNumberOfThreads(6);
normEst.setInputCloud(cloud_bev);
normEst.setSearchMethod(tree);
normEst.setKSearch(8);  //法向估计的点数
normEst.compute(*normals);
cout << "normal size is " << normals->size() << endl;
//计算法向量结束

显示

visualizer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(
            cloud_bev,normals,5,3,"normals"+id);

Z轴投影

添加头文件

#include <pcl/filters/project_inliers.h>

//进行点云投影测试
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
coefficients->values.resize(4);
coefficients->values[0] = 0;
coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bev(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE); // 三维平面参数
proj.setInputCloud(trans_pc);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_bev);
//点云投影结束

边界搜索

pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Boundary> boundary;
pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  //创建一个快速k近邻查询,查询的时候若该点在点云中，则第一个近邻点是其本身
kdtree.setInputCloud(cloud_bev);
//其中pcl::PointXYZ表示输入类型数据，
//pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率
//计算法向量
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst; //使用OMP加速
normEst.setNumberOfThreads(6);
normEst.setInputCloud(cloud_bev);
normEst.setSearchMethod(tree);
normEst.setKSearch(8);  //法向估计的点数
normEst.compute(*normals);
cout << "normal size is " << normals->size() << endl;
//计算法向量结束

//边界
est.setInputCloud(cloud_bev);
est.setInputNormals(normals);
est.setSearchMethod(tree);
est.setKSearch(20);//一般这里的数值越高，最终边界识别的精度越好
est.compute(boundary);

//存储估计为边界的点云数据，将边界结果保存为pcl::PointXYZ类型
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);
for (int i=0;i<boundary.size();i++)
{
    if (boundary[i].boundary_point>0)
    {
        cloud_boundary->push_back(cloud_bev->points[i]);
    }
}
pcl::PCDWriter writer;
writer.write("bounrady.pcd", *cloud_boundary);

立方体滤波

添加头文件

#include <pcl/filters/crop_box.h>

先使用pcl::MomentOfInertiaEstimation 对点云做PCA分析并计算点云在相机坐标系下的刚体变换。将得到的旋转矩阵，通过旋转矩阵转欧拉角（外旋，绕相机固定轴。先绕X，再Y，再Z，不是内旋），求解欧拉角。再通过cropBox滤波，控制立方体体积进行滤波。

pcl::PointCloud<pcl::PointXYZ>::Ptr trans_pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;

//通过PCA计算RT矩阵
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> estimBox;
estimBox.setInputCloud(cloud);
estimBox.compute();
estimBox.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//计算旋转矩阵对应欧拉角
Eigen::Vector3f euler_angle_temp = rotational_matrix_OBB.eulerAngles(2, 1, 0);
auto eulerAngle = Eigen::Vector3f(euler_angle_temp(2), euler_angle_temp(1), euler_angle_temp(0));

pcl::CropBox<pcl::PointXYZ> crop_box;
crop_box.setInputCloud(cloud);
crop_box.setMin(obb_info_.minPoints);
crop_box.setMax(obb_info_.maxPoints);
//下面两句是将立方体滤波盒通过刚体变换移动到输入点云附近，使滤波盒中心与输入点云中心重合，旋转一致。
crop_box.setTranslation(obb_info_.translasion);
//对应外旋欧拉角，绕固定轴X，再绕固定轴Y，再绕固定轴Z，根据左乘。为 rz*ry*rx
crop_box.setRotation(obb_info_.eulerAngle);
crop_box.setNegative(false);
crop_box.filter(*trans_pc);

pcl球面拟合

根据球面模型方程进行拟合

std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pclRansacSphere(const std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pc)
{
	std::vector<int> inliers;
    Eigen::VectorXf modelParas;
    //model_s是预先定义好的球面模型，再在拟合之后，球心坐标和半径也被保存在里面。同时error_sqr_dists_是选中点与球面距离的集合
	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
		model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(pc));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
	ransac.setDistanceThreshold(1);
	ransac.computeModel();
	ransac.getInliers(inliers);//返回查找到的内点
    ransac.getModelCoefficients(modelParas);
    std::cout << modelParas << "\n\n";
    //model_s->error_sqr_dists_;
    std::vector<double> re=model_s->error_sqr_dists_;
    double re_sum=0,rmse=0;
    for (double rei:re)
    {
        re_sum += pow(rei, 2);
    }
    rmse = sqrt(re_sum / re.size());
    std::cout << "rmse: " << rmse << std::endl;
	std::cout << "cloud_in: " << pc->size() << std::endl;
	std::cout << "inlier.size: " << inliers.size() << std::endl;
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
    //根据inliers索引，将输入的点云cloud中的点挑选到final存放
	pcl::copyPointCloud<pcl::PointXYZ>(*pc, inliers, *final);
	std::cerr << "final: " << final->size() << std::endl;
    return final;
}

点云刚体变换

void targetFiltting(const ProcessResult process_result)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr trans_pc(new pcl::PointCloud<pcl::PointXYZ>);
    //源点云process_result.cloud左乘刚体变换矩阵process_result.matrix，得到*trans_pc
    pcl::transformPointCloud(*process_result.cloud, *trans_pc, process_result.matrix);

}

vtk3D文本显示

设置完之后，需要添加 visualizer->resetCamera()，否则可能会出现显示失败

pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer);
    visualizer->setBackgroundColor(0, 0, 0);
visualizer->addText3D(algorithm->output()->Results[i].label,
			PointXYZ(algorithm->output()->Results[i].centroid.x()+3,
				algorithm->output()->Results[i].centroid.y()+3,
				algorithm->output()->Results[i].centroid.z() + 3), 8, r, g, b);
        visualizer->resetCamera();

点云类型与v::mat的相互转换

点云计算时，可能需要借助opecv库中的某些函数。直接转成mat可能比较好用

带法线的点云转mat

void PointCloudToMat(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const pcl::PointCloud<pcl::Normal>::Ptr& normal, cv::Mat& mat)
{
    size_t count = cloud->size();
    if (count == 0 || normal->size() != count)
        {
	    return;
	}

	mat.create(count, 6, CV_32FC1);
	for (int i = 0; i < count; i++)
	{
	    mat.at<float>(i, 0) = cloud->points[i].x;
	    mat.at<float>(i, 1) = cloud->points[i].y;
	    mat.at<float>(i, 2) = cloud->points[i].z;
	    mat.at<float>(i, 3) = normal->points[i].normal_x;
	    mat.at<float>(i, 4) = normal->points[i].normal_y;
	    mat.at<float>(i, 5) = normal->points[i].normal_z;
	}
}

mat转点云

pcl::PointCloud<pcl::PointXYZ>::Ptr MatToPointcloud(const cv::Mat& mat)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		if (mat.type() != CV_32FC1 || mat.cols < 3)
		{
			return cloud;
		}

		for (int i = 0; i < mat.rows; i++)
		{
			cloud->push_back(pcl::PointXYZ(mat.at<float>(i, 0), mat.at<float>(i, 1), mat.at<float>(i, 2)));
		}
		return cloud;
	}