PCL 点云手记

Tue, 21 Apr 2026 17:29:00 +0800

点云即三维空间中点的集合，PCL 库专门处理点云数据，包括点云进行滤波，分割，匹配，查找等操作

数据结构

PCL 使用 pcl::PointCloud<T> 来存储点云，其中 T 是点的类型，下面是一些 PCL 定义的常用点类型：

pcl::PointXYZ : 仅包括三维空间坐标信息
pcl::PointXYZI : 包括三维坐标和强度值，常用于激光雷达
pcl::PointXYZL : 包括三维坐标和标签，常用于点云分割或分类
pcl::PointXY : 二维点结构，用于处理投影或二维平面数据
pcl::PointXYZRGB : 包含三维坐标和 RGB 颜色信息
pcl::PointXYZRGBA : 包含三维坐标和 RGBA 颜色信息

还有更多的点类型，用于不同的任务会，后续会逐渐接触

pcl::PointCloud<T> 中包括以下主要的数据成员：

width : 点云数据的宽度，对于无组织的点，宽度就是点的个数，对于有组织的点，宽度就是一行点个数
height : 点云数据的高度，对于无组织的数据，高度为 1，对于有组织的点，高度就是行数，总的点数量为 width * height
points : 点的数据
is_dense : 点是否包含 Inf/NaN 值

pcl::PointCloud<T> 的增删遍历点的成员函数和标准库中的集合类似，也支持迭代器

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

template <typename T>
pcl::PointCloud<T>::Ptr genPointCloud(int n)
{
 srand(time(NULL));
 typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>());
 cloud->resize(n);

 for (int i = 0; i < n; i++) {
 auto& p = (*cloud)[i];
 p.x = 1024 * rand () / (RAND_MAX + 1.0f);
 p.y = 1024 * rand () / (RAND_MAX + 1.0f);
 p.z = 1024 * rand () / (RAND_MAX + 1.0f);
 }
 return cloud;
}

#include <iostream>
#include <format>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

int main(int argc, char* argv[]) {
 auto cloud = genPointCloud<pcl::PointXYZ>(5);
 std::cout << std::format("width: {}, height: {}, is_dense: {}, is organized: {}\n", cloud->width, cloud->height, cloud->is_dense, cloud->isOrganized());

 for (const auto& point : *cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }

 return 0;
}

width: 5, height: 1, is_dense: true, is organized: false
(-0.042350292, 0.7831731, 0.9874444)
(-0.11391306, 0.35958624, 0.083025455)
(-0.57813406, -0.5630765, -0.2032013)
(-0.30399895, -0.48886824, -0.71495104)
(0.69121265, -0.97114754, -0.49422264)

IO

文件

PCL 保存文件的格式为 .pcd ，下面是读写 PCD 文件的例子


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

int main([[maybe_unused]] int argc, [[maybe_unused]] char* argv[]) {
 pcl::PointCloud<pcl::PointXYZ> cloud = *genPointCloud<pcl::PointXYZ>(5);

 pcl::io::savePCDFileASCII("/tmp/test_pcd.pcd", cloud);

 std::cout << "Saved " << cloud.size() << " data points to test_pcd.pcd." << std::endl;

 for (const auto& point : cloud) {
 std::cout << point.x << " " << point.y << " " << point.z << std::endl;
 }

 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile("/tmp/test_pcd.pcd", *cloud_ptr) == -1) {
 PCL_ERROR("Couldn't read file\n");
 return -1;
 }

 std::cout << "Loaded " << cloud_ptr->width * cloud_ptr->height << " data points" << std::endl;
 for (const auto& point : *cloud_ptr) {
 std::cout << point.x << " " << point.y << " " << point.z << std::endl;
 }
 return 0;
}

Saved 5 data points to test_pcd.pcd.
0.902143 0.32537 -0.246792
-0.427928 -0.0364971 -0.7693
0.0920715 -0.533491 0.701829
-0.292022 0.932315 0.600912
0.245783 0.191243 -0.526157
Loaded 5 data points
0.902143 0.32537 -0.246792
-0.427928 -0.0364971 -0.7693
0.0920715 -0.533491 0.701829
-0.292022 0.932315 0.600912
0.245783 0.191243 -0.526157

pcd 文件内容如下：

VERSION 0.7 # 版本 FIELDS x y z # data 中点的字段格式 SIZE 4 4 4 # 每个字段的大小 TYPE F F F # 每个字段的类型 COUNT 1 1 1 # 每个字段元素数量 WIDTH 5 # 宽度 HEIGHT 1 # 高度 VIEWPOINT 0 0 0 1 0 0 0 # 平移变换和旋转变换 POINTS 5 # 点的数量 DATA ascii # 数据格式，ascii 表示文本格式，每行一个点， binary 为二进制格式，binary_compressed 为压缩的二进制格式 0.902143 0.32537 -0.246792 -0.427928 -0.0364971 -0.7693 0.0920715 -0.533491 0.701829 -0.292022 0.932315 0.600912 0.245783 0.191243 -0.526157

可视化

可以使用 pcl_viewer 工具查看 pcd 文件

或者在代码中调用 pcl_viewer 可视化点云

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

struct Color { uint8_t r, g, b; };

// 传入 n，返回 n 个视觉上区分度大的颜色
std::vector<Color> generateColors(int n)
{
 std::vector<Color> colors;
 colors.reserve(n);

 for (int i = 0; i < n; ++i)
 {
 // 黄金角度 137.508°，每迭代一次旋转一个角度
 // 人眼对色相变化最敏感，所以用 HSV 空间的 H 均匀分布
 double hue = fmod(i * 137.508, 360.0); // 0~360
 double s = 0.85; // 饱和度
 double v = 0.95; // 明度

 // HSV → RGB
 double c = v * s;
 double x = c * (1.0 - fabs(fmod(hue / 60.0, 2.0) - 1.0));
 double m = v - c;

 double r = 0, g = 0, b = 0;
 if (hue < 60) { r = c; g = x; }
 else if (hue < 120) { r = x; g = c; }
 else if (hue < 180) { g = c; b = x; }
 else if (hue < 240) { g = x; b = c; }
 else if (hue < 300) { r = x; b = c; }
 else { r = c; b = x; }

 colors.push_back({
 static_cast<uint8_t>((r + m) * 255),
 static_cast<uint8_t>((g + m) * 255),
 static_cast<uint8_t>((b + m) * 255)
 });
 }
 return colors;
}


void pclViewer(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 0, 255, 0);
 viewer.addPointCloud<pcl::PointXYZ>(cloud, color, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}

