机器学习算法一种适用于提取障碍物点云的DBSCAN聚类算法（ROS costmsp_convert）

“ 最近在做项目过程中，我们注意到当考虑车体轮廓去进行全局路径规划（如混合A*算法），如果仅仅将障碍物点云数据中得每一个点只当成点，会造成较大的计算时间，如若将点云聚类成圆形线型多边形障碍物会提高计算效率，这是其中一个原因，另一个原因是，我们需要知道障碍物的大小，障碍物大小程度决定了我们的动态避障的方法和策略。因此我根据costmap_conventer 进行了脱ROS处理，删除了一些暂时不用的逻辑，方便项目中使用。”

带噪声的基于密度的空间聚类（DBSCAN）是一种数据聚类算法，由Martin Ester、Hans-Peter Kriegel、Jörg Sander和Xiaowei Xu于 1996 年提出。它是一种基于密度的聚类非参数算法：给定某个空间中的一组点，它将紧密堆积的点（具有许多附近邻居的点）组合在一起，并将单独位于低密度区域（其最近邻居太远的点）的点标记为异常值。DBSCAN 是最常见、最常被引用的聚类算法之一。

2014 年，该算法在领先的数据挖掘会议 ACM SIGKDD上获得了时间考验奖（该奖项授予在理论和实践上受到广泛关注的算法）。截至 2020 年 7 月，后续论文“重新审视 DBSCAN：为什么以及如何（仍然）使用 DBSCAN” [4]出现在著名的ACM Transactions on Database Systems (TODS)期刊下载次数最多的 8 篇文章列表中。‍‍‍‍‍‍‍‍

广受欢迎的后续HDBSCAN*最初由 Ricardo JG Campello、David Moulavi 和 Jörg Sander 于 2013 年发布，随后于 2015 年与Arthur Zimek一起进行了扩展。它修改了一些原始决策‍，例如边界点，并产生了一个层次结构的结果，而不是平面的结果。

—

基本概念

（1）核心对象：若某个点的密度达到算法设定的阈值，则其为核心点，即r领域内点的数量不小于minPts。也就是说，当一个数据点为圆心点时，设定他的半径为，以圆心点为圆心、为半径画圆，当这个圆所包含的点的数量大于等于我们设定的最小数量点minPts，我们就称这个数据点为核心对象也叫核心点。

（2）邻域的距离阈值：就是我们需要设定的以数据点为圆心画圆的圆所需要的半径r。

（3）直接密度可达：如果点p在点q的r邻域内，并且q是一个核心点，则我们称p-q直接密度可达。

（4）密度可达：如果有一个点的序列为，对任意的是直接密度可达的，则称~密度可达。实际上就是两两个数据点之间直接密度可达最终形成一条传播的链。

（5）密度相连：如果从核心点出发，点和点都是和密度可达的，则称点和是密度相连的，同理，点和都是和密度可达的，我们也可以称和是密度相连的。

（6）边界点：属于某一个类的非核心点，不能再成为核心点。简单来说就是所画圆的圆边上的点，根据这个点为圆心画一个圆，园内所包含的数据点小于minPts。

（7）噪声点：不属于任何一个类的点，以任何一个核心点画圆，都不能把这个点包含在园内，用密度可达的思想来解释就是从任何一个核心点到噪声点都是密度不可达的。

—

算法流程

考虑某个空间中要聚类的一组点。令ε为指定某个点的邻域半径的参数。为了进行 DBSCAN 聚类，这些点被分类为核心点、（直接）可达点和异常值，如下所示：

如果至少有minPts 个点在与点 p的距离内（包括p），则点p为核心点。
如果点q与核心点p的距离在ε以内，则点q可从p直接到达。只有点才可以说是可从核心点直接到达。
如果存在一条路径p ₁ , ..., p _{n ，}其中p ₁ = p且p _n = q，并且每个p _i₊₁都可从p _i直接到达，则点q可从p到达。请注意，这意味着初始点和路径上的所有点都必须是核心点，q可能除外。
所有无法从其他点到达的点都是异常点或噪声点。

