1. 개요
이전 글에서 객체 인식을 위해, PCL (point cloud library)를 사용하였다. PCL은 기본적인 3차원 포인트 클라우드 처리 기능이 포함되어 있다.
PCL (point cloud library)
물론, PCL이 모든 솔류션을 제공해주지는 않는다. 하지만, 기본적인 3차원 포인트 클라우드 필터링, 세그먼테이션 등의 기능을 제공해 줌으로, 이를 개발할 시간에, 다른 알고리즘을 구현할 수 있는 시간을 벌 수 있다.
2. ROS PCL
1. 메시지 유형
ROS에서 제공하는 PCL 토픽은 다음과 같은 데이터 구조를 사용한다.
- sensor_msgs::PointCloud - 첫번째 채택된 ROS의 포인트 클라우드 메시지 구조이다. x, y, z 포인트 (모두 float형)를 포함하고 있다. 멀티 채널을 지원하며, 각 채널은 string 이름으로 구분한다. 이 메시지는 point_cloud_mapping 패키지에서 생성되며, ROS 1.0 이전 포맷 기반으로 데이터 가시화를 지원한다.
- sensor_msgs::PointCloud2 - 새로 개선된 ROS 포인트 클라우드 메시지이다. n-D 데이터를 표현할 수 있다. 포인트 값은 어떤 데이터 유형도 가능하다. 밀도, 높이, 폭, 2차원 이미지 등을 포함할 수 있다. 관련 구조는 PCL_March_2010.pdf 혹은 pcl_icra2010.pdf를 참고하라.
- pcl::PointCloud<T> - PCL라이브러리의 핵심 포인트 클라우드 클래스이다. point_types.h 에 정의되어 있는 템플릿 클래스이다. 이 타입은 PointCloud2 메시지 타입과 유사하며, 헤더를 포함한다. 메시지와 포인트 템플릿 간 변환은 양방향(sensor_msgs::convertPointCloud2ToPointCloud and sensor_msgs::convertPointCloudToPointCloud2.)이며, 포인트 클라우드를 처리할 때는 이 유형이 더 나을 수 있다.
2. PointCloud2 필드 이름
필드 이름과 설명은 다음과 같다.
- x - the X Cartesian coordinate of a point (float32)
- y - the Y Cartesian coordinate of a point (float32)
- z - the Z Cartesian coordinate of a point (float32)
- rgb - the RGB (24-bit packed) color at a point (uint32)
- rgba - the A-RGB (32-bit packed) color at a point (uint32), the field name is unfortunately misleading
- normal_x - the first component of the normal direction vector at a point (float32)
- normal_y - the second component of the normal direction vector at a point (float32)
- normal_z - the third component of the normal direction vector at a point (float32)
- curvature - the surface curvature change estimate at a point (float32)
- j1 - the first moment invariant at a point (float32)
- j2 - the second moment invariant at a point (float32)
- j3 - the third moment invariant at a point (float32)
- boundary_point - the boundary property of a point (e.g., set to 1 if the point is lying on a boundary) (bool)
- principal_curvature_x - the first component of the direction of the principal curvature at a point (float32)
- principal_curvature_y - the second component of the direction of the principal curvature at a point (float32)
- principal_curvature_z - the third component of the direction of the principal curvature at a point (float32)
- pc1 - the magnitude of the first component of the principal curvature at a point (float32)
- pc2 - the magnitude of the second component of the principal curvature at a point (float32)
- pfh - the Point Feature Histogram (PFH) feature signature at a point (float32[])
- fpfh - the Fast Point Feature Histogram (FPFH) feature signature at a point (float32[])
- ...
3. 다른 유형으로 퍼블리쉬
다음은 다른 유형으로 퍼블리쉬하는 예이다.
ros::NodeHandle nh;
std::string topic = nh.resolveName("point_cloud");
uint32_t queue_size = 1;
// get your point cloud message from somewhere
sensor_msgs::PointCloud2 cloud_msg;
// to advertise you can do it like this (as above):
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>(topic, queue_size);
/// and publish the message
pub.publish(cloud_msg);
// Need to include the pcl ros utilities
#include "pcl_ros/point_cloud.h"
// you have an object already, eg with pcl::PointXYZRGB points
pcl::PointCloud<pcl::PointXYZRGB> cloud;
// create a templated publisher
ros::Publisher pub = nh.advertise<pcl::PointCloud<pcl::PointXYZRGB> > (topic, queue_size);
// and just publish the object directly
pub.publish(cloud);
3. 다중 객체 인식
여기서 부터는 ROS에서 관련 예제가 없으므로, 많은 시행 착오를 반복하면서 길을 찾아나가야 한다. 우선 몇몇 레퍼런스를 확인해 보았다. 다음 레퍼런스는 평면 추출과 관련된 기본적인 내용이 포함되어 있다. 다만, ROS에서는 메시지 처리를 위해 보완되어야 하는 부분이 있다.
다음 코드를 이전 글에서 개발했던 my_pcl_tutorial_node 소스로 덮어쓴다.
다음 코드를 이전 글에서 개발했던 my_pcl_tutorial_node 소스로 덮어쓴다.
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
ros::Publisher pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, cloud);
pcl::ModelCoefficients coefficients;
pcl::PointIndices inliers;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud.makeShared ());
seg.segment (inliers, coefficients);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
ROS_INFO("extract the inliers");
pcl::PointCloud<pcl::PointXYZ> in_cloud;
pcl::IndicesPtr indices(&inliers.indices);
extract.setInputCloud (cloud.makeShared ());
extract.setIndices (indices);
extract.setNegative (false);
extract.filter (in_cloud);
// Convert To ROS data type
ROS_INFO("convert to ros data type");
pcl::PCLPointCloud2 cloud_p;
pcl::toPCLPointCloud2(in_cloud, cloud_p);
sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_p, output);
pub.publish(output);
ROS_INFO("published it.");
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);
// Create a ROS publisher for the output model coefficients
pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
// Spin
ros::spin ();
}
실행해 보면, 생각했던 결과가 나오지 않는다. 메모리 free() 시 오류가 난다. 메시지 유형과 PCL의 유형 간 변환 시 고려해야 하는 분이 있어 보인다. 구글링을 통해, 스마트포인트로 인한 메모리 릭(pcl::IndicesPtr indices(&inliers.indices) 부분)을 삭제/수정하고 다시 실행해 보았다.
다음과 같이 잘 추출된다.
이제 다중 평면을 추출하기 위해, 다음과 같이 코딩해 보았다.
#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
ros::Publisher pub;
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (*input, cloud);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
pcl::PointCloud<pcl::PointXYZI> TotalCloud;
sensor_msgs::PointCloud2 output;
for(int i = 0; i < 8; i++)
{
if(cloud.size() < 10000)
break;
seg.setInputCloud (cloud.makeShared ());
pcl::ModelCoefficients coefficients;
// pcl::PointIndices inliers;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
seg.segment (*inliers, coefficients);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
ROS_INFO("extract the inliers");
pcl::PointCloud<pcl::PointXYZ> in_cloud;
extract.setInputCloud (cloud.makeShared ());
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (in_cloud);
extract.setNegative (true);
extract.filter (cloud);
pcl::PointCloud<pcl::PointXYZ>::iterator index = in_cloud.begin();
for(; index != in_cloud.end(); index++)
{
pcl::PointXYZ pt = *index;
pcl::PointXYZI pt2;
pt2.x = pt.x, pt2.y = pt.y, pt2.z = pt.z;
pt2.intensity = (float)(i + 1);
TotalCloud.push_back(pt2);
}
ROS_INFO("%d. remained point cloud = %d", i, (int)cloud.size());
}
// Convert To ROS data type
pcl::PCLPointCloud2 cloud_p;
pcl::toPCLPointCloud2(TotalCloud, cloud_p);
pcl_conversions::fromPCL(cloud_p, output);
output.header.frame_id = "camera_depth_optical_frame";
pub.publish(output);
ROS_INFO("published it.");
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "my_pcl_tutorial");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("camera/depth/points", 1, cloud_cb);
// Create a ROS publisher for the output model coefficients
// pub = nh.advertise<pcl_msgs::ModelCoefficients> ("pclplaneoutput", 1);
pub = nh.advertise<sensor_msgs::PointCloud2> ("pclplaneoutput", 1);
// Spin
ros::spin ();
}
그리고, 빌드/실행해 보면, 다음과 같이 다중으로 평면 객체가 잘 추출되는 것을 확인할 수 있다.
4. 결론
PCL을 사용할 때, ROS 유형간의 변환에 주의할 필요가 있다. 이 부분의 솔류션을 알아내기 위해 약간 삽질이 있었는 데, PCL에서 extract하는 부분의 객체 유형을 달리해서 해결했다. 또한, PCL처리로 얻은 포인트 클라우드를 ROS유형으로 변환할 때, frame_id가 제대로 설정안되는 경우가 있다. 이 경우에는 수동으로 설정해 주어야 한다.
또 다른 주의점은 PCL은 포인터 레퍼런스를 관리하기 위해, 스마트 포인트 인자를 받도록 되어 있다. 이 부분을 잘 못 사용하면 메모리릭이 발생하거나, 시스템 폴트가 생긴다. 이런 경우, 찾아내기가 어려운데, ROS_INFO() 매크로와 소스 리마크를 잘 활용해 코드 하나 하나씩 확인해 보면 찾을 수 있다.
이 모든 조합을 확인하고, 솔류션 획득까지 걸린 시행착오와 코딩에 걸린 시간은 대략 5시간이다. 하지만, 무한삽질도 아닌 이 정도손실로 ROS에 불평하면 메이커라 말할 자격이 없을 것이다(스스로를 위로해 본다. 헐~).
ROS와 PCL 기반으로 다중 객체를 추출해 보고 나니 더욱 더 욕심이 생긴다. 다음 단계는 느린 추출 속도를 보완하기 위해 GPU를 고려해 보고, 더 다양한 형상을 추출해 본다. 고지를 향해서~