void pclViewer(pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> color(cloud, 0, 255, 0);
 viewer.addPointCloud<pcl::PointXYZI>(cloud, color, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}

void pclViewer(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
 pcl::visualization::PCLVisualizer viewer;
 viewer.setBackgroundColor(0.05, 0.05, 0.05);

 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
 viewer.addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "cloud");

 while (!viewer.wasStopped()) {
 viewer.spinOnce(100);
 }

}


template <typename PointT>
void pclViewCluster(const std::shared_ptr<pcl::PointCloud<PointT>>& cloud, const std::vector<pcl::PointIndices>& clusters)
{
 auto cloud_colored = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
 cloud_colored->reserve(cloud->size());
 auto colors = generateColors(clusters.size());
 for (size_t i = 0; i < clusters.size(); i++) {
 auto& cluster = clusters[i];
 auto color = colors[i];
 for (const auto& idx : cluster.indices) {
 auto& p = (*cloud)[idx];
 cloud_colored->emplace_back(p.x, p.y, p.z, color.r, color.g, color.b);
 }
 }
 pclViewer(cloud_colored);
}

查找

点云处理中查找主要包括两方面：

指定一个点，查找它最近的 k 个邻居
指定一个点，查找在指定半径范围内的邻居

KdTree

KdTree 是一个数据结构，能够高效的查找 K 近邻或半径邻居

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <format>
#include <vector>
#include <ctime>

int main() {
 auto cloud = genPointCloud(1000);

 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

 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 近邻搜索
 int K = 5;
 std::vector<int> pointIdxKNNSearch(K);
 std::vector<float> pointKNNSquaredDistance(K);

 std::cout << std::format("K nearest neighbor search at ({}, {}, {}) with K: {}\n",
 searchPoint.x, searchPoint.y, searchPoint.z, K);

 if (kdtree.nearestKSearch(searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0) {
 for (size_t i = 0; i < pointIdxKNNSearch.size(); i++) {
 auto& p = (*cloud)[pointIdxKNNSearch[i]];
 std::cout << std::format("find {}: point: ({}, {}, {}), squared distance: {}", i, p.x,
 p.y, p.z, pointKNNSquaredDistance[i])
 << std::endl;
 }
 }

 // 半径搜索
 std::vector<int> pointIdxRadiusSearch;
 std::vector<float> pointRadiusSquaredDistance;

 float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

 std::cout << std::format("Neighbors within radius search at ({}, {}, {}) with radius: {}",
 searchPoint.x, searchPoint.y, searchPoint.z, radius)
 << std::endl;

 if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >
 0) {
 for (size_t i = 0; i < pointIdxRadiusSearch.size(); i++) {
 auto& p = (*cloud)[pointIdxRadiusSearch[i]];
 std::cout << std::format("find {}: point: ({}, {}, {}), squared distance: {}", i, p.x,
 p.y, p.z, pointRadiusSquaredDistance[i])
 << std::endl;
 }
 }

 return 0;
}

K nearest neighbor search at (843.17914, 724.95197, 942.59766) with K: 5
find 0: point: (838.75775, 707.2718, 923.97406), squared distance: 678.9756
find 1: point: (860.5885, 722.6938, 968.37213), squared distance: 972.5088
find 2: point: (867.26166, 656.1342, 939.431), squared distance: 5325.878
find 3: point: (891.3544, 763.66846, 989.54846), squared distance: 6024.198
find 4: point: (896.51337, 697.6261, 893.1407), squared distance: 6037.2354
Neighbors within radius search at (843.17914, 724.95197, 942.59766) with radius: 98.56139
find 0: point: (838.75775, 707.2718, 923.97406), squared distance: 678.9756
find 1: point: (860.5885, 722.6938, 968.37213), squared distance: 972.5088
find 2: point: (867.26166, 656.1342, 939.431), squared distance: 5325.878
find 3: point: (891.3544, 763.66846, 989.54846), squared distance: 6024.198
find 4: point: (896.51337, 697.6261, 893.1407), squared distance: 6037.2354
find 5: point: (908.95496, 783.8818, 909.6123), squared distance: 8887.214

特征

特征是通过周围一圈领域点算出来，描述局部表面形状，集合结构等

积分图法向量估计

积分图即预处理后的累加和图，任意矩形区域的和都可以用 4 次查表算出

#include <iostream>
#include <format>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <filesystem>

using PointT = pcl::PointXYZ;

int main([[maybe_unused]] int argc, [[maybe_unused]] char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd_file =
 std::filesystem::path(home / "Documents/Dataset/table_scene_mug_stereo_textured.pcd");

 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 pcl::PCDReader reader;
 reader.read(pcd_file.c_str(), *cloud);
 std::cout << std::format("PointCloud has {} data points.\n", cloud->size());

 // 估计法向量
 pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

 pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
 ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
 ne.setMaxDepthChangeFactor(0.02f);
 ne.setNormalSmoothingSize(10.0f);
 ne.setInputCloud(cloud);
 ne.compute(*normals);

 pcl::visualization::PCLVisualizer viewer(" PCL Viewer");
 viewer.setBackgroundColor(0.0, 0.0, 0.5);
 viewer.addPointCloudNormals<PointT, pcl::Normal>(cloud, normals);

 while (!viewer.wasStopped()) {
 viewer.spinOnce();
 }

 return 0;
}

`1`	`PointCloud has 307200 data points.`

滤波

PCL 中的滤波指保留点云中满足指定条件的点

直通滤波(PassThrough filter)

直通滤波的条件是指定维度值的范围，比如保留 z 轴在 [0, 1] 区间内的点

#include <format>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int main() {
 auto cloud = genPointCloud(5);
 auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

 cloud->emplace_back(0.0, 0.0, 0.0);
 cloud->emplace_back(0.0, 0.0, 1.0);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }

 pcl::PassThrough<pcl::PointXYZ> pass; // 创建直通滤波
 pass.setInputCloud(cloud); // 设置输入点云
 pass.setFilterFieldName("z"); // 设置需要过滤的维度
 pass.setFilterLimits(0.0, 1.0); // 设置范围
 // pass.setNegative(true); // 反向过滤
 pass.filter(*filtered_cloud); // 开始过滤并设置到输出点云

 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *filtered_cloud) {
 std::cout << std::format("({}, {}, {})\n", point.x, point.y, point.z);
 }
 return 0;
}

