欧式聚类（C++编写实现＜不需要任何库＞）

欧式聚类（C++编写实现<不需要任何库>）

简介：

本项目应用背景：点云数在1000个以内，但需要使用欧式聚类，故过程中未使用建树过程，针对实际项目接口编写，此项目数据结构 (struct OBJPOINT) 为项目中数据结构，不能改变，故采用pair反应点状态。输入接口有两个 (ImportPointCloud 、SetDate)， 一个测试使用，一个实际项目接口使用。

1、设置三个参数

**tolerance **是设置 kdtree 的近邻搜索的搜索半径，从实验结果来看，tolerance 越大，检测到的范围也就越大； 同时如果搜索半径取一个非常小的值，那么一个实际的对象就会被分割为多个聚类；如果将值设置得太高，那么多个对象就会被分割为一个聚类;
**setMinClusterSize()**来限制一个聚类最少需要的点数目；
**setMaXClusterSize()**来限制最多需要的点数目；这两个不要解释；

2、聚类过程

1、 找到空间中某点p10，有kdTree找到离他最近的n个点，判断这n个点到p的距离。将距离小于阈值r的点p12,p13,p14…放在类Q里

2、 在 Q(p10) 里找到一点p12,重复1

3 、在 Q(p10,p12) 找到一点，重复1，找到p22,p23,p24…全部放进Q里

4 、当 Q 再也不能有新点加入了，则完成搜索了；

3、PCL源码解析

pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                               const typename search::Search<PointT>::Ptr &tree,
                               float tolerance, std::vector<PointIndices> &clusters,
                               unsigned int min_pts_per_cluster,
                               unsigned int max_pts_per_cluster)
{
  if (tree->getInputCloud ()->points.size () != cloud.points.size ())   // 点数量检查
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
    return;
  }
  // Check if the tree is sorted -- if it is we don't need to check the first element
  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
  // Create a bool vector of processed point indices, and initialize it to false
  std::vector<bool> processed (cloud.points.size (), false);
  std::vector<int> nn_indices;
  std::vector<float> nn_distances;   // 定义需要的变量


  // Process all points in the indices vector
  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)   //遍历点云中的每一个点
  {
    if (processed[i])   //如果该点已经处理则跳过
      continue;

    std::vector<int> seed_queue;   //定义一个种子队列

    int sq_idx = 0;
    seed_queue.push_back (i);  //加入一个种子


    processed[i] = true;

    while (sq_idx < static_cast<int> (seed_queue.size ()))    //遍历每一个种子
    {
      // Search for sq_idx  kdtree 树的近邻搜索
      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))  
      {
        sq_idx++;
        continue;   //没找到近邻点就继续
      }

      for (size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
      {
        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
          continue;   // 种子点的近邻点中如果已经处理就跳出此次循环继续

        // Perform a simple Euclidean clustering
        seed_queue.push_back (nn_indices[j]);   //将此种子点的临近点作为新的种子点。入队操作
        processed[nn_indices[j]] = true;  // 该点已经处理，打标签
      }

      sq_idx++;
    }

    // If this queue is satisfactory, add to the clusters    最大点数和最小点数的类过滤
    if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
    {
      pcl::PointIndices r;
      r.indices.resize (seed_queue.size ());
      for (size_t j = 0; j < seed_queue.size (); ++j)
        r.indices[j] = seed_queue[j];

      // These two lines should not be needed: (can anyone confirm?) -FF
      std::sort (r.indices.begin (), r.indices.end ());
      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

      r.header = cloud.header;
      clusters.push_back (r);   // We could avoid a copy by working directly in the vector
    }
  }
}

4、C++编写

adapt_cluster.h文件

#pragma once
#include <vector>

struct OBJPOINT
{
	int nid;//ID
	double dx;
	double dy;
	double dz;
	double devp;//重投影误差
	OBJPOINT()
	{
		nid = -1;
		devp = 0;
	}
};

class ADAPT_Cluster
{
public:
	ADAPT_Cluster();
	~ADAPT_Cluster();
	void ImportPointCloud(char& filename);
	void SetDate(std::vector<double*>& vPoints, double& R, int minPts, int maxPts);
	bool RadiusSearch(OBJPOINT& mPoint, std::vector<std::pair<OBJPOINT, bool>>& mvPtStatusList);  // Neighborhood radius search
	void ExtractEuclideanClusters();   // Euclidean clustering
	void slove();  // Test function

public:
	double radius;
	int minPtsPerCluster;
	int maxPtsPerCluster;

private:
	// The status of each point
	std::vector<std::pair<OBJPOINT, bool>> vPtStatusList; 
};

adapt_cluster.cpp 文件

#include <iostream>
#include <fstream>
#include "adapt_cluster.h"

ADAPT_Cluster::ADAPT_Cluster()
{
    radius = 0;
    minPtsPerCluster = 0;
    maxPtsPerCluster = INT_MAX;
}

ADAPT_Cluster::~ADAPT_Cluster()
{}

