포인트 클라우드 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);
}
코드 분석
데이터 형의 변화 흐름을 보고자 한다. 어떤 데이터 형이 어떻게 변환되어가는지 정리해 보겠다.
-
sensor_msgs::PointCloud2ConstPtr&-> pcl::PointCloud<pcl::PointXYZI>::Ptr [ fromROSMsg(,); ]
-
pcl::PointCloud<pcl::PointXYZI>::Ptr -> pcl::PointCloud<pcl::PointXYZI>::Ptr [ pcl::transformPointCloud(,,*);]
-
pcl::PointCloud<pcl::PointXYZI>::Ptr -> pcl::PCLPointCloud2 [
pcl::toPCLPointCloud2]
-
pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2 [ pcl_conversions::fromPCL(,); ]
-
sensor_msgs::PointCloud2 -> pcl::PointCloud<pcl::PointXYZI> [ fromROSMsg(,); ]
-
PointCloud<pcl::PointXYZI> -> pcl::PCLPointCloud2 [pcl_conversions::fromPCL(,); ]
-
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;
}
}
Author And Source
이 문제에 관하여(포인트 클라우드 ROI 설정), 우리는 이곳에서 더 많은 자료를 발견하고 링크를 클릭하여 보았다 https://velog.io/@qaszx1004/포인트-클라우드-ROI-설정저자 귀속: 원작자 정보가 원작자 URL에 포함되어 있으며 저작권은 원작자 소유입니다.
우수한 개발자 콘텐츠 발견에 전념 (Collection and Share based on the CC Protocol.)