Cloud before filtering:
(0.35222197, -0.15188313, -0.106395245)
(-0.3974061, -0.4731059, 0.29260206)
(-0.7318983, 0.6671047, 0.44130373)
(-0.7347655, 0.8545809, -0.036173344)
(-0.46070004, -0.2774682, -0.9167619)
(0, 0, 0)
(0, 0, 1)
Cloud after filtering:
(-0.3974061, -0.4731059, 0.29260206)
(-0.7318983, 0.6671047, 0.44130373)
(0, 0, 0)
(0, 0, 1)

体素滤波(VoxelGrid filter)

体素就是三维空间中的一个立方体，体素滤波就是用一个个立方体去装填整个点云，然后每个立方体里只保留一个点，从而精简点云数量。

对于每个包含点的立方体，PCL 会计算其中所有点的重心，并用这个重心来代表这个立方体里原本所有的点，这种下采样算法能够保留点云几何形状和结构。

#include <iostream>
#include <filesystem>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

int main() {
 auto cloud = std::make_shared<pcl::PCLPointCloud2>();
 auto filtered_cloud = std::make_shared<pcl::PCLPointCloud2>();

 std::filesystem::path home(std::getenv("HOME"));
 // Fill in the cloud data
 pcl::PCDReader reader;
 reader.read(std::filesystem::path( home / "Documents/Dataset/table_scene_lms400.pcd").c_str(),
 *cloud);

 std::cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points ("
 << pcl::getFieldsList(*cloud) << ")." << std::endl;

 // Create the filtering object
 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
 sor.setInputCloud(cloud); // 设置输入点云
 sor.setLeafSize(0.01f, 0.01f, 0.01f); // 设置立方体大小 0.1 * 0.1 * 0.1
 sor.filter(*filtered_cloud);

 std::cout << "PointCloud after filtering: " << filtered_cloud->width * filtered_cloud->height
 << " data points (" << pcl::getFieldsList(*filtered_cloud) << ")." << std::endl;

 pcl::PCDWriter writer;
 writer.write(std::filesystem::path(home / "Documents/Dataset/table_scene_lms400_downsampled.pcd"),
 *filtered_cloud, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);

 return (0);
}

1
2

PointCloud before filtering: 460400 data points (x y z intensity distance sid).
PointCloud after filtering: 41049 data points (x y z intensity distance sid).

统计离群点移除滤波

激光扫描生成的点云数据集通常具有变化的点密度。此外，测量误差会引入稀疏的离群点，进一步破坏数据质量。

统计离群点滤波算法：对于每个点，计算它到所有领域点的平均距离，如果平均距离落在全局距离均值和标准差所定义的区间外，则视为离群点

#include <iostream>
#include <filesystem>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

 std::filesystem::path home(std::getenv("HOME"));
 std::filesystem::path dataset = home / "Documents/Dataset";
 // Fill in the cloud data
 pcl::PCDReader reader;
 reader.read(std::filesystem::path(dataset / "table_scene_lms400.pcd").c_str(),*cloud);

 std::cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points ("
 << pcl::getFieldsList(*cloud) << ")." << std::endl;

 pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
 sor.setInputCloud(cloud);
 sor.setMeanK(50); // 领域中点的数量
 sor.setStddevMulThresh(1.0); // 标准差的阈值，大于 1.0 的点被认为是离群点
 sor.filter(*cloud_filtered);

 pcl::PCDWriter writer;
 writer.write<pcl::PointXYZ>(std::filesystem::path(dataset / "table_scene_lms400_inliers.pcd").c_str(), *cloud_filtered, false);

 sor.setNegative(true);
 sor.filter(*cloud_filtered);
 writer.write<pcl::PointXYZ>(std::filesystem::path(dataset / "table_scene_lms400_outliers.pcd").c_str(), *cloud_filtered,false);

 return 0;
}

`1`	`PointCloud before filtering: 460400 data points (x y z).`

条件滤波

条件滤波即按照给定条件过滤点云中的点，只是给定的这个条件是用 PCL 中的结构

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 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);
 }

 // 构建 and 条件
 pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());

 // 添加一个比较条件，z > 0
 range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
 new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));
 // 添加一个比较条件，z < 0.8
 range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(
 new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));

 // 构建滤波
 pcl::ConditionalRemoval<pcl::PointXYZ> cond_rem;
 cond_rem.setCondition(range_cond);
 cond_rem.setInputCloud(cloud);
 cond_rem.setKeepOrganized(true); // 保持点云的数据格式，即将不符合条件的点设置为 NAN ，点云总数不变
 cond_rem.filter(*cloud_filtered);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 // display pointcloud after filtering
 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 return 0;
}

Cloud before filtering:
 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
Cloud after filtering:
 nan nan nan
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 nan nan nan
 nan nan nan

半径离群点移除滤波

指定半径和邻居数量，对于一个点，如果指定半径内其它点数量小于指定的邻居数量，则视为离群点

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 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::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
 outrem.setInputCloud(cloud);
 outrem.setRadiusSearch(1.0); // 设置半径
 outrem.setMinNeighborsInRadius(1); // 设置邻居数量
 outrem.setKeepOrganized(true);
 outrem.filter(*cloud_filtered);

 std::cout << "Cloud before filtering: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 // display pointcloud after filtering
 std::cout << "Cloud after filtering: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 return 0;
}

Cloud before filtering:
 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
Cloud after filtering:
 0.352222 -0.151883 -0.106395
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -0.0361733
 nan nan nan

投影滤波

将点投影到指定的参数模型上，比如将点投影到平面上，对应的参数模型为： aX + bY + cZ +d = 0

支持的投影模型及系数：

宏定义	模型	系数个数	系数含义简述
SACMODEL_PLANE	平面	4	[a,b,c,d] 平面方程 ax+by+cz+d=0
SACMODEL_LINE	3D 直线	6	基准点 + 方向向量
SACMODEL_CIRCLE2D	2D 平面圆	3	圆心(cx,cy) + 半径 r
SACMODEL_CIRCLE3D	3D 空间圆	7	圆心+平面法向+半径
SACMODEL_SPHERE	球体	4	球心(cx,cy,cz) + 半径 r
SACMODEL_CYLINDER	圆柱体	7	轴线基准点+轴线方向+底面半径
SACMODEL_CONE	圆锥体	7	顶点+轴线方向+锥角

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->points.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);
 }

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 // 设置投影系数
 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
 coefficients->values.resize(4);
 coefficients->values[0] = coefficients->values[1] = 0; // x = y = 0;
 coefficients->values[2] = 1.0; // z = 1.0;
 coefficients->values[3] = 0;

 pcl::ProjectInliers<pcl::PointXYZ> proj;
 proj.setModelType(pcl::SACMODEL_PLANE);
 proj.setInputCloud(cloud);
 proj.setModelCoefficients(coefficients);
 proj.filter(*cloud_projected);

 std::cout << "Cloud after projection: " << std::endl;
 for (const auto& point : *cloud_projected)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 return 0;
}

