[LiDAR] clustering

LiDAR에 쓰이는 군집화 코드를 보았습니다

아래의 코드는 K-means clustering으로, 물체를 빠르게 찾기 위해 채택한 것으로 생각합니다.

 

K-means clustring 설명은, 그림과 함께 이해하기 쉬운 글을 참고하세요.

https://ratsgo.github.io/machine%20learning/2017/04/19/KC/

K-평균 군집화(K-means Clustering) · ratsgo's blog

 

KUUVe는 velodyne vlp-16을 사용하며, 대회에서 사용하는 코드의 원본입니다. (사용하는 코드는 대회에 맞게 최적화함)

//library include
#include <iostream>
#include <cmath>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <velodyne_pointcloud/point_types.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <set>
#include <pcl/io/pcd_io.h>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/console/time.h>
#include <pcl/features/normal_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/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>
#include <sensor_msgs/PointCloud2.h>

//define
#define VPoint velodyne_pointcloud::PointXYZIR
#define Point2 pcl::PointXYZI
#define PI 3.14159265359
using namespace std;

typedef pcl::PointXYZI PointTypeIO;
typedef pcl::PointXYZINormal PointTypeFull;

double ROI_theta(double x, double y);

ros::Publisher pub1;
ros::Publisher pub2;
ros::Publisher pub3;
ros::Publisher pub4;
ros::Publisher pub5;

float theta_r = 180 * M_PI/ 180; // The angle of rotation in radians

// x,y 좌표계에 있는 점을 구 좌표계로 변환
double ROI_theta(double x, double y)
{
    double r;
    double theta;

    r = sqrt((x*x)+(y*y));
    theta = acos(x/r)*180/PI;
    return theta;
}

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{

    //1. Msg to pointcloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*input,*cloud);

    //2. 회전변환행렬
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    transform_1 (0,0) = std::cos (theta_r);
    transform_1 (0,1) = -sin(theta_r);
    transform_1 (1,0) = sin (theta_r);
    transform_1 (1,1) = std::cos (theta_r);

    // Executing the transformation
    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    pcl::transformPointCloud (*cloud, *transformed_cloud, transform_1); 

    // transform_1 의형식으로 cloud 를 transformed_cloud로 변환
    pcl::PCLPointCloud2 cloud_p;
    pcl::toPCLPointCloud2(*transformed_cloud, cloud_p);

    sensor_msgs::PointCloud2 output;
    pcl_conversions::fromPCL(cloud_p, output);
    output.header.frame_id = "velodyne";
    //pub1.publish(output);

    //3. ROI 설정. 센서 기준으로 앞 좌우 45도, 입력된 데이터의 z(높이값) -1 이하의 데이터만 추출
    pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
    pcl::fromROSMsg(output,laserCloudIn); //roi
    pcl::PCLPointCloud2 cloud_ROI;
    for(unsigned int j=0; j<laserCloudIn.points.size(); j++)
	{
        if(laserCloudIn.points[j].intensity < 310)
        {
            laserCloudIn.points[j].x = 0;
            laserCloudIn.points[j].y = 0;
            laserCloudIn.points[j].z = 0;
        }
        if(ROI_theta(laserCloudIn.points[j].y , laserCloudIn.points[j].x) < 45)
        {
            laserCloudIn.points[j].x = 0;
            laserCloudIn.points[j].y = 0;
            laserCloudIn.points[j].z = 0;
        }
        if(ROI_theta(laserCloudIn.points[j].y , laserCloudIn.points[j].x) > 135)
        {
            laserCloudIn.points[j].x = 0;
            laserCloudIn.points[j].y = 0;
            laserCloudIn.points[j].z = 0;
        }
        if(laserCloudIn.points[j].x < 0)
        {
            laserCloudIn.points[j].x = 0;
            laserCloudIn.points[j].y = 0;
            laserCloudIn.points[j].z = 0;
        }
        //z축에서 차량에 달려있는 센서의 기준으로 -1m 아래의 데이터 이외는 필터링
        if(laserCloudIn.points[j].z > -1)
        {
            laserCloudIn.points[j].x = 0;
            laserCloudIn.points[j].y = 0;
            laserCloudIn.points[j].z = 0;
        }
    }

    pcl::toPCLPointCloud2(laserCloudIn, cloud_ROI);
    sensor_msgs::PointCloud2 output_ROI; 	//출력할 방식인 PC2 선정 및 이름 output_v 정의
    pcl_conversions::fromPCL(cloud_ROI, output_ROI);
    output_ROI.header.frame_id = "velodyne";
    pub4.publish(output_ROI);

    pcl::PointCloud<pcl::PointXYZ> cloudClusterIn;
    copyPointCloud(laserCloudIn, cloudClusterIn);

    pcl::VoxelGrid<pcl::PointXYZ> vg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_v2 (new pcl::PointCloud<pcl::PointXYZ>);
    vg.setInputCloud (cloudClusterIn.makeShared());
    vg.setLeafSize (0.5f, 0.5f, 0.5f);		//복셀 다운샘플링의 단위 (한 복셀의 크기)
    vg.filter (*cloud_filtered_v2);			
  
    pcl::PCLPointCloud2 cloud_v; 			//복셀화된 클라우드 데이터 구조 선언
    sensor_msgs::PointCloud2 output_v; 		//출력할 방식인 PC2 선정 및 이름 output_v 정의
    pcl::toPCLPointCloud2(*cloud_filtered_v2, cloud_v);
    pcl_conversions::fromPCL(cloud_v, output_v);
    output_v.header.frame_id = "velodyne";
    pub2.publish(output_v);
    
    std::cout << "PointCloud after filtering has: " << cloud_filtered_v2->points.size ()  << " data points." << std::endl;

    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_filtered_v2);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (1); 			// 1m의 포인트와 포인트 간의 간격
    ec.setMinClusterSize (4);				// 한 군집의 최소 포인트 개수
    ec.setMaxClusterSize (40);				// 한 군집의 최대 포인트 개수
    ec.setSearchMethod (tree);				// 검색 방법 : tree 
    ec.setInputCloud (cloud_filtered_v2);	// cloud_filtered_v2에 클러스터링 결과를 입력
    ec.extract (cluster_indices);

    //클러스터링 된 군집의 개수를 리턴한다. cluster_indices.size()
    std::cout << "Number of clusters is equal to " << cluster_indices.size () << std::endl;
	
    pcl::PointCloud<pcl::PointXYZI> TotalCloud; 
    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
    	for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    	{
        	pcl::PointXYZ pt = cloud_filtered_v2->points[*pit];
          	pcl::PointXYZI pt2;
          	pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
          	pt2.intensity = (float)(j + 1);
          	TotalCloud.push_back(pt2);
    	}
    j++;
    }
    
    pcl::PCLPointCloud2 cloud_clustered;
    pcl::toPCLPointCloud2(TotalCloud, cloud_clustered);
    sensor_msgs::PointCloud2 output_clustered; 
    pcl_conversions::fromPCL(cloud_clustered, output_clustered);
    output_clustered.header.frame_id = "velodyne";
    pub5.publish(output_clustered); 
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "input");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/os1_cloud_node/points", 10, cloud_cb); //front ouster
    pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_rotated", 100);
    pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_voxelized", 10);
    pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_passthrough", 100);
    pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_ROI", 10);
    pub5 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_clustered", 1);

    ros::spin();
}