[LIDAR팀] Skeleton Code Review

이번에 리뷰할 코드는 skeleton code입니다. skeleton code는 일종의 뼈대 코드로, 기본적인 구조와 기능을 가지고 있지만, 구현되어 있지 않거나 미완성인 부분이 많습니다. 이러한 skeleton code는 주로 프로젝트 초기에 사용되며, 팀 내에서 코드 작성 방식을 표준화하고, 기본 구조를 확립하는데 사용됩니다.

이번 코드 리뷰를 통해 skeleton code의 구조와 기능에 대해 이해하고 이를 미션에 적용시킬수 있도록 개선함으로써 더 나은 미션 코드를 작성하려고 합니다.

그러면 이제부터 코드리뷰를 진행해보겠습니다.

#include "header.h"  //header.h 참조
#include "dbscan.h"  // dbscan.h 참조

using namespace std;

/*dbscan clustering 사용을 위한 변수*/
int minPoints = 10; //10          //Core Point 기준 필요 인접점 최소 개수
double epsilon = 0.3; //0.3        //Core Point 기준 주변 탐색 반경 

//clustering 사이즈 설정해주어서 사이즈에 해당하는 clustring만 보여줌 - clustering 사이즈에 따라서 인식할 수 있는 물체가 달라짐 
// min clustering과 maxclustering size를 이용하여 미션시 라바콘과 실제 차량의 차이를 구별 할 수 있지 않을까 생각됨.
int minClusterSize = 100; //10     //Cluster 최소 사이즈
int maxClusterSize = 10000; //10000  //Cluster 최대 사이즈

//roi 값을 바꿔주면 관심 범위가 달라짐
// ex) xmin,ymin,zmin을 모두 0보다 크게 주면, 범위가 양수의 범위에만 bounding box가 쳐짐

double xMinROI = 0;
double xMaxROI = 5; 
double yMinROI = -5; 
double yMaxROI = 5; 
double zMinROI = -5; 
double zMaxROI = 5;

//ROI를 이용하여 걸러내진 못한 물체들을 BoundingBOX변수를 이용하여 한번 더 filtering 할 수 있음.
double xMinBoundingBox = -3;
double xMaxBoundingBox = 3;
double yMinBoundingBox = -3;
double yMaxBoundingBox = 3;
double zMinBoundingBox = -3;
double zMaxBoundingBox = 3; // BoundingBox 크기 범위 지정 변수

 

* DBSCAN 알고리즘 이란?

DBSCAN(Density-based spatial clustering of applications with noise)은 밀도 기반의 클러스터링으로 점이 세밀하게 몰려 있어서 밀도가 높은 부분을 클러스터링 하는 방식입니다.

쉽게 설명하면, 어느점을 기준으로 반경 x내에 점이 n개 이상 있으면 하나의 군집으로 인식하는 방식입니다.

 

DBSCAN 알고리즘에서는 2개의 하이퍼파라미터가 존재하는데 그것이 바로 epsilon과 minimum points입니다.

이때 점 p에서 부터 거리 e (epsilon)내에 점이 m(minPts) 개 있으면 하나의 군집으로 인식합니다.

[조건, 즉 거리 e 내에 점 m개를 가지고 있는 점 p를 core point (중심점) 이라고 합니다.]

---

그림을 보면서 자세하게 설명드리겠습니다.

 

 

이처럼 epsilon과 minpoints는 DBSCAN 알고리즘에서 사용되는 하이퍼파라미터로, 이 값들에 의해서 clustering이 형성됩니다.

• epsilon(ε) 값은 클러스터링에서 결정적인 역할을 하며, 이 값이 작으면 더 많은 작은 클러스터가 형성되고, 이 값이 크면 더 적은 수의 큰 클러스터가 형성됩니다.
• epsilon(ε)값이 클 경우, minpoints 값을 증가시켜도 클러스터링이 더 느슨한 기준으로 이루어집니다. epsilon(ε)값이 작을 경우,minimum samples값을 감소시켜도 잡음점의 수가 더 많아집니다.
  (주의할 점. epsilon과 minpoints를 모두 만조시켜야 클러스터가 됩니다.)