Cloud before projection:
 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
Cloud after projection:
 0.352222 -0.151883 0
 -0.397406 -0.473106 0
 -0.731898 0.667105 0
 -0.734766 0.854581 0
 -0.4607 -0.277468 0

索引提取滤波

即指定点的索引，将点从点云中提取出来

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 5;
 cloud->height = 1;
 cloud->points.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);
 }

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 // 指定点的索引，这里指定 1， 3， 实际上一般通过模板匹配或其它方式获得
 pcl::PointIndices::Ptr indices(new pcl::PointIndices());
 indices->indices.push_back(1);
 indices->indices.push_back(3);

 pcl::ExtractIndices<pcl::PointXYZ> extract;
 extract.setInputCloud(cloud);
 extract.setIndices(indices);
 extract.setNegative(false); // false 表示保留索引点，true 表示去掉索引点
 extract.setKeepOrganized(true);
 extract.filter(*cloud_filtered);

 std::cout << "Cloud after projection: " << std::endl;
 for (const auto& point : *cloud_filtered)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
 return 0;
}

Cloud before projection:
 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
Cloud after projection:
 nan nan nan
 -0.397406 -0.473106 0.292602
 nan nan nan
 -0.734766 0.854581 -0.0361733
 nan nan nan

分割

点云分割即找出点云中的指定模型

平面模型分割

平面模型分割即在点云中找出一个平面

常用的是 SAC_RANSAC 算法：

随机猜 3 个点算平面
数有多少点贴合这个平面
重复 N 次，留下贴合点最多的那个平面

实际工程中可以用来剔除地面，剔除墙面，分离背景和障碍物等

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main() {
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

 // Fill in the cloud data
 cloud->width = 15;
 cloud->height = 1;
 cloud->points.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);
 }

 // 设置离群点
 (*cloud)[0].z = 2.0;
 (*cloud)[3].z = -2.0;
 (*cloud)[6].z = 4.0;

 std::cout << "Cloud before projection: " << std::endl;
 for (const auto& point : *cloud)
 std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;

 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

 pcl::SACSegmentation<pcl::PointXYZ> seg;
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_PLANE);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setDistanceThreshold(0.01); // 距离系数，点到平面距离小于 0.01 为内点

 seg.setInputCloud(cloud);
 seg.segment(*inliers, *coefficients);

 if (inliers->indices.size() == 0) {
 std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
 return -1;
 }

 std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1]
 << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;

 std::cout << "Model inliers: " << inliers->indices.size() << std::endl;
 for (const auto& idx : inliers->indices)
 std::cout << idx << " " << cloud->points[idx].x << " " << cloud->points[idx].y << " "
 << cloud->points[idx].z << std::endl;
 return 0;
}

Cloud before projection:
 0.352222 -0.151883 2
 -0.397406 -0.473106 0.292602
 -0.731898 0.667105 0.441304
 -0.734766 0.854581 -2
 -0.4607 -0.277468 -0.916762
 0.183749 0.968809 0.512055
 -0.998983 -0.463871 4
 0.716053 0.525135 -0.523004
 0.439387 0.56706 0.905417
 -0.579787 0.898706 -0.504929
 -0.757228 -0.749072 -0.656812
 -0.863624 0.853522 0.870082
 -0.571022 0.121624 -0.462813
 -0.129718 -0.613142 0.391768
 -0.165891 0.926158 0.1143
Model coefficients: 0.910654 -0.197082 -0.363135 0.375378
Model inliers: 4
0 0.352222 -0.151883 2
1 -0.397406 -0.473106 0.292602
12 -0.571022 0.121624 -0.462813
14 -0.165891 0.926158 0.1143

圆柱模型分割

即在点云中找出一个圆柱体

#include <iostream>
#include <format>
#include <filesystem>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

using PointT = pcl::PointXYZ;

int main() {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd_file =
 std::filesystem::path(home / "Documents/Dataset/table_scene_mug_stereo_textured.pcd");

 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 pcl::PCDReader reader;
 reader.read(pcd_file.c_str(), *cloud);
 std::cout << std::format("PointCloud has {} data points.\n", cloud->size());

 pclViewer(cloud);

 // 移除 NaN 和场景外的点
 pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
 pcl::PassThrough<PointT> pass;
 pass.setInputCloud(cloud);
 pass.setFilterFieldName("z");
 pass.setFilterLimits(0, 1.5);
 pass.filter(*cloud_filtered);
 std::cout << std::format("PointCloud has {} data points after passthrough filter.\n", cloud->size());

 pclViewer(cloud_filtered);

 // 估计点的法向量
 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
 pcl::NormalEstimation<PointT, pcl::Normal> ne;
 pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
 ne.setSearchMethod(tree);
 ne.setInputCloud(cloud_filtered);
 ne.setKSearch(50);
 ne.compute(*cloud_normals);

 pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
 pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
 pcl::ExtractIndices<PointT> extract;
 pcl::ExtractIndices<pcl::Normal> extract_normals;

 // 先进行平面分割
 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
 seg.setNormalDistanceWeight(0.1);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setMaxIterations(100);
 seg.setDistanceThreshold(0.03);
 seg.setInputCloud(cloud_filtered);
 seg.setInputNormals(cloud_normals);
 seg.segment(*inliers_plane, *coefficients_plane);

 std::cout << "Plane coefficients: " << *coefficients_plane << std::endl;

 pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
 extract.setInputCloud(cloud_filtered);
 extract.setIndices(inliers_plane);
 extract.setNegative(true); // 剔除平面
 extract.filter(*cloud_filtered2);
 extract_normals.setNegative(true);
 extract_normals.setInputCloud(cloud_normals);
 extract_normals.setIndices(inliers_plane);
 extract_normals.filter(*cloud_normals2);

 pclViewer(cloud_filtered2);

 // 圆柱分割
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_CYLINDER);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setNormalDistanceWeight(0.1);
 seg.setMaxIterations(10000);
 seg.setDistanceThreshold(0.05);
 seg.setRadiusLimits(0, 0.1);
 seg.setInputCloud(cloud_filtered2);
 seg.setInputNormals(cloud_normals2);
 seg.segment(*inliers_cylinder, *coefficients_cylinder);
 std::cout << "cylinder coefficients: " << *coefficients_cylinder << std::endl;

 // 提取圆柱
 pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
 extract.setInputCloud(cloud_filtered2);
 extract.setIndices(inliers_cylinder);
 extract.setNegative(false);
 extract.filter(*cloud_cylinder);

 pclViewer(cloud_cylinder);
 return 0;
}