现在，如果p是核心点，那么它与所有可从其到达的点（核心或非核心）一起形成一个集群。每个集群至少包含一个核心点；非核心点可以成为集群的一部分，但它们形成集群的“边缘”，因为它们不能用于到达更多点。

原始的基于查询的算法

DBSCAN 需要两个参数：ε (eps) 和形成密集区域所需的最小点数 (minPts)。它从尚未访问的任意起点开始。检索此点的 ε 邻域，如果它包含足够多的点，则启动一个集群。否则，该点被标记为噪声。请注意，此点稍后可能会在另一个点的足够大小的 ε 环境中被发现，因此成为集群的一部分。

如果发现某个点是某个簇的密集部分，则其 ε 邻域也是该簇的一部分。因此，所有位于 ε 邻域内的点都会被添加，如果这些点也是密集的，则它们自己的 ε 邻域也会被添加。此过程持续进行，直到完全找到密度连接的簇。然后，检索并处理新的未访问点，从而发现另一个簇或噪声。

^{DBSCAN 可与任何距离函数}（以及相似性函数或其他谓词）一起使用。因此，距离函数 (dist) 可以看作是一个附加参数。

该算法可以用伪代码表示如下

DBSCAN(DB, distFunc, eps, minPts) {    C := 0                                                  /* Cluster counter */    for each point P in database DB {        if label(P) ≠ undefined then continue               /* Previously processed in inner loop */        Neighbors N := RangeQuery(DB, distFunc, P, eps)     /* Find neighbors */        if |N| < minPts then {                              /* Density check */            label(P) := Noise                               /* Label as Noise */            continue        }        C := C + 1                                          /* next cluster label */        label(P) := C                                       /* Label initial point */        SeedSet S := N \ {P}                                /* Neighbors to expand */        for each point Q in S {                             /* Process every seed point Q */            if label(Q) = Noise then label(Q) := C          /* Change Noise to border point */            if label(Q) ≠ undefined then continue           /* Previously processed (e.g., border point) */            label(Q) := C                                   /* Label neighbor */            Neighbors N := RangeQuery(DB, distFunc, Q, eps) /* Find neighbors */            if |N| ≥ minPts then {                          /* Density check (if Q is a core point) */                S := S ∪ N                                  /* Add new neighbors to seed set */            }        }    }}

优点：

与k-means相反，DBSCAN 不需要预先指定数据中的聚类数量。
DBSCAN 可以找到任意形状的簇。它甚至可以找到完全被另一个簇包围（但不连接到另一个簇）的簇。由于 MinPts 参数的存在，所谓的单链接效应（不同的簇由一条细线的点连接）被减弱了。
DBSCAN 具有噪声的概念，并且对异常值具有鲁棒性。
DBSCAN 仅需要两个参数，并且对数据库中点的顺序基本不敏感。（但是，如果点的顺序发生变化，位于两个不同簇边缘的点可能会交换簇成员身份，并且簇分配只有在同构的情况下才是唯一的。）
DBSCAN 设计用于可以加速区域查询的数据库，例如使用R* 树。
如果对数据有很好的理解，领域专家可以设置参数 minPts 和 ε。

缺点：

DBSCAN 并非完全确定性：可从多个群集到达的边界点可以是任一群集的一部分，具体取决于数据处理的顺序。对于大多数数据集和域，这种情况并不经常发生，并且对聚类结果影响不大：无论是核心点还是噪声点，DBSCAN 都是确定性的。DBSCAN* 是一种将边界点视为噪声的变体，这种方式实现了完全确定的结果以及对密度连通分量的更一致的统计解释。
DBSCAN 的质量取决于函数 regionQuery(P,ε) 中使用的距离度量。最常用的距离度量是欧几里得距离。特别是对于高维数据，由于所谓的“维数灾难 ”，这种度量几乎毫无用处，很难找到合适的 ε 值。然而，这种影响也存在于任何其他基于欧几里得距离的算法中。
DBSCAN 无法很好地对密度差异较大的数据集进行聚类，因为无法为所有聚类适当地选择 minPts-ε 组合。
如果对数据和尺度不太了解，选择一个有意义的距离阈值 ε 可能会很困难。

—

C++代码实施

