포인트 클라우드 ROI 설정

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

}

코드 분석

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

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(,);
  : 입력값과 출력값을 입력해주면 알아서 형변환이 이루어짐을 알 수 있었다.

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

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

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