PCL 点云手记

点云即三维空间中点的集合，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