我的工程代码链接：https://github.com/JackJu-HIT/costmap_converter_.git

我是将costmap_converter_代码中的ROS相关去掉了，直接可运行的。

程序的入口：

main.cpp
////////////////////////////////
/**Function: convert costmap obstacle to Polygon*Create by:juchunyu*Date:2024-6-7 9:52:00*Last modified:juchunyu*/#include #include 
int main(){
  boost::shared_ptr<:basecostmaptopolygons> costmap_converter_(new costmap_converter::CostmapToPolygonsDBSMCCH());       costmap_converter_->initialize();
  costmap_converter_->setCostmap2D();
  costmap_converter_->startWorker();
  costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();

  for (std::size_t i=0; iobstacles.size(); ++i)  {    const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);    const geometry_msgs::Polygon* polygon = &obstacle->polygon;
    if (polygon->points.size()==1 && obstacle->radius > 0) // Circle    {      std::cout << "Obstacle Type:Circle " << "(" <points[0].x  << ", " <<  polygon->points[0].y << ")  r=" <radius << std::endl;    }    else if (polygon->points.size()==1) // Point    {      std::cout << "Obstacle Type:Point "  <<  "(" <points[0].x  << ", " <<  polygon->points[0].y<< ")" << std::endl;    }    else if (polygon->points.size()==2) // Line    {        std::cout << "Obstacle Type:Line " << "Line start = (" <points[0].x  << "," <<  polygon->points[0].y <<             ") Line end = ( " <points[1].x  << "," <<  polygon->points[1].y << ")" << std::endl;         }    else if


    
 (polygon->points.size()>2) // Real polygon    {        std::cout << "Obstacle Type:Polygon ";
        for(int j = 0; j < polygon->points.size();j++)        {   if(j > 0)              std::cout << "->";            std::cout << "(" << polygon->points[j].x << "," <<  polygon->points[j].x << ")";        }        std::cout << std::endl;    }
     }
}

显然通过上述的代码，我们知道可以得到Obstacle Type:Circle（圆形） Obstacle Type:Line（线形） Obstacle Type:Polygon（多变形）

程序的给的待聚类的点云集合设置：

costmap_to_polygons.cpp//////////////////////////void CostmapToPolygonsDBSMCCH::updateCostmap2D(){      //std::cout << " CostmapToPolygonsDBSMCCH::updateCostmap2D()" << std::endl;      occupied_cells_.clear();      
      // allocate neighbor lookup      int cells_x = 1000;//int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;      int cells_y = 1000;//int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {        neighbor_size_x_ = cells_x;        neighbor_size_y_ = cells_y;        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);      }          int i = 0;     int j = 2;     while(i < 100)     {        addPoint(j,j);        j += 0.1;        i++;     }
     i = 0;     j = 50;     while(i < 100)     {        addPoint(j,j);        j += 0.1;        i++;     }    */        addPoint(10,10);    addPoint(9.8,9.8);    addPoint(9.8,10);    addPoint(10,9.8);    addPoint(10.1,10.1);    addPoint(10.1,9.8);    addPoint(9.7,9.8);    addPoint(9.6,9.6);    addPoint(9.5,9.5);


         addPoint(1,1);    addPoint(1.1,1.0);    addPoint(1.2,1.0);    addPoint(1.3,1.0);    addPoint(1.4,1.0);



    

    //std::cout << " CostmapToPolygonsDBSMCCH::updateCostmap2D()2222" << std::endl;
}

首先需要设定cells_x和cells_有，这俩确定地图边界，然后我们通过addPoint函数输入需要聚类的点云集合。

如何运行？

mkdir build cd build
cmake.. make
./main

效果？

我在上述的基础上使用c++ matplotlib 可视化工具，最新代码参见https://github.com/JackJu-HIT/costmap_converter_ 。

原始的障碍物点云分布：

经过聚类后的障碍物：

我代码中举例的三个障碍物点云集合聚类成了三个多边形，效果还不错。

需要指出的是：DBSCAN聚类的效果还和参数设定有关，需要根据情况设定。

Reference：

https://en.wikipedia.org/wiki/DBSCAN