typedef pcl::PointXYZ PointT;

ros::Publisher roiPub; //ROI Publishser
ros::Publisher clusterPub; //Cluster Publishser
ros::Publisher boundingBoxPub; //Bounding Box Visualization Publisher

//point cloud callback 함수 선언
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& inputcloud) {
  //ROS message 변환
  //PointXYZI가 아닌 PointXYZ로 선언하는 이유 -> 각각의 Cluster를 다른 색으로 표현해주기 위해서. Clustering 이후 각각 구별되는 intensity value를 넣어줄 예정.
  //PointXYZI는 intensity정보를 가지고 있고, 이것은 반사강도를 나타내는데 크게 중요하지 않기 때문에 PointXYZ를 사용함.
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//스마트 포인터를 선언하고, 이 포인터를 통해 새로운 Point Cloud를 생성
  pcl::fromROSMsg(*inputcloud, *cloud); // ROS에서 받아온 Point Cloud 메시지를 PCL의 Point Cloud 형식으로 변환

  //Visualizing에 필요한 Marker 선언
  visualization_msgs::Marker boundingBox;  // xyz를 boundingbox 처리
  visualization_msgs::MarkerArray boundingBoxArray; // boundingbox들을 array로 만들어서 한번에 여러개의 boundingbox를 보여줌 

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xf(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PassThrough<pcl::PointXYZ> xfilter;
  xfilter.setInputCloud(cloud);
  xfilter.setFilterFieldName("x");
  xfilter.setFilterLimits(xMinROI, xMaxROI); 
  xfilter.setFilterLimitsNegative(false);
  xfilter.filter(*cloud_xf);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyf(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PassThrough<pcl::PointXYZ> yfilter;
  yfilter.setInputCloud(cloud_xf);
  yfilter.setFilterFieldName("y");
  yfilter.setFilterLimits(yMinROI, yMaxROI);
  yfilter.setFilterLimitsNegative(false);
  yfilter.filter(*cloud_xyf);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyzf(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PassThrough<pcl::PointXYZ> zfilter;
  zfilter.setInputCloud(cloud_xyf);
  zfilter.setFilterFieldName("z");
  zfilter.setFilterLimits(zMinROI, zMaxROI); // -0.62, 0.0
  zfilter.setFilterLimitsNegative(false);
  zfilter.filter(*cloud_xyzf);

PointCloud를 얻기 위한 방법

• pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xf(new pcl::PointCloud<pcl::PointXYZ>);
  • 설명 : pcl::PointCloudpcl::PointXYZ::Ptr 형식의 스마트 포인터를 선언하고, 이 포인터를 통해 새로운 Point Cloud를 생성합니다. 이때, pcl::PointXYZ는 PCL 라이브러리에서 사용되는 3차원 점(Point) 데이터 타입입니다.  
• pcl::PassThrough<pcl::PointXYZ> xfilter;
  • 설명 : PassThrough 필터를 생성합니다. 이 필터는 Point Cloud에서 지정된 범위 내의 점들만 선택하거나, 지정된 범위 외의 점들만 선택하는 기능을 제공합니다.
• xfilter.setInputCloud(cloud);
  • PassThrough 필터의 입력 Point Cloud를 설정합니다. 이때, setInputCloud 함수를 사용하여 첫 번째 인자로 필터링할 Point Cloud를 가리키는 포인터(cloud)를 전달합니다.
• xfilter.setFilterFieldName("x")
  • PassThrough 필터에서 사용할 필터링 기준(예: x, y, z 좌표)을 설정합니다. 이때, setFilterFieldName 함수를 사용하여 필터링할 좌표를 문자열 형태로 전달합니다. 위 코드에서는 x좌표를 기준으로 필터링을 수행하도록 "x"를 전달하고 있습니다.
• xfilter.setFilterLimits(xMinROI, xMaxROI);
  • PassThrough 필터에서 사용할 범위를 설정합니다. 이때, setFilterLimits 함수를 사용하여 필터링할 범위의 최소값과 최대값을 전달합니다. 위 코드에서는 x좌표가 xMinROI부터 xMaxROI까지의 범위 내에 있는 점들만 선택하도록 필터링 범위를 설정하고 있습니다.
• xfilter.setFilterLimitsNegative(false);
  • PassThrough 필터에서 범위 내의 점들만 선택하도록 필터링하는 옵션을 설정합니다. 이때, setFilterLimitsNegative 함수를 사용하여 false 값을 전달하면 범위 내의 점들만 선택하도록 필터링합니다.
  • 만약 false가 아니고 true일 경우에는, 해당 범위 밖의 점들만 선택하도록 필터링됩니다.
• xfilter.filter(*cloud_xf);
  • PassThrough 필터를 적용하여 필터링된 Point Cloud를 새로운 Point Cloud에 저장합니다. 이때, filter 함수를 사용하여 필터링된 Point Cloud를 가리키는 포인터(cloud_xf)를 전달합니다.

위의 과정을 x -> y -> z 차례대로 걸쳐서, xyz로 필터링 된 cloud_xyzf Point Cloud를 얻을 수 있습니다.

// //Voxel Grid를 이용한 DownSampling
  // pcl::VoxelGrid<pcl::PointXYZ> vg;    // VoxelGrid 선언
  // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); //Filtering 된 Data를 담을 PointCloud 선언
  // vg.setInputCloud(cloud);             // Raw Data 입력
  // vg.setLeafSize(0.5f, 0.5f, 0.5f); // 사이즈를 너무 작게 하면 샘플링 에러 발생
  // vg.filter(*cloud);          // Filtering 된 Data를 cloud PointCloud에 삽입

  //KD-Tree
  //이 포인터를 통해 KD-Tree 검색 방법을 사용할 수 있도록 하는 객체를 생성합니다
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  //Point Cloud(cloud_xyzf)가 비어있지 않은 경우에 한해 KD-Tree 검색 방법에서 사용할 입력 Point Cloud를 설정
  if (cloud_xyzf->size() > 0) {
    tree->setInputCloud(cloud_xyzf);
  }
  //Segmentation
  //클러스터의 index 정보를 저장할 vector를 선언합니다. 
  vector<pcl::PointIndices> cluster_indices;

DBSCANKdtreeCluster<pcl::PointXYZ> dc;
dc.setCorePointMinPts(minPoints);   //Set minimum number of neighbor points
dc.setClusterTolerance(epsilon); //Set Epsilon 
dc.setMinClusterSize(minClusterSize);
dc.setMaxClusterSize(maxClusterSize);
dc.setSearchMethod(tree); // kdtree
dc.setInputCloud(cloud_xyzf); // point_cloud xyz
dc.extract(cluster_indices);

Kdtree기반 DBSCAN 알고리즘 수행

1. 첫 번째 줄에서는 DBSCAN 알고리즘을 수행하기 위해 DBSCANKdtreeClusterpcl::PointXYZ 타입의 객체 dc를 선언합니다.
2. 두 번째 줄에서는 DBSCAN 알고리즘에서 core point(핵심점)으로 인식되기 위한 이웃 포인트의 최소 개수를 설정합니다. 이때, setCorePointMinPts 함수를 사용하여 최소 이웃 포인트 개수를 설정합니다.
   • 위에서 설정해준 minPoints값을 사용합니다.
3. 세 번째 줄에서는 DBSCAN 알고리즘에서 포인트들이 같은 클러스터로 인식되기 위한 거리(threshold)를 설정합니다. 이때, setClusterTolerance 함수를 사용하여 클러스터 거리 threshold를 설정합니다.
   • 위에서 설정해준 epsilon값을 사용합니다.
4. 네 번째 줄과 다섯 번째 줄에서는 클러스터의 최소 크기와 최대 크기를 설정합니다. 이때, setMinClusterSize와 setMaxClusterSize 함수를 사용하여 최소 크기와 최대 크기를 설정합니다.
   • 위에서 설정해준 minClusterSize와 maxClusterSize를 사용합니다.
5. 여섯 번째 줄에서는 KD-Tree 검색 방법을 사용하여 클러스터링을 수행하기 위해 setSeachMethod 함수를 사용하여 KD-Tree 객체를 전달합니다.
6. 일곱 번째 줄에서는 DBSCAN 알고리즘에서 클러스터링을 수행할 입력 Point Cloud를 설정합니다. 이때, setInputCloud 함수를 사용하여 입력 Point Cloud를 가리키는 포인터를 전달합니다.
7. 여덟 번째 줄에서는 DBSCAN 알고리즘을 실행하고, 클러스터링 결과로 얻어진 클러스터의 인덱스 정보를 cluster_indices에 저장합니다. 이때, extract 함수를 사용하여 클러스터링을 수행합니다.

위 코드 이후에는 클러스터링 결과로 얻어진 클러스터의 인덱스 정보(cluster_indices)를 사용하여 클러스터링 결과를 처리할 수 있습니다.

for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++, cluster_id++) {
    pcl::PointCloud<pcl::PointXYZI> eachcloud_clustered; // 클러스터 하나의 point cloud
    float cluster_counts = cluster_indices.size();
    //각 Cluster내 각 Point 접근
    for(vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) {
        pcl::PointXYZI tmp;
        tmp.x = cloud_xyzf->points[*pit].x;
        tmp.y = cloud_xyzf->points[*pit].y;
        tmp.z = cloud_xyzf->points[*pit].z;
        tmp.intensity = cluster_id%8; // 클러스터별로 색을 다르게 하기 위해 tmp.intensity 값을 cluster_id % 8로 설정
        eachcloud_clustered.push_back(tmp); // 클러스터 하나의 point cloud
        totalcloud_clustered.push_back(tmp)짐 // 모든 클러스터의 point cloud
    }

    //minPoint와 maxPoint 받아오기
    // 각 클러스터의 bounding box의 크기를 계산하기 위해, pcl::getMinMax3D 함수를 통해 eachcloud_clustered의 최소값과 최대값을 구합니다.
    pcl::PointXYZI minPoint, maxPoint;
    pcl::getMinMax3D(eachcloud_clustered, minPoint, maxPoint);

    float x_len = abs(maxPoint.x - minPoint.x);   //직육면체 x 모서리 크기
    float y_len = abs(maxPoint.y - minPoint.y);   //직육면체 y 모서리 크기
    float z_len = abs(maxPoint.z - minPoint.z);   //직육면체 z 모서리 크기 
    float volume = x_len * y_len * z_len;         //직육면체 부피

    float center_x = (minPoint.x + maxPoint.x)/2; //직육면체 중심 x 좌표
    float center_y = (minPoint.y + maxPoint.y)/2; //직육면체 중심 y 좌표
    float center_z = (minPoint.z + maxPoint.z)/2; //직육면체 중심 z 좌표 

    // 라이다 센서의 위치는 0,0,0이기 때문에 이를 바탕으로 거리정보를 구할 수 있음
    float distance = sqrt(center_x * center_x + center_y * center_y); //장애물 <-> 차량 거리

    if ( (xMinBoundingBox < x_len && x_len < xMaxBoundingBox) && (yMinBoundingBox < y_len && y_len < yMaxBoundingBox) && (zMinBoundingBox < z_len && z_len < zMaxBoundingBox) ) {
      boundingBox.header.frame_id = "velodyne";
      boundingBox.header.stamp = ros::Time();
      boundingBox.ns = cluster_counts; //ns = namespace
      boundingBox.id = cluster_id; 
      boundingBox.type = visualization_msgs::Marker::CUBE; //직육면체로 표시
      boundingBox.action = visualization_msgs::Marker::ADD;
     
      // 각 좌표의 중심
      boundingBox.pose.position.x = center_x; 
      boundingBox.pose.position.y = center_y;
      boundingBox.pose.position.z = center_z;
	  
      boundingBox.pose.orientation.x = 0.0;
      boundingBox.pose.orientation.y = 0.0;
      boundingBox.pose.orientation.z = 0.0;
      boundingBox.pose.orientation.w = 1.0; // w 값이 1.0이면, 바운딩 박스의 회전이 없음을 의미. 
      //즉, bounding box가 원점에 대해 정렬되어 있어서 회전하지 않은 상태입니다. 이는 bounding box의 모든 면이 축에 평행하게 정렬되어 있을 때 발생할 수 있습니다.
	
      // 길이 
      boundingBox.scale.x = x_len;
      boundingBox.scale.y = y_len;
      boundingBox.scale.z = z_len;
	  
      // bounding box 색깔 설정
      boundingBox.color.a = 0.5; //직육면체 투명도, a = alpha
      boundingBox.color.r = 1.0; //직육면체 색상 RGB값
      boundingBox.color.g = 1.0;
      boundingBox.color.b = 1.0;
      
      // boundingbox 지속 시간 및 boundingBoxArray에 boundingbox추가
      // boundingbox지속시간을 0.1로 준 이유는 cluster한개만 보이는 것은 필요없기 때문에 빠르게 사라지게 하기 위해서 시간을 짧게 설정
      // emplace_back은 push_back과 같속
      boundingBox.lifetime = ros::Duration(0.1); //box 지속시간
      boundingBoxArray.markers.emplace_back(boundingBox);
    }

    cluster_id++; //intensity 증가

  }

클러스터에 대한 bounding box를 생성하고, 이를 시각화하는 과정

1. 반복문에서는 it->indices를 이용해 현재 클러스터에 속하는 포인트들에 접근하고, 각 포인트의 x, y, z 좌표값과 intensity 값을 pcl::PointXYZI 형태로 저장합니다.
   • cluster_indices는 vector이며, 각각의 요소는 pcl::PointIndices 구조체로 구성되어 있습니다. pcl::PointIndices 구조체는 해당 클러스터에 속한 포인트의 인덱스 정보를 포함합니다. 따라서 vector<pcl::PointIndices> 형태로 저장된 cluster_indices에서 반복문을 수행하여 각 클러스터에 속한 포인트들의 인덱스를 가져와야 합니다.
   • 따라서 cluster_indices의 요소를 하나씩 가져오기 위해서는 범위 기반(range-based) for 루프나 반복자(iterator)를 사용해야 합니다. 일반적인 for 루프로는 cluster_indices에 저장된 요소들에 접근할 수 없습니다.
2. eachcloud_clustered는 클러스터 하나의 point cloud 정보만을 가지고 totalcloud_clustered는 전체 클러스트의 point cloud정보를 가집니다.
3. bounding box의 크기를 계산하기 위해서 eachcloud_clustered의 최소값과 최대값을 구합니다. 이를 바탕으로 clustering된 물체의 부피와 해당 물체와의 거리를 계산할 수 있게 됩니다.
4. 그 다음, 조건문을 통해 조건을 만족하면 visualization_msgs::Marker 형식의 boundingBox 변수를 생성하고, 각 필드를 설정합니다.
5. boundingBox 변수를 boundingBoxArray에 추가하고, cluster_id를 증가시킵니다. boundingBoxArray를 사용하는 이유는 한번에 여러개의 boundingbox를 보여주기 위해서 array에 추가해줍니다.
   • 이때 array에 추가할 때, push_back이 아닌 emplace_back을 사용하는 이유는 emplace_back() 함수를 사용하면 벡터에 새로운 요소를 추가하면서 객체를 생성하고 초기화할 수 있으므로 성능 면에서 이점을 가질 수 있습니다. 이는 복사 생성자와 이동 생성자를 호출하지 않아도 되기 때문입니다. 따라서 emplace_back() 함수는 성능을 개선할 수 있으며, 벡터의 복사 및 이동 연산 비용을 줄일 수 있습니다.

//Convert To ROS data type
  pcl::PCLPointCloud2 cloud_p;
  pcl::toPCLPointCloud2(totalcloud_clustered, cloud_p);

  sensor_msgs::PointCloud2 cluster;
  pcl_conversions::fromPCL(cloud_p, cluster);
  cluster.header.frame_id = "velodyne";


  pcl::PCLPointCloud2 cloud_cropbox;
  pcl::toPCLPointCloud2(*cloud_xyzf, cloud_cropbox);

  sensor_msgs::PointCloud2 ROI;
  pcl_conversions::fromPCL(cloud_cropbox, ROI);
  ROI.header.frame_id = "velodyne";


  roiPub.publish(ROI);
  clusterPub.publish(cluster);
  boundingBoxPub.publish(boundingBoxArray);
}

ROS topic으로 publish하기

1. 먼저 PCL에서 제공하는 toPCLPointCloud2 함수를 사용하여 PCL 포인트 클라우드 데이터(totalcloud_clustered)를 PCLPointCloud2 형식(cloud_p)으로 변환합니다. 이는 일반적으로 PCL에서 다른 모듈과의 호환성을 위해서입니다.
2. 다음으로 pcl_conversions 라이브러리에서 제공하는 fromPCL 함수를 사용하여 PCLPointCloud2 데이터(cloud_p)를 ROS의 sensor_msgs::PointCloud2 메시지(cluster)로 변환합니다. 이렇게 변환한 이유는 ROS에서 PointCloud2 메시지가 일반적으로 사용되기 때문입니다.
3. ROI도 cluster와 마찬가지로 두번의 과정(1과 2)을 거쳐 변환하게 됩니다.
4. 마지막으로 ROI, cluster, bondingboxarray를 ROS topic으로 publish합니다.

int main(int argc, char **argv) {
  ros::init(argc, argv, "bounding_box");
  ros::NodeHandle nh;

  ros::Subscriber rawDataSub = nh.subscribe("/velodyne_points", 1, cloud_cb);  // velodyne_points 토픽 구독. velodyne_points = 라이다 raw data

  roiPub = nh.advertise<sensor_msgs::PointCloud2>("/roi", 0.001); 
  clusterPub = nh.advertise<sensor_msgs::PointCloud2>("/cluster", 0.001);                  
  boundingBoxPub = nh.advertise<visualization_msgs::MarkerArray>("/boundingBox", 0.001);     

  ros::spin();

  return 0;
}

main 함수 호출

1. ros::init(argc, argv, "bounding_box") : ROS를 초기화하고 노드의 이름을 "bounding_box"로 지정합니다.
2. ros::NodeHandle nh : 노드 핸들러를 생성합니다.
3. ros::Subscriber rawDataSub = nh.subscribe("/velodyne_points", 1, cloud_cb) : "/velodyne_points" 토픽을 1개의 메시지를 버퍼링하며 구독합니다. 메시지가 수신되면 cloud_cb 콜백 함수가 호출됩니다.
4. roiPub = nh.advertise<sensor_msgs::PointCloud2>("/roi", 0.001) : "/roi" 토픽에 PointCloud2 형식의 메시지를 발행할 수 있는 Publisher를 생성합니다.
5. clusterPub = nh.advertise<sensor_msgs::PointCloud2>("/cluster", 0.001) : "/cluster" 토픽에 PointCloud2 형식의 메시지를 발행할 수 있는 Publisher를 생성합니다.
6. boundingBoxPub = nh.advertise<visualization_msgs::MarkerArray>("/boundingBox", 0.001) : "/boundingBox" 토픽에 MarkerArray 형식의 메시지를 발행할 수 있는 Publisher를 생성합니다.
7. ros::spin() : 콜백 함수 호출 및 노드 실행을 위한 ROS 이벤트 루프를 시작합니다.
8. return 0 : 프로그램을 정상적으로 종료합니다.