void ADAPT_Cluster::ImportPointCloud(char& filename)
{
    OBJPOINT temp;
    bool mStatus = false;
    std::ifstream import;
    import.open(&filename);
    while (import.peek() != EOF)
    {
        import >> temp.dx >> temp.dy >> temp.dz ;
        vPtStatusList.push_back(std::pair<OBJPOINT, bool>(temp, mStatus));
    }
    import.close();

    radius = 1;
    minPtsPerCluster = 0;
    maxPtsPerCluster = 1000;

}

void ADAPT_Cluster::SetDate(std::vector<double*>& vPoints, double& R, int minPts, int maxPts)
{
    radius = R;
    minPtsPerCluster = minPts;
    maxPtsPerCluster = maxPts;

    OBJPOINT mPt;
    bool mStatus;
    for (int i = 0; i < static_cast<int>(vPoints.size()); ++i)
    {
        mPt.dx = vPoints[i][1];
        mPt.dy = vPoints[i][2];
        mPt.dz = vPoints[i][3];
        mStatus = false;          // Initialize all point states
        vPtStatusList.push_back(std::pair<OBJPOINT, bool>(mPt, mStatus));
    }
}

bool ADAPT_Cluster::RadiusSearch(OBJPOINT& mPoint, std::vector<std::pair<OBJPOINT, bool>>& mvPtStatusList)
{
    OBJPOINT nPt;
    bool nStatus;
    int num = 0;  // Point within radius
    for (int i = 0; i < static_cast<int>(vPtStatusList.size()); ++i) //Status is false
    {
        double dis = 0, disX, disY, disZ;
        if (!vPtStatusList[i].second)
        {
            disX = (mPoint.dx - vPtStatusList[i].first.dx) * (mPoint.dx - vPtStatusList[i].first.dx);
            disY = (mPoint.dy - vPtStatusList[i].first.dy) * (mPoint.dy - vPtStatusList[i].first.dy);
            disZ = (mPoint.dz - vPtStatusList[i].first.dz) * (mPoint.dz - vPtStatusList[i].first.dz);
            dis = sqrt(disX + disY + disZ);

            if (dis < radius)
            {
                nPt.dx = vPtStatusList[i].first.dx;
                nPt.dy = vPtStatusList[i].first.dy;
                nPt.dz = vPtStatusList[i].first.dz;
                nStatus = vPtStatusList[i].second;
                mvPtStatusList.push_back(std::pair<OBJPOINT, bool>(nPt, nStatus));
                num++;
            }
        }
    }
    if (num == 0)
        return false;
    else
        return true;
}

void ADAPT_Cluster::ExtractEuclideanClusters()
{
    std::vector<std::pair<OBJPOINT, bool>> seedvPtStatusList;
    for (int i = 0; i < static_cast<int>(vPtStatusList.size()); ++i)
    {
        // Skip if the point has been processed
        if (vPtStatusList[i].second)
            continue;

        vPtStatusList[i].second = true;
        seedvPtStatusList.push_back(std::pair<OBJPOINT, bool>(vPtStatusList[i].first, vPtStatusList[i].second));

        int n = 0;
        while (n < static_cast<int>(seedvPtStatusList.size()))
        {
            std::vector<std::pair<OBJPOINT, bool>> RvPtStatusList;
            if (!RadiusSearch(seedvPtStatusList[n].first, RvPtStatusList))
            {
                n++;
                continue;   // Continue without finding a neighbor
            }

            for (int j = 0; j < static_cast<int>(RvPtStatusList.size()); ++j)
            {
                seedvPtStatusList.push_back(std::pair<OBJPOINT, bool>(RvPtStatusList[j].first, RvPtStatusList[j].second));
                for (int m = 0; m < static_cast<int>(vPtStatusList.size()); ++m)
                {
                    if (RvPtStatusList[j].first.dx == vPtStatusList[m].first.dx &&
                        RvPtStatusList[j].first.dy == vPtStatusList[m].first.dy &&
                        RvPtStatusList[j].first.dz == vPtStatusList[m].first.dz &&
                        vPtStatusList[m].second == false)
                    {vPtStatusList[m].second = true; break;}
                }
            }
            RvPtStatusList.clear();
            n++;
        }

        // Judge the number of each category
        if (seedvPtStatusList.size() >= minPtsPerCluster && seedvPtStatusList.size() <= maxPtsPerCluster)
        {
            for (int a = 0; a < static_cast<int>(seedvPtStatusList.size()); ++a)
            {
                std::cout << seedvPtStatusList[a].first.dx << " "
                          << seedvPtStatusList[a].first.dy << " "
                          << seedvPtStatusList[a].first.dz << std::endl;
            }
        }

        std::cout << std::endl;
        seedvPtStatusList.clear();
    }
}

void ADAPT_Cluster::slove()
{
    char filename[255];
    sprintf_s(filename, 255, "import3.asc");
    ImportPointCloud(*filename);
    ExtractEuclideanClusters();
}

int main()
{
    ADAPT_Cluster mADAPT_Cluster;
    mADAPT_Cluster.slove();
    return 0;
}

5、测试效果