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

  이전 글에서 OS-1 rosbag 파일을 서브스크라이브 하면서 회전변환행렬을 통한 Z축 Rotation을 해주었었습니다. 다음은 센서의 각도를 통해서 ROI를 설정하여 관심영역을 설정하는 방법을 알아보도록 하겠습니다. 여러가지 방법들이 있겠지만, 본 포스팅에서는 pcl::PointCloud<pcl::PointXYZI>의 변수형에서 points클래스에 접근하여 x포인트와 y포인트의 각도를 계산하여 z축 기준으로의 각도내의 범위의 포인트들을 입력받게 하고 나머지는 0을 대입시키는 형식의 코드입니다. 더 나은 이해를 위해서 이전글의 링크를 달아둡니다. 참고하길 바랍니다. 

https://saint-swithins-day.tistory.com/56

[ROS PCL] 포인트 클라우드 회전변환행렬을 통한 Rotation 하기 : Using a matrix to transform a point cloud

 

---

 

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

 

[ROS PCL] Setting ROI of the point cloud

---

1. 전체 코드 

#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>

double ROI_theta(double x, double y);
using namespace std;

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

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 설정
    pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
    pcl::fromROSMsg(output,laserCloudIn); 
    pcl::PCLPointCloud2 cloud_ROI;

    
    for(unsigned int j=0; j<laserCloudIn.points.size(); j++)
	{
        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;
        }
    }

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

}

2. 코드 분석

2.1 데이터 형

 

데이터 형의 변화 흐름을 보고자 한다. 어떤 데이터 형이 어떻게 변환되어가는지 정리해 보겠습니다.

1. sensor_msgs::PointCloud2ConstPtr&-> pcl::PointCloud<pcl::PointXYZI>::Ptr [ fromROSMsg(*,*); ]

 

2. pcl::PointCloud<pcl::PointXYZI>::Ptr -> pcl::PointCloud<pcl::PointXYZI>::Ptr [ pcl::transformPointCloud(*,*,*);]

 

3. pcl::PointCloud<pcl::PointXYZI>::Ptr -> pcl::PCLPointCloud2 [

pcl::toPCLPointCloud2]

4. pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2 [ pcl_conversions::fromPCL(*,*); ]

5. sensor_msgs::PointCloud2 -> pcl::PointCloud<pcl::PointXYZI> [ fromROSMsg(*,*); ]

 

6. PointCloud<pcl::PointXYZI> -> pcl::PCLPointCloud2 [pcl_conversions::fromPCL(*,*); ]

 

 

 

7. pub4.publish(pcl::PCLPointCloud2);

 

 

형변환 관련 함수들

 

 

fromROSMsg(*,*);

: sersor_msgs 를 pcl의 변수형으로 알아서 변환해준다.

pcl::toPCLPointCloud2(*,*);

: sersor_msgs의 형중 하나인 PointCloud2로 형변환 시켜주는 함수.

pcl_conversions::fromPCL(*,*);

: 입력값과 출력값을 입력해주면 알아서 형변환이 이루어짐을 알 수 있었다.

 

2.2 코드 및 함수 

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;
}

x 와 y 포인트간의 원점에서의 각도를 구하는 함수.

 

    for(unsigned int j=0; j<laserCloudIn.points.size(); j++)
	{
        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;
        }
    }

구한 각도가 조건에 해당하지 않으면 모두 0을 대입시켜 데이터를 처리하는 함수.

 

3. 결론

 

 

 

원하는대로 잘 설정되어 나왔습니다.

---

오늘은 조금 힘이 드네요. 아무쪼록 원하는 결과가 잘 나와서 다행입니다. 다음에는 도로 위 객체들을 모두 지워버리거나 바닥을 제거하는 예제를 해보도록 하겠습니다.