PCL将无序轮廓点云排序

原理

灵感来源于(1 封私信 / 4 条消息) 找出无序离散点的先后顺序? - 知乎 (zhihu.com)，大致思路如下：

1. 设置距离阈值，并将第一个点作为种子点，在剩余点里求离种子点最近的点，判断二者距离；
2. 若距离满足条件，就将种子点加入到有序列表里面，同时更新最近的点作为种子点，循环执行；
3. 若距离不满足条件，剩余点重复步骤1和2，作为新的轮廓列表。

如果只执行上述过程，可能会因为种子点不在轮廓起点的位置，划分出多条轮廓，需要判断每条轮廓的起点、终点和其他轮廓的起点终点距离是否满足条件，若满足条件，要将两个轮廓点云合并。直到没有起点和终点距离满足条件。

代码

头文件

#include <vector>                  
#include <Eigen/Core>                
#include <pcl/point_cloud.h>         
#include <pcl/point_types.h>         
#include <pcl/kdtree/kdtree_flann.h> 
#include <cmath>                    
#include <string>
#include <omp.h>
#include <pcl/common/io.h>
#include <pcl/common/distances.h>

#include <opencv2/opencv.hpp>

using namespace pcl;


    class ContourSorter
    {
    private:
        double euclideanDistance(const PointXYZ &a, const PointXYZ &b)
        {
            return pcl::euclideanDistance(a, b);
        }

        bool continueQ(const PointXYZ &pt1, const PointXYZ &pt2, const double &distanceLimit)
        {
            return euclideanDistance(pt1, pt2) < distanceLimit;
        }

        void findNearest(const PointCloud<PointXYZ>::Ptr &cloud_in, const PointXYZ &pt_in, int &nearestidx);

        std::vector<PointCloud<PointXYZ>::Ptr> findOrderedContour(const PointCloud<PointXYZ>::Ptr &cloud_in, double distanceLimit);

        bool isMerge(std::vector<PointCloud<PointXYZ>::Ptr> &cloud_cluster, double distanceLimit);

    public:
        ContourSorter(/* args */);
        ~ContourSorter();

        void run(const PointCloud<PointXYZ>::Ptr &cloud_in, double distanceLimit, std::vector<PointCloud<PointXYZ>::Ptr> &cloud_cluster);
    };

cpp文件

#include "ContourSorter.h"

ContourSorter::ContourSorter()
{
}

ContourSorter::~ContourSorter()
{
}

void ContourSorter::findNearest(const PointCloud<PointXYZ>::Ptr &cloud_in, const PointXYZ &pt_in, int &nearestidx)
{
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud_in);
    std::vector<int> NeighborsKNSearch(1);
    std::vector<float> NeighborsKNSquaredDistance(1);
    if (kdtree.nearestKSearch(pt_in, 1, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0)
    {
        nearestidx = NeighborsKNSearch[0];
    }
    else
    {
        std::cerr << "找不到最近点" << std::endl;
        nearestidx = -1;
    }
}

std::vector<PointCloud<PointXYZ>::Ptr> ContourSorter::findOrderedContour(const PointCloud<PointXYZ>::Ptr &cloud_in, double distanceLimit)
{
    PointCloud<PointXYZ>::Ptr oneCont(new PointCloud<PointXYZ>);
    PointCloud<PointXYZ>::Ptr restCont(new PointCloud<PointXYZ>);
    std::vector<PointCloud<PointXYZ>::Ptr> cloud_cluster;

    // 将除第一个点以外的点加入到restCont中
    for (auto it = cloud_in->begin() + 1; it != cloud_in->end(); ++it)
    {
        restCont->push_back(*it);
    }
    // 将第一个点作为搜索起点，并加入到oneCont中
    PointXYZ tryPt = cloud_in->points[0];
    oneCont->points.push_back(tryPt);
    if (restCont->points.empty())
    {
        cloud_cluster.push_back(oneCont);
        return cloud_cluster;
    }

    // 寻找离tryPt最近的点
    int nextnum;
    findNearest(restCont, tryPt, nextnum);
    // 如果tryPt和最近点距离小于阈值，就将最近点加入到结果中，并更新tryPt
    while (continueQ(restCont->points[nextnum], tryPt, distanceLimit))
    {
        // 将nextnum加入到轮廓点云中，更新tryPt，并从restCont中移除
        tryPt = restCont->points[nextnum];
        oneCont->points.push_back(restCont->points[nextnum]);
        restCont->points.erase(restCont->points.begin() + nextnum);
        if (restCont->points.empty())
        {
            cloud_cluster.push_back(oneCont);
            return cloud_cluster;
        }
        findNearest(restCont, tryPt, nextnum);
    }

    // 寻找结束，说明tryPt和下一个点距离不满足条件，但还存在剩余点云，需要将当前找到的轮廓加入到轮廓集合中
    cloud_cluster.push_back(oneCont);

    // 递归地寻找剩余点的轮廓
    std::vector<PointCloud<PointXYZ>::Ptr> remainingCluser = findOrderedContour(restCont, distanceLimit);
    cloud_cluster.insert(cloud_cluster.end(), remainingCluser.begin(), remainingCluser.end());
    return cloud_cluster;
}

bool ContourSorter::isMerge(std::vector<PointCloud<PointXYZ>::Ptr> &cloud_cluster, double distanceLimit)
{
    for (size_t i = 0; i < cloud_cluster.size(); i++)
    {
        for (size_t j = 0; j < cloud_cluster.size() && j != i; j++)
        {
            // i起点和j终点距离相近
            if (euclideanDistance(cloud_cluster[i]->points[cloud_cluster[i]->points.size() - 1], cloud_cluster[j]->points[0]) < distanceLimit)
            {
                cloud_cluster[i]->insert(cloud_cluster[i]->end(), cloud_cluster[j]->begin(), cloud_cluster[j]->end());
                cloud_cluster.erase(cloud_cluster.begin() + j);
                return true;
            }

            // i起点和j起点距离相近
            if (euclideanDistance(cloud_cluster[i]->points[0], cloud_cluster[j]->points[0]) < distanceLimit)
            {
                // 此时要逆转j，将i插入到j的末尾
                std::reverse(cloud_cluster[j]->begin(), cloud_cluster[j]->end());
                cloud_cluster[j]->insert(cloud_cluster[j]->end(), cloud_cluster[i]->begin(), cloud_cluster[i]->end());
                cloud_cluster.erase(cloud_cluster.begin() + i);
                return true;
            }

            // i终点和j终点相近
            if (euclideanDistance(cloud_cluster[i]->points[cloud_cluster[i]->points.size() - 1], cloud_cluster[j]->points[cloud_cluster[j]->points.size() - 1]) < distanceLimit)
            {
                // 此时要逆转j，将j插入到i末尾
                std::reverse(cloud_cluster[j]->begin(), cloud_cluster[j]->end());
                cloud_cluster[i]->insert(cloud_cluster[i]->end(), cloud_cluster[j]->begin(), cloud_cluster[j]->end());
                cloud_cluster.erase(cloud_cluster.begin() + j);
                return true;
            }
        }
    }
    return false;
}

void ContourSorter::run(const PointCloud<PointXYZ>::Ptr &cloud_in, double distanceLimit, std::vector<PointCloud<PointXYZ>::Ptr> &cloud_cluster)
{
    cloud_cluster = findOrderedContour(cloud_in, distanceLimit);
    while (isMerge(cloud_cluster, distanceLimit * 1.2)) // 扩大距离阈值，处理特殊情况
        ;                                               // 循环执行合并，若无法合并，就执行完毕
}

效果

封闭点云

开放点云