ROS机器人程序设计(原书第2版)补充资料 (陆) 第六章 点云 PCL
书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用。
RGBD深度摄像头传感器最常用的数据存储,处理和显示方式就是点云。
推荐查阅-PCL官网:http://www.pointclouds.org/
1. http://wiki.ros.org/pcl_ros 2. http://wiki.ros.org/pcl
补充阅读:
1 http://blog.csdn.net/zhangrelay/article/details/50053733
2 http://blog.csdn.net/zhangrelay/article/details/50240935
第163页:
简介点云。
第163-165页:
理解点云,包括类型,算法和接口等。
第166-190页:
学习在ROS使用PCL,包括创建点云,可视化,滤波,缩减采样,配准,匹配,分区,分割等。
第191页:
本章小结。
思考与巩固:
1 使用深度摄像头采集环境信息,并用点云显示,用本章提及的方法进行处理。
2 在ROSwiki上查阅点云相关功能包并完成编译使用。
附:
目录
$ catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs
Then, modify the package.xml to add
<build_depend>libpcl-all-dev</build_depend>
<run_depend>libpcl-all</run_depend>
Create an empty file called src/example.cpp and paste the following code in it:
1 #include <ros/ros.h>
2 // PCL specific includes
3 #include <sensor_msgs/PointCloud2.h>
4 #include <pcl_conversions/pcl_conversions.h>
5 #include <pcl/point_cloud.h>
6 #include <pcl/point_types.h>
7
8 ros::Publisher pub;
9
10 void
11 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
12 {
13 // Create a container for the data.
14 sensor_msgs::PointCloud2 output;
15
16 // Do data processing here...
17 output = *input;
18
19 // Publish the data.
20 pub.publish (output);
21 }
22
23 int
24 main (int argc, char** argv)
25 {
26 // Initialize ROS
27 ros::init (argc, argv, "my_pcl_tutorial");
28 ros::NodeHandle nh;
29
30 // Create a ROS subscriber for the input point cloud
31 ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
32
33 // Create a ROS publisher for the output point cloud
34 pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
35
36 // Spin
37 ros::spin ();
38 }
The code above does nothing but initialize ROS, create a subscriber and a publisher for PointCloud2 data.
Edit the CMakeLists.txt file in your newly created package and add:
add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})
PCL has about four different ways of representing point cloud data, so it can get a bit confusing, but we'll try to keep it simple for you. The types are:
In the following code examples we will focus on the ROS message (sensor_msgs::PointCloud2) and the standard PCL data structure (pcl::PointCloud<T>). However, you should also note that pcl::PCLPointCloud2 is an important and useful type as well: you can directly subscribe to nodes using that type and it will be automatically serialized to/from the sensor_msgs type. See this example to try PCLPointCloud2 yourself.
If you'd like to save yourself some copying and pasting, you can download the source file for this example here. Just remember to rename the file to example.cpp or edit your CMakeLists.txt to match.
The sensor_msgs/PointCloud2 format was designed as a ROS message, and is the preferred choice for ROS applications. In the following example, we downsample a PointCloud2 structure using a 3D grid, thus reducing the number of points in the input dataset considerably.
To add this capability to the code skeleton above, perform the following steps:
1 // Create the filtering object
2 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
3 sor.setInputCloud (cloud);
4 sor.setLeafSize (0.01, 0.01, 0.01);
5 sor.filter (*cloud_filtered);
1 #include <pcl/filters/voxel_grid.h>
2
3 ...
4
5 void
6 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
7 {
8 // Container for original & filtered data
9 pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
10 pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
11 pcl::PCLPointCloud2 cloud_filtered;
12
13 // Convert to PCL data type
14 pcl_conversions::toPCL(*cloud_msg, *cloud);
15
16 // Perform the actual filtering
17 pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
18 sor.setInputCloud (cloudPtr);
19 sor.setLeafSize (0.1, 0.1, 0.1);
20 sor.filter (cloud_filtered);
21
22 // Convert to ROS data type
23 sensor_msgs::PointCloud2 output;
24 pcl_conversions::fromPCL(cloud_filtered, output);
25
26 // Publish the data
27 pub.publish (output);
28 }
Note
Since different tutorials will often use different variable names for their inputs and outputs, remember that you may need to modify the code slightly when integrating the tutorial code into your own ROS node. In this case, notice that we had to change the variable name input to cloud, and output to cloud_filtered in order to match up with the code from the tutorial we copied.
Note that there is a slight inefficiency here. The fromPCL can be replaced with moveFromPCL to prevent copying the entire (filtered) point cloud. However, the toPCL call cannot be optimized in this way since the original input is const.
Save the output file then build:
$ cd %TOP_DIR_YOUR_CATKIN_HOME%
$ catkin_make
Then run:
$ rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2
or, if you're running an OpenNI-compatible depth sensor, try:
$ roslaunch openni_launch openni.launch
$ rosrun my_pcl_tutorial example input:=/camera/depth/points
You can visualize the result by running RViz:
$ rosrun rviz rviz
and adding a "PointCloud2" display. Select camera_depth_frame for the Fixed Frame (or whatever frame is appropriate for your sensor) and select output for the PointCloud2 topic. You should see a highly downsampled point cloud. For comparison, you can view the /camera/depth/points topic and see how much it has been downsampled.
As with the previous example, if you want to skip a few steps, you can download the source file for this example here.
The pcl/PointCloud<T> format represents the internal PCL point cloud format. For modularity and efficiency reasons, the format is templated on the point type, and PCL provides a list of templated common types which are SSE aligned. In the following example, we estimate the planar coefficients of the largest plane found in a scene.
To add this capability to the code skeleton above, perform the following steps:
1 pcl::ModelCoefficients coefficients;
2 pcl::PointIndices inliers;
3 // Create the segmentation object
4 pcl::SACSegmentation<pcl::PointXYZ> seg;
5 // Optional
6 seg.setOptimizeCoefficients (true);
7 // Mandatory
8 seg.setModelType (pcl::SACMODEL_PLANE);
9 seg.setMethodType (pcl::SAC_RANSAC);
10 seg.setDistanceThreshold (0.01);
11
12 seg.setInputCloud (cloud.makeShared ());
13 seg.segment (inliers, coefficients);
Copy these lines, in the code snippet above, by modifying the callback function as follows:
1 #include <pcl/sample_consensus/model_types.h>
2 #include <pcl/sample_consensus/method_types.h>
3 #include <pcl/segmentation/sac_segmentation.h>
4
5 ...
6
7 void
8 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
9 {
10 // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
11 pcl::PointCloud<pcl::PointXYZ> cloud;
12 pcl::fromROSMsg (*input, cloud);
13
14 pcl::ModelCoefficients coefficients;
15 pcl::PointIndices inliers;
16 // Create the segmentation object
17 pcl::SACSegmentation<pcl::PointXYZ> seg;
18 // Optional
19 seg.setOptimizeCoefficients (true);
20 // Mandatory
21 seg.setModelType (pcl::SACMODEL_PLANE);
22 seg.setMethodType (pcl::SAC_RANSAC);
23 seg.setDistanceThreshold (0.01);
24
25 seg.setInputCloud (cloud.makeShared ());
26 seg.segment (inliers, coefficients);
27
28 // Publish the model coefficients
29 pcl_msgs::ModelCoefficients ros_coefficients;
30 pcl_conversions::fromPCL(coefficients, ros_coefficients);
31 pub.publish (ros_coefficients);
32 }
Notice that we added two conversion steps: from sensor_msgs/PointCloud2 to pcl/PointCloud<T>and from pcl::ModelCoefficients to pcl_msgs::ModelCoefficients. We also changed the variable that we publish from output to coefficients.
In addition, since we're now publishing the planar model coefficients found rather than point cloud data, we have to change our publisher type from:
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
to:
// Create a ROS publisher for the output model coefficients
pub = nh.advertise<pcl_msgs::ModelCoefficients> ("output", 1);
Save the output file, then compile and run the code above:
$ rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2
or, if you're running an OpenNI-compatible depth sensor, try:
$ rosrun my_pcl_tutorial example input:=/camera/depth/points
See the output with
$ rostopic echo output
-End-