PointCloud has 307200 data points.
PointCloud has 307200 data points after passthrough filter.
Plane coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
 values[0]: 0.016223
 values[1]: -0.83761
 values[2]: -0.546028
 values[3]: 0.528923

cylinder coefficients: header:
seq: 0 stamp: 0 frame_id:
values[]
 values[0]: 0.0441161
 values[1]: 0.461857
 values[2]: 1.02395
 values[3]: 0.0227775
 values[4]: -0.836755
 values[5]: -0.547104
 values[6]: 0.0387651

欧几里得聚类分割

算法步骤：

对点云 P 创建 kdtree
初始化聚类队列为 C ，初始化空队列 Q
对于 P 中的每一个点 \(p_i\)，执行下面的步骤：
1. 将 \(P_i\) 加到队列 Q 中，对 Q 中的每个点 \(P_i\) 执行下面步骤：
  1. 查找 \(P_i\) 的邻居点，如果邻居点没有经过处理，将邻居点插入队列 Q 中
2. 当 Q 中所有点都处理后，将 Q 中的点作为一类添加到 C 中
当所有点处理完成后，算法结束

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

int main()
{
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/table_scene_lms400.pcd";
 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 auto cloud_f = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 pcl::PCDReader reader;
 reader.read(dataset.c_str(), *cloud);
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pclViewer(cloud);
 // 下采样
 pcl::VoxelGrid<pcl::PointXYZ> vg;
 auto cloud_filtered = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 vg.setInputCloud(cloud);
 vg.setLeafSize(0.01f, 0.01f, 0.01f);
 vg.filter(*cloud_filtered);
 std::cout << "PointCloud after filter has: " << cloud_filtered->size() << " data points" << std::endl;

 pclViewer(cloud_filtered);

 // 平面分割
 pcl::SACSegmentation<pcl::PointXYZ> seg;
 auto inliers = std::make_shared<pcl::PointIndices>();
 auto coefficients = std::make_shared<pcl::ModelCoefficients>();
 auto cloud_plane = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 seg.setOptimizeCoefficients(true);
 seg.setModelType(pcl::SACMODEL_PLANE);
 seg.setMethodType(pcl::SAC_RANSAC);
 seg.setMaxIterations(100);
 seg.setDistanceThreshold(0.02);

 size_t nr_points = cloud_filtered->size();
 while (cloud_filtered->size() > 0.3 * nr_points) {
 seg.setInputCloud(cloud_filtered);
 seg.segment(*inliers, *coefficients);
 if (inliers->indices.size() == 0) {
 std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
 break;
 }

 pcl::ExtractIndices<pcl::PointXYZ> extract;
 extract.setInputCloud(cloud_filtered);
 extract.setIndices(inliers);
 extract.setNegative(false);
 extract.filter(*cloud_plane);

 extract.setNegative(true);
 extract.filter(*cloud_f);
 *cloud_filtered = *cloud_f;
 }

 std::cout << "PointCloud after plane seg has: " << cloud_filtered->size() << " data points" << std::endl;

 pclViewer(cloud_filtered);

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
 tree->setInputCloud(cloud_filtered);

 std::vector<pcl::PointIndices> cluster_indices;
 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
 ec.setClusterTolerance(0.02);
 ec.setMinClusterSize(100);
 ec.setMaxClusterSize(25000);
 ec.setSearchMethod(tree);
 ec.setInputCloud(cloud_filtered);
 ec.extract(cluster_indices);
 std::cout << "PointCloud after cluster has: " << cluster_indices.size() << " clusters" << std::endl;

 pclViewCluster(cloud_filtered, cluster_indices);

 return 0;
}

PointCloud before filter has: 460400 data points
PointCloud after filter has: 41049 data points
PointCloud after plane seg has: 8071 data points
PointCloud after cluster has: 5 clusters

区域生长分割算法

区域生长算法目的是合并在平滑性约束下足够接近的点，因此该算法的输出是一组聚类，每个聚类是一组被认为属于同一平滑表面的点。

算法步骤：

选取的点被加入到名为种子点（seeds）的点集中。
算法遍历每一个种子点，查找其邻域点。
1. 逐一检测每个邻域点：计算该点法向量与当前种子点法向量之间的夹角。若夹角小于阈值，则将该邻域点归入当前区域。
2. 随后再检测该邻域点的曲率值。若曲率小于阈值，则将该点加入种子点集。
3. 将当前种子点从种子点集中移除。

当种子点集合为空时，生长完成

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/region_growing.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/region_growing_tutorial.pcd";
 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZ>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
 auto normals = std::make_shared<pcl::PointCloud<pcl::Normal>>();
 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;

 normal_estimator.setSearchMethod(tree);
 normal_estimator.setInputCloud(cloud);
 normal_estimator.setKSearch(50);
 normal_estimator.compute(*normals);

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *indices);

 pclViewer(cloud);

 // 区域增长
 pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
 reg.setMinClusterSize(50);
 reg.setMaxClusterSize(1000000);
 reg.setSearchMethod(tree);
 reg.setNumberOfNeighbours(30);
 reg.setInputCloud(cloud);
 reg.setIndices(indices);
 reg.setInputNormals(normals);
 reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
 reg.setCurvatureThreshold(1.0);

 std::vector<pcl::PointIndices> clusters;
 reg.extract(clusters);
 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;

 pclViewCluster(cloud, clusters);
 return 0;
}

1
2

PointCloud before filter has: 108104 data points
Number of clusters is equal to 115

基于颜色的区域生长分割算法

和区域生长算法的区别：

使用颜色而不是法线特征
会合并颜色差异较小的聚类

#include <format>
#include <iostream>
#include <filesystem>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/region_growing_rgb.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/region_growing_rgb_tutorial.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>();

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *cloud, *indices);

 pclViewer(cloud);

 // 区域增长
 pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
 reg.setInputCloud(cloud);
 reg.setSearchMethod(tree);
 reg.setDistanceThreshold(10);
 reg.setPointColorThreshold(6);
 reg.setRegionColorThreshold(5);
 reg.setMinClusterSize(600);

 std::vector<pcl::PointIndices> clusters;
 reg.extract(clusters);
 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
 pclViewCluster(cloud, clusters);
 return 0;
}

