ROS PCL : Voxelize 포인트 클라우드 라이브러리를 이용하여 voxelize 하기

ROS 환경에 익숙해지기 위해 다양한 feature들 ex) rosbag, rospackage, catkin 등 여러 환경적인 부분들을 직접 구성해보며 익히는 것을 목표로 소규모 프로젝트를 진행한다.

 

 

최종 전체 프로그램 구동 환경 이미지

 

 

---

소규모 프로젝트 이름 : 포인트 클라우드 라이브러리를 이용하여 voxelize 하기

소규모 프로젝트의 목표 : 처음부터 시작하여 본 프로그램을 만드는것을 목표로 한다. rosbag 파일을 이용하여 들어오는 데이터들을 voxelize 하여 down sampling을 진행하고 원본 데이터와 voxelized 된 데이터를 시각화하여 비교하는 roslaunch파일을 만든다.

 

프로젝트를 위해 필요한 준비물 : LiDAR rosbag 파일(구글에서 쉽게 벨로다인라이다 파일을 구할 수 있음), ros melodic이 설치되어있는 ubuntu 노트북 https://saint-swithins-day.tistory.com/22?category=867801 참조

ROS를 위한 UBUNTU 18.04 작업환경 구축

 

프로그램 목표를 위한 순서

1.  rosbag파일을 실행하여 publish 한다.
2.  pkg를 만들고 package.xml에 환경설정을 한 후 pkg 안의 src 폴더 안의 example.cpp를 만들어준다.
3.  example.cpp에 기본 실행 코드를 복사한 후 저장한다.
4.  cmakelist.txt 파일에 <example.cpp>소스파일을 추가해준다.
5.  example.cpp파일을 수정하여 원본데이터를 받는다.
6.  voxelized된 데이터를 publish 하고 원본 파일을 다시 publish 하는 병렬구조의 코드를 작성한다.
7.  roslaunch를 통해 두 개의 rviz 와 rosbag파일을 실행 그리고 voxelize 코드를 실행하는 것을 한 번에 할 수 있도록 만든다.

---

1. rosbag 파일 실행하기

$ rosbag play nsh_indoor_outdoor.bag -l

데이터의 확인을 위해 rosbag으로 무한히 실행시켜 준다. 본 명령으로 인하여 rosbag에 담긴 data들이 계속 publish 될 것이다. 

현재 publish 되고 있는 토픽들을 확인하기 위해서는 rostopic list 명령어를 통해 확인해보도록 하자.

 

2. pkg를 만들고 package.xml에 환경설정을 한 후 pkg 안의 src 폴더 안의 example.cpp를 만들어준다.

$ catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs 

package가 생성되면 package.xml 파일을 열어 본 내용을 추가한다. 

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>

 

 

 

 

사진과 같이 src폴더에 진입하여 cpp파일을 만들어준다.

 

3. example.cpp에 기본 실행 코드를 복사한 후 저장한다.

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;

  // Do data processing here...
  output = *input;

  // Publish the data.
  pub.publish (output);
}

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 ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
  ros::spin ();
}

영어로 skeleton code 라고 하는데, 뼈대 코드에 대한 설명은 주석을 보기 바란다. 

 

4. cmakelist.txt 파일에 <example.cpp> 소스파일을 추가해준다.

 

 

 

 

add_executable(example src/example.cpp) 
target_link_libraries(example ${catkin_LIBRARIES})

위의 코드를 복사후 붙여 넣기 한다.

5. example.cpp파일을 수정하여 원본 데이터를 받는 코드를 작성한다.

6. voxelized된 데이터를 publish 하고 원본 파일을 다시 publish 하는 병렬구조의 코드를 작성한다.

수정 내용은 주석을 참고하기 바란다. 5번 관련 내용이 pub1 이고 6번 관련 내용이 pub2이다.

#include <iostream>
#include <cmath>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.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 <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>

using namespace std;

ros::Publisher pub1;
ros::Publisher pub2;

#define VPoint velodyne_pointcloud::PointXYZIR
#define Point2 pcl::PointXYZI

void input(const sensor_msgs::PointCloud2ConstPtr& scan)
{
// Msg to pointcloud
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*scan,*cloud);

//ROS로 다시 바꿔주기위해서 필요한 작업
pcl::PCLPointCloud2 cloud_p;

pcl::toPCLPointCloud2(*cloud, cloud_p);

sensor_msgs::PointCloud2 output;
pcl_conversions::fromPCL(cloud_p, output);


output.header.frame_id = "velodyne";
pub1.publish(output);
//ROS_INFO("published it.");


pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PCLPointCloud2 cloud_v;
pcl::VoxelGrid<pcl::PointXYZI> sor;

sor.setInputCloud (cloud);
sor.setLeafSize (0.5f, 0.5f, 0.5f);
sor.filter (*cloud_filtered); 
pcl::toPCLPointCloud2(*cloud_filtered, cloud_v);

sensor_msgs::PointCloud2 output_v;
pcl_conversions::fromPCL(cloud_v, output_v);
output_v.header.frame_id = "velodyne";
pub2.publish(output_v);

}


int main(int argc, char** argv)
{
	ros::init(argc, argv, "input");
	ros::NodeHandle nh;
	ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 100, input); //front ouster
	pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_voxelized", 100);
	ros::spin();
}

 

다음은 실행 동영상 파일이다. 오른쪽이 원본 파일 왼쪽이 voxelized 된 파일이다. 영상에서 보면 알겠지만, 일정 영역 안의 포인트들을 하나로 합치는 downsampling 알고리즘이다. 다른 맵매칭이나 여러 알고리즘들을 구동할 때 사용하는 데이터량이 줄면서 계산량이 줄어 computing power 측면에서 유리하지만, 반대로 물체들이 가질 수 있는 특징점이 줄어들어서 matching 기법들이나 deep-learning을 통한 객체 분류에서 disadvantage를 가질 수 있다.

 

오늘은 여기까지

 

<참고 문헌 및 출처>

http://wiki.ros.org/pcl/Tutorials

pcl/Tutorials - ROS Wiki