[ROS PCL] 포인트 클라우드 포인트 추출하고 군집화 하기 (차선 추출 및 Euclidean Distance Clustering)

  이전 글에서 포인트 클라우드의 관심영역을 설정하는것까지 완료했습니다. 다음으론 관심대상인 차선을 검출하고 군집화를 통해서 차선의 차량대비 위치를 파악하여 LKAS(Lane Keeping Asistance System)을 구축하기위한 베이스를 구축해보는 과정을 코드를 통해 살펴보도록 하겠습니다. 

[ROS PCL] 포인트 클라우드 ROI(Region of Interest) 설정하기

---

 

[ROS PCL] 포인트 클라우드 포인트 추출하고 군집화 하기 (차선 추출 및 Euclidean Distance Clustering)

[ROS PCL] Getting point data of lane from the point cloud and clustering

 

---

1. 전체 코드

//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
    
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 input(const sensor_msgs::PointCloud2ConstPtr& scan)
{

    //1. Msg to pointcloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*scan,*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);

    std::cout << "Number of clusters is equal to " << cluster_indices.size () << std::endl;
	//클러스터링 된 군집의 개수를 리턴한다. cluster_indices.size()
    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", 100, input); //front ouster
	//pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_rotated", 100);
    pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_voxelized", 100);
    //pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_passthrough", 100);
    pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_ROI", 100);
    pub5 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_clustered", 1);
    
	ros::spin();
}

2. 실행 동영상 

오른쪽: 필터를 통한 차선 추출 / 왼쪽: 추출된 포인트들의 클러스터링 후 색깔 입혀서 시각화하기

 

  결과 동영상 중 한쪽이 rotated 되었네요. 유클리드 거리에 의한 군집화는 계산량이 적은 대신 제대로 군집화가 이뤄지지 않는다는 단점이 있습니다. 따라서 K-MEANS 혹은 DBSCAN 등의 군집화 알고리즘과 비교해 봤던 논문이 있는데, 각각의 장단점을 파악하고 이중의 알고리즘을 통해서 더욱 정확한 군집화를 이룰 수 있을것이라 생각이 듭니다. 본 알고리즘은 완성된것이 아니고 부족한 부분이 많아서 앞으로 어떻게 처리해야할지 아직 잘 모르겠지만, 차선을 제대로 인식할 수 있도록 다른 전처리 과정을 진행하도록 하겠습니다. 

---

<참고 문헌> 

[1] Daddymakers 블로그, daddynkidsmakers.blogspot.com/2015/08/3.html (2020.08.18)