1
2

PointCloud before filter has: 307200 data points
Number of clusters is equal to 25

基于最小割的分割算法

这个算法对点云进行二值分割。根据对象中心和半径将点云分割为前景（属于对象的点）和背景（不属于对象的点）

#include <format>
#include <iostream>
#include <filesystem>

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

#include <pcl/filters/filter_indices.h>
#include <pcl/segmentation/min_cut_segmentation.h>

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/min_cut_segmentation_tutorial.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZ>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pcl::IndicesPtr indices(new std::vector<int>);
 pcl::removeNaNFromPointCloud(*cloud, *cloud, *indices);

 pclViewer(cloud);

 // min cut 分割
 pcl::MinCutSegmentation<pcl::PointXYZ> seg;
 seg.setInputCloud(cloud);
 seg.setIndices(indices);

 pcl::PointCloud<pcl::PointXYZ>::Ptr foreground_points(new pcl::PointCloud<pcl::PointXYZ>());
 pcl::PointXYZ point;
 point.x = 68.97;
 point.y = -18.55;
 point.z = 0.57;
 foreground_points->points.push_back(point);
 seg.setForegroundPoints(foreground_points);

 seg.setSigma(0.25);
 seg.setRadius(3.0433856);
 seg.setNumberOfNeighbours(14);
 seg.setSourceWeight(0.8);


 std::vector<pcl::PointIndices> clusters;
 seg.extract(clusters);

 std::cout << "Maximum flow is " << seg.getMaxFlow() << std::endl;

 std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
 pclViewCluster(cloud, clusters);
 return 0;
}

1
2
3

PointCloud before filter has: 9311 data points
Maximum flow is 5970.37
Number of clusters is equal to 2

条件欧几里得聚类分割

支持自定义条件的欧几里德

#include <format>
#include <iostream>
#include <filesystem>

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

#include <pcl/filters/filter_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>

bool enforceNormalOrIntensitySimilarity(const pcl::PointXYZINormal& point_a,
 const pcl::PointXYZINormal& point_b,
 float /*squared_distance*/) {
 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(),
 point_b_normal = point_b.getNormalVector3fMap();
 if (std::abs(point_a.intensity - point_b.intensity) < 5.0f) return (true);
 if (std::abs(point_a_normal.dot(point_b_normal)) >
 std::cos(30.0f / 180.0f * static_cast<float>(M_PI)))
 return (true);
 return (false);
}

bool customRegionGrowing(const pcl::PointXYZINormal& point_a,
 const pcl::PointXYZINormal& point_b,
 float squared_distance) {
 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap(),
 point_b_normal = point_b.getNormalVector3fMap();

 if (squared_distance < 10000) {
 if (std::abs(point_a.intensity - point_b.intensity) < 8.0f) return (true);
 if (std::abs(point_a_normal.dot(point_b_normal)) >
 std::cos(30.0f / 180.0f * static_cast<float>(M_PI)))
 return (true);
 } else {
 if (std::abs(point_a.intensity - point_b.intensity) < 3.0f) return (true);
 }
 return (false);
}

int main() {
 auto home = std::filesystem::path(getenv("HOME"));
 auto dataset = home / "Documents/Dataset/Statues_4.pcd";

 auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
 auto cloud_out = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
 if (pcl::io::loadPCDFile<pcl::PointXYZI>(dataset.c_str(), *cloud) == -1) {
 std::cout << "Cloud reading failed." << std::endl;
 return (-1);
 }
 std::cout << "PointCloud before filter has: " << cloud->size() << " data points" << std::endl;

 pcl::VoxelGrid<pcl::PointXYZI> vg;
 vg.setInputCloud(cloud);
 vg.setLeafSize(80.0, 80.0, 80.0);
 vg.setDownsampleAllData(true);
 vg.filter(*cloud_out);
 std::cout << "PointCloud after voxel filter has: " << cloud_out->size() << " data points"
 << std::endl;

 pclViewer(cloud_out);

 // 法向量估计
 auto cloud_with_normals = std::make_shared<pcl::PointCloud<pcl::PointXYZINormal>>();
 auto search_tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZI>>();
 pcl::copyPointCloud(*cloud_out, *cloud_with_normals);
 pcl::NormalEstimation<pcl::PointXYZI, pcl::PointXYZINormal> ne;
 ne.setInputCloud(cloud_out);
 ne.setSearchMethod(search_tree);
 ne.setRadiusSearch(300.0);
 ne.compute(*cloud_with_normals);

 // 聚类设置
 auto clusters = std::make_shared<pcl::IndicesClusters>();
 auto small_clusters = std::make_shared<pcl::IndicesClusters>();
 auto large_clusters = std::make_shared<pcl::IndicesClusters>();
 pcl::ConditionalEuclideanClustering<pcl::PointXYZINormal> cec(true);
 cec.setInputCloud(cloud_with_normals);
 cec.setConditionFunction(&customRegionGrowing);
 cec.setClusterTolerance(500.0);
 cec.setMinClusterSize(cloud_with_normals->size() / 1000);
 cec.setMaxClusterSize(cloud_with_normals->size() / 5);
 cec.segment(*clusters);
 cec.getRemovedClusters(small_clusters, large_clusters);

 // pclViewCluster(cloud_out, *clusters);

 pclViewCluster(cloud_out, *small_clusters);

 pclViewCluster(cloud_out, *large_clusters);
 return 0;
}

1
2

PointCloud before filter has: 19553780 data points
PointCloud after voxel filter has: 202529 data points

配准

点云配准就是把多片散乱的点云，拼到同一个坐标系里，拼成一整块完整的点云。

配准 A ，B 点云，就是计算从 A 到 B 所做的旋转 R 和平移 t 变换。

配准算法流程：

提取关键点（KeyPoints）
使用特征描述关键点
找到相关联的特征，估计变换，有误差
不断迭代，直到误差达到阈值

ICP 配准

迭代最近点算法，通过最小化两片点云之间点的距离估计出变换

