【PCL】PCL 常用方法汇总（持续更新）

---

方法汇总

pcl::PointCloud::Ptr 和 pcl::PointCloud 相互转换

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointer(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    cloud = *cloudPointer;
    cloudPointer = cloud.makeShared(); // 深拷贝
    return 0;
}

查找点云 x，y，z 最值

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
    cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ> ("your_pcd_file.pcd", *cloud);
    pcl::PointXYZ minPt, maxPt;
    pcl::getMinMax3D (*cloud, minPt, maxPt);
    std::cout << "Max x: " << maxPt.x << std::endl;
    std::cout << "Min z: " << minPt.z << std::endl;
    return 0;
}

已知点索引，从原点云拷贝索引点到新点云

#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("your_pcd_file.pcd", *cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<int > indexs = {1, 2, 5};
    pcl::copyPointCloud(*cloud, indexs, *cloudOut);
    return 0;
}

删除、添加索引点

#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("your_pcd_file.pcd", *cloud);
    pcl::PointCloud<pcl::PointXYZ>::iterator index = cloud->begin();
    cloud->erase(index); // 删除第 1 个
    index = cloud->begin() + 5;
    cloud->erase(cloud->begin()); // 删除第 6 个
    pcl::PointXYZ point = {1, 1, 1};
    cloud->insert(cloud->begin() + 5, point); // 在索引 5 的位置上插入一点
    cloud->push_back(point); // 从点云最后面插入一点
    std::cout << cloud->points[5].x; // 输出 1
    return 0;
}

对点云进行全局、局部变换

#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("your_pcd_file.pcd",*cloud);
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    float theta = M_PI/4; // 旋转的度数 45°
    // 绕 Z 轴旋转
    transform_1 (0,0) = cos (theta);
    transform_1 (0,1) = -sin(theta);
    transform_1 (1,0) = sin (theta);
    transform_1 (1,1) = cos (theta);
    // 缩放
    transform_1 (0,0) = 0.3;
    transform_1 (1,1) = 0.6;
    transform_1 (2,2) = 1;
    // 平移
    transform_1 (0,3) = 25;
    transform_1 (1,3) = 30;
    transform_1 (2,3) = 35;
    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud, *transform_cloud1, transform_1);
    pcl::transformPointCloud(*cloud, pcl::PointIndices indices, *transform_cloud1, matrix); // 第二个参数为输入点云中部分点集索引
    return 0;
}

FLANN KdTree 查询 k 近邻

bool findNearTree(const pcl::PointCloud<pcl::PointXYZ> incloud, pcl::PointCloud<pcl::PointXYZ> searchcloud, multimap<float, vector<pcl::PointXYZ>> &inpt_outpt_map) {
	// FLANN 快速近似近邻算法库实现的 KdTree
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; 
	kdtree.setInputCloud(searchcloud.makeShared());
	int k = 1;
	// 保存邻近点索引 
	std::vector<int> pointIdxNKNSearch(k); 
	// 保存对象点与邻近点的距离平方值
	std::vector<float> pointNKNSquaredDistance(k);
	for (int j = 0; j < incloud.points.size(); j++) {
		vector<pcl::PointXYZ> temptvec;
		pcl::PointXYZ inpt;
		inpt.x = incloud[j].x;
		inpt.y = incloud[j].y;
		inpt.z = incloud[j].z;
		temptvec.push_back(inpt);
		if (kdtree.nearestKSearch(inpt, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { // > 0 表示找到，= 0 表示找不到
			for (std::size_t i = 0; i < pointIdxNKNSearch.size(); ++i) {
				pcl::PointXYZ tempt;
				tempt.x = searchcloud[pointIdxNKNSearch[i]].x;
				tempt.y = searchcloud[pointIdxNKNSearch[i]].y;
				tempt.z = searchcloud[pointIdxNKNSearch[i]].z;
				// 保存一一对应点对
				temptvec.push_back(tempt);
				// multimap<float, vector<pcl::PointXYZ>> inpt_outpt_map; 底层红黑树，对距离默认排序
				inpt_outpt_map.insert(make_pair(pointNKNSquaredDistance[i], temptvec));
			}
		} else {
			return false;
		}
	}
	return true;
}

计算点云索引，即 KdTree

例如 SIFT 算法中，PCL 无法直接提供索引（主要原因是 SIFT 点是通过计算出来的，在某些不同参数下，SIFT 点可能并非源数据中的点，而是某些点的近似），若要获取索引，则可利用以下函数：

void getIndices (pointcloud::Ptr cloudin, pointcloud keypoints, pcl::PointIndices::Ptr indices) {
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloudin);
    std::vector<float>pointNKNSquareDistance; //近邻点集的距离
    std::vector<int> pointIdxNKNSearch;
    for (size_t i =0; i < keypoints.size();i++) {
        kdtree.nearestKSearch(keypoints.points[i],1,pointIdxNKNSearch,pointNKNSquareDistance);
        // cout<<"the distance is:"<<pointNKNSquareDistance[0]<<endl;
        // cout<<"the indieces is:"<<pointIdxNKNSearch[0]<<endl;
        indices->indices.push_back(pointIdxNKNSearch[0]);
    }
	return ;
}

其思想就是：将原始数据插入到 FLANN 的 KdTree 中，寻找 keypoints 的最近邻,如果距离等于 0，则说明是同一点，提取索引即可。

计算质心

Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_smoothed, centroid); // 估计质心的坐标并保存

---