#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main() {
 auto cloud_in = genPointCloud<pcl::PointXYZ>(5);
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

 *cloud_out = *cloud_in;

 for (auto& p : *cloud_out) {
 p.x += 0.7f;
 }

 std::cout << "Transformed " << cloud_out->size() << " data points" << std::endl;

 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 icp.setInputSource(cloud_in);
 icp.setInputTarget(cloud_out);
 pcl::PointCloud<pcl::PointXYZ> final;
 icp.align(final);

 std::cout << "ICP has " << (icp.hasConverged() ? "coverage" : "not coverage") << ", score: " << icp.getFitnessScore() << std::endl;
 std::cout << "trans formation: " << icp.getFinalTransformation() << std::endl;

 return 0;
}

Transformed 5 data points
ICP has coverage, score: 1.28697e-13
trans formation: 1 3.31085e-07 -1.86265e-07 0.7
 4.28409e-08 1 2.12342e-07 1.2964e-07
 -1.3411e-07 1.19209e-07 1 1.9297e-07
 0 0 0 1

增量配准

使用 ICP 算法增量配准多片点云

#include <iostream>
#include <filesystem>
#include <format>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp_nl.h>

#include <pcl/point_representation.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

using PointT = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<PointT>;
using PointNormalT = pcl::PointNormal;
using PointCloudWithNormals = pcl::PointCloud<PointNormalT>;

static pcl::visualization::PCLVisualizer* p;
static int vp_1, vp_2;
static bool next_step = false;


struct PCD {
 PointCloud::Ptr cloud;
 std::string name;
 PCD() : cloud(new PointCloud){};
};

class MyPointRepresentation : public pcl::PointRepresentation<PointNormalT> {
 using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:
 MyPointRepresentation() {
 nr_dimensions_ = 4;
 }

 void copyToFloatArray(const PointNormalT& p, float* out) const override {
 out[0] = p.x;
 out[1] = p.y;
 out[2] = p.z;
 out[3] = p.curvature;
 }
};

void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source) {
 p->removePointCloud("vp1_target");
 p->removePointCloud("vp1_source");

 PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
 PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
 p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
 p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);

 next_step = false;
 while (!next_step && !p->wasStopped())
 {
 p->spinOnce(100);
 }
}

void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target,
 const PointCloudWithNormals::Ptr cloud_source) {
 p->removePointCloud("source");
 p->removePointCloud("target");

 PointCloudColorHandlerGenericField<PointNormalT> tgt_h(cloud_target, "curvature");
 if (!tgt_h.isCapable()) {
 std::cerr << "Can not create curvature color handler" << std::endl;
 }

 PointCloudColorHandlerGenericField<PointNormalT> src_h(cloud_source, "curvature");
 if (!src_h.isCapable()) {
 std::cerr << "Can not create curvature color handler" << std::endl;
 }
 p->addPointCloud(cloud_target, tgt_h, "target", vp_2);
 p->addPointCloud(cloud_source, src_h, "source", vp_2);

 p->spinOnce();
}

std::vector<PCD> loadData() {
 std::vector<PCD> ret;
 std::filesystem::path home(std::getenv("HOME"));
 std::vector<std::string> filenames = {
 std::filesystem::path(home / "Documents/Dataset/capture0001.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0002.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0003.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0004.pcd").string(),
 std::filesystem::path(home / "Documents/Dataset/capture0005.pcd").string(),
 };

 for (size_t i = 0; i < filenames.size(); i++) {
 PCD m;
 m.name = filenames[i];
 pcl::io::loadPCDFile(filenames[i], *m.cloud);
 std::vector<int> indices;
 pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
 ret.push_back(m);
 }

 return ret;
}

void pairAlign(const PointCloud::Ptr cloud_src,
 const PointCloud::Ptr cloud_tgt,
 PointCloud::Ptr output,
 Eigen::Matrix4f& final_transform,
 bool downsample = false) {
 PointCloud::Ptr src(new PointCloud);
 PointCloud::Ptr tgt(new PointCloud);
 pcl::VoxelGrid<PointT> grid;

 // 下采样
 if (downsample) {
 grid.setLeafSize(0.05, 0.05, 0.05);
 grid.setInputCloud(cloud_src);
 grid.filter(*src);

 grid.setInputCloud(cloud_tgt);
 grid.filter(*tgt);
 } else {
 src = cloud_src;
 tgt = cloud_tgt;
 }

 // 估计法向量
 PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
 PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);

 pcl::NormalEstimation<PointT, PointNormalT> norm_est;
 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
 norm_est.setSearchMethod(tree);
 norm_est.setKSearch(30);

 norm_est.setInputCloud(src);
 norm_est.compute(*points_with_normals_src);
 pcl::copyPointCloud(*src, *points_with_normals_src);

 norm_est.setInputCloud(tgt);
 norm_est.compute(*points_with_normals_tgt);
 pcl::copyPointCloud(*tgt, *points_with_normals_tgt);

 MyPointRepresentation point_repr;
 float alpha[4] = {1.0, 1.0, 1.0, 1.0};
 point_repr.setRescaleValues(alpha);

 // ICP
 pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
 reg.setTransformationEpsilon(1e-6);
 reg.setMaxCorrespondenceDistance(0.1);
 reg.setPointRepresentation(std::make_shared<const MyPointRepresentation>(point_repr));

 reg.setInputSource(points_with_normals_src);
 reg.setInputTarget(points_with_normals_tgt);

 Eigen::Matrix4f ti = Eigen::Matrix4f::Identity(), prev, target_to_source;
 PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
 reg.setMaximumIterations(2);
 for (int i = 0; i < 30; ++i) {
 std::cerr << "Iteration nr: " << i << std::endl;

 points_with_normals_src = reg_result;
 reg.setInputSource(points_with_normals_src);
 reg.align(*reg_result);

 ti = reg.getFinalTransformation() * ti;
 if (std::abs((reg.getLastIncrementalTransformation() - prev).sum()) <
 reg.getTransformationEpsilon()) {
 reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
 }
 prev = reg.getLastIncrementalTransformation();
 showCloudsRight(points_with_normals_tgt, points_with_normals_src);
 }

 target_to_source = ti.inverse();

 pcl::transformPointCloud(*cloud_tgt, *output, target_to_source);

 p->removePointCloud("source");
 p->removePointCloud("target");
 PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
 PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
 p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
 p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);

 next_step = false;
 while (!next_step && !p->wasStopped())
 {
 p->spinOnce(100);
 }

 p->removePointCloud("source");
 p->removePointCloud("target");

 *output += *cloud_src;
 final_transform = target_to_source;
}


// 键盘回调函数
void keyboardCallback(const pcl::visualization::KeyboardEvent& event, void*)
{
 if (event.keyDown() && event.getKeyCode() == 'n')
 {
 next_step = true;
 }
}


int main(int argc, char* argv[]) {

 auto pcds = loadData();

 p = new pcl::visualization::PCLVisualizer(argc, argv, "example");
 p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
 p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
 p->setBackgroundColor(0.2, 0.2, 0.2, vp_1);
 p->setBackgroundColor(0.7, 0.7, 0.7, vp_2);

 p->registerKeyboardCallback(keyboardCallback);

 PointCloud::Ptr result(new PointCloud), source, target;
 Eigen::Matrix4f global_transform = Eigen::Matrix4f::Identity(), pair_transform;

 for (size_t i = 1; i < pcds.size(); i++) {
 source = pcds[i - 1].cloud;
 target = pcds[i].cloud;


 showCloudsLeft(source, target);

 PointCloud::Ptr temp(new PointCloud);

 pairAlign(source, target, temp, pair_transform, true);

 pcl::transformPointCloud(*temp, *result, global_transform);

 global_transform *= pair_transform;
 }

 p->spin();

 return 0;
}

NDT 算法

#include <iostream>
#include <filesystem>
#include <thread>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/ndt.h>

using namespace std::chrono_literals;

int main(int argc, char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto pcd1_file =
 std::filesystem::path(home / "Documents/Dataset/room_scan1.pcd");
 auto pcd2_file =
 std::filesystem::path(home / "Documents/Dataset/room_scan2.pcd");

 pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile(pcd1_file.string(), *target_cloud) == -1) {
 return -1;
 }

 pclViewer(target_cloud);

 pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 if (pcl::io::loadPCDFile(pcd2_file.string(), *input_cloud) == -1) {
 return -1;
 }

 pclViewer(input_cloud);

 // 近似体素滤波，精度比体素滤波低，但是速度快
 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
 approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
 approximate_voxel_filter.setInputCloud(input_cloud);
 approximate_voxel_filter.filter(*filtered_cloud);

 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
 ndt.setTransformationEpsilon(0.01);
 ndt.setStepSize(0.1);
 ndt.setResolution(1.0);
 ndt.setMaximumIterations(35);

 ndt.setInputSource(filtered_cloud);
 ndt.setInputTarget(target_cloud);

 // 初始化估计
 Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
 Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
 Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

 pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 ndt.align(*output_cloud, init_guess);

 std::cout << "Normal Distributions Transform has " << (ndt.hasConverged ()?"converged":"not converged")
 << ", score: " << ndt.getFitnessScore () << std::endl;

 // 可视化
 pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
 viewer->setBackgroundColor(0.1, 0.1, 0.1);

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
 viewer->addPointCloud(target_cloud, target_color, "target cloud");

 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 255, 0);
 viewer->addPointCloud(output_cloud, output_color, "output cloud");

 viewer->addCoordinateSystem(1.0, "global");
 viewer->initCameraParameters();

 while (!viewer->wasStopped()) {
 viewer->spinOnce(100);
 std::this_thread::sleep_for(100ms);
 }

 return 0;
}

`1`	`Normal Distributions Transform has converged, score: 0.679887`

在场景中估计对象位姿

#include <filesystem>

#include <Eigen/Core>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

using PointNT = pcl::PointNormal;
using PointCloudT = pcl::PointCloud<PointNT>;
using FeatureT = pcl::FPFHSignature33;
using FeatureEstimationT = pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT>;
using FeatureCloudT = pcl::PointCloud<FeatureT>;
using ColorHandlerT = pcl::visualization::PointCloudColorHandlerCustom<PointNT>;

int main(int argc, char* argv[]) {
 std::filesystem::path home(std::getenv("HOME"));
 auto object_file =
 std::filesystem::path(home / "Documents/Dataset/chef.pcd");
 auto scene_file =
 std::filesystem::path(home / "Documents/Dataset/rs1.pcd");

 PointCloudT::Ptr object(new PointCloudT);
 PointCloudT::Ptr object_aligned(new PointCloudT);
 PointCloudT::Ptr scene_before_downsampling(new PointCloudT);
 PointCloudT::Ptr scene(new PointCloudT);
 FeatureCloudT::Ptr object_features(new FeatureCloudT);
 FeatureCloudT::Ptr scene_features(new FeatureCloudT);

 if (pcl::io::loadPCDFile<PointNT>(object_file.string(), *object) == -1) {
 return -1;
 }

 if (pcl::io::loadPCDFile<PointNT>(scene_file.string(), *scene_before_downsampling) == -1) {
 return -1;
 }

 // 下采样
 pcl::VoxelGrid<PointNT> voxel_filter;
 const float leaf = 0.005f;
 voxel_filter.setLeafSize(leaf, leaf, leaf);
 voxel_filter.setInputCloud(object);
 voxel_filter.filter(*object);

 voxel_filter.setInputCloud(scene_before_downsampling);
 voxel_filter.filter(*scene);

 // 法向量估计
 pcl::NormalEstimationOMP<PointNT, PointNT> nest;
 nest.setRadiusSearch(0.005);
 nest.setInputCloud(scene);
 nest.setSearchSurface(scene_before_downsampling);
 nest.compute(*scene);

 // 特征估计
 FeatureEstimationT fest;
 fest.setRadiusSearch(0.025);
 fest.setInputCloud(object);
 fest.setInputNormals(object);
 fest.compute(*object_features);

 fest.setInputCloud(scene);
 fest.setInputNormals(scene);
 fest.compute(*scene_features);

 // 配准
 pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;
 align.setInputSource(object);
 align.setSourceFeatures(object_features);
 align.setInputTarget(scene);
 align.setTargetFeatures(scene_features);
 align.setMaximumIterations(50000);
 align.setNumberOfSamples(3);
 align.setCorrespondenceRandomness(5);
 align.setSimilarityThreshold(0.95f);
 align.setMaxCorrespondenceDistance(2.5f * leaf);
 align.setInlierFraction(0.25f);
 align.align(*object_aligned);

 if (align.hasConverged()) {
 printf ("\n");
 Eigen::Matrix4f transformation = align.getFinalTransformation ();
 pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
 pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
 pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
 pcl::console::print_info ("\n");
 pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
 pcl::console::print_info ("\n");
 pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
 pcl::visualization::PCLVisualizer visu("Alignment");
 visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
 visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
 visu.spin ();
 }

 return 0;
}


 | 0.007 -0.993 -0.118 |
R = | -1.000 -0.009 0.020 |
 | -0.021 0.118 -0.993 |

t = < -0.130, 0.065, 0.078 >

Inliers: 1422/3432

Pcl on 小幻Coding - 记录成长