SLAM从入门到精通（3d 点云数据访问）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        3d 点云设备现在汽车上用的很多。之前3d lidar这种高端传感器，只能被少部分智能汽车使用。后来很多国产厂家也开始研发3d lidar之后，它的价格快速下跌下来，部分3d lidar的价格已经降到了几千元左右，实用性一下子就提升上来了。不管用它来做slam，还是用来检测物体、识别物体、避障检测，都是很方便的。所以，对于slam的同学来说，除了轮速编码器、imu、camera、单线lidar这些传统传感器之外，对多线lidar、深度摄像机一定要多加关注，它们肯定是未来发展的方向。

        和图像主要采用opencv库一样，目前3d lidar数据主要采用的库是pcl。

1、编写pc_node.cpp

 
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
 
void PointcloudCB(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	pcl::PointCloud<pcl::PointXYZ> pointCloudIn;
	pcl::fromROSMsg(*msg, pointCloudIn);
 
	int cloudSize = pointCloudIn.points.size();
	for(int i = 0; i < cloudSize; i++)
	{
		ROS_INFO("[i=%d] (%.2f, %.2f, %.2f)",
			i,
			pointCloudIn.points[i].x,
			pointCloudIn.points[i].y,
			pointCloudIn.points[i].z);
	}
}
 
int main(int argc, char* argv[])
{
	ros::init(argc, argv, "pc_node");
	ROS_WARN("pc_node start");
 
	ros::NodeHandle nh;
	ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 1, PointcloudCB);
	ros::spin();
	return 0;
}

        代码不复杂。首先我们创建一个pc_sub订阅器，它订阅了话题/kinect2/sd/points，并且为这个话题准备了回调函数PointcloudCB。在这个回调函数里面，代码对收到的点云数据进行了打印，分别显示它们的x/y/z浮点数值。一般来说，点云还会有一个反光强度的值，但这里没有提及。

        多线激光雷达和单线激光雷达很相似，只不过多了一个z方向的数值。也正是因为这个z数值，让我们知道了周围环境的深度信息，这也是它最有价值的地方。

2、准备CMakeLists.txt

        因为pc_node.cpp依赖于pcl库，所以这里有两件事情要解决。第一件事情，查找一下当前的依赖库里面有没有pcl，

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS message_generation roscpp rospy std_msgs genmsg tf cv_bridge pcl_ros)
find_package(PCL REQUIRED)

        第二件事情，就是添加编译规则，告诉CMakeLists.txt，pc_node.cpp应该怎么编译，

add_executable(pc_node src/pc_node.cpp)
target_link_libraries(pc_node ${catkin_LIBRARIES})
add_dependencies(pc_node beginner_tutorials_generate_messages_cpp)

3、catkin_make编译

        pc_node.cpp和CMakeLists.txt都准备好了，那么就可以开始编译了。编译的方法，就是在catkin_ws目录下面直接输入catkin_make即可。

4、测试和验证

        测试的方法其实和camera是一样的。第一步，需要一个仿真环境，输入roslaunch wpr_simulation wpb_pointcloud.launch即可。

        仿真环境准备好之后，第二步就可以输入rosrun beginner_tutorials pc_node，这个时候会看到很多的数据打印。这些数据就是看到的3d数据。

[ INFO] [1696920724.029184374, 77.884000000]: [i=16505] (-0.82, -1.09, 2.22)
[ INFO] [1696920724.029235237, 77.884000000]: [i=16506] (-0.81, -1.09, 2.22)
[ INFO] [1696920724.029306573, 77.884000000]: [i=16507] (-0.81, -1.09, 2.22)
[ INFO] [1696920724.029391593, 77.884000000]: [i=16508] (-0.80, -1.09, 2.22)
[ INFO] [1696920724.029448090, 77.884000000]: [i=16509] (-0.79, -1.09, 2.22)
[ INFO] [1696920724.029506356, 77.884000000]: [i=16510] (-0.79, -1.10, 2.24)
[ INFO] [1696920724.029556718, 77.884000000]: [i=16511] (-0.79, -1.11, 2.25)
[ INFO] [1696920724.029606507, 77.884000000]: [i=16512] (-0.79, -1.12, 2.27)
[ INFO] [1696920724.029656374, 77.884000000]: [i=16513] (-0.79, -1.12, 2.29)
[ INFO] [1696920724.029706286, 77.884000000]: [i=16514] (-0.79, -1.13, 2.31)
[ INFO] [1696920724.029756426, 77.884000000]: [i=16515] (-0.79, -1.14, 2.33)
[ INFO] [1696920724.029805764, 77.884000000]: [i=16516] (-0.79, -1.15, 2.35)
[ INFO] [1696920724.029855296, 77.884000000]: [i=16517] (-0.79, -1.16, 2.37)
[ INFO] [1696920724.029904895, 77.884000000]: [i=16518] (-0.79, -1.17, 2.38)
[ INFO] [1696920724.029954521, 77.884000000]: [i=16519] (-0.79, -1.18, 2.40)
[ INFO] [1696920724.030003954, 77.884000000]: [i=16520] (-0.79, -1.19, 2.42)
[ INFO] [1696920724.030053707, 77.884000000]: [i=16521] (-0.79, -1.20, 2.45)
[ INFO] [1696920724.030103470, 77.884000000]: [i=16522] (-0.79, -1.21, 2.47)
[ INFO] [1696920724.030152928, 77.884000000]: [i=16523] (-0.79, -1.22, 2.49)
[ INFO] [1696920724.030202438, 77.884000000]: [i=16524] (-0.79, -1.23, 2.51)
[ INFO] [1696920724.030251816, 77.884000000]: [i=16525] (-0.79, -1.24, 2.53)

5、后续的工作

        拿到点云数据只是第一步，后续可以通过x/y/z限制、滤波、分割、识别、统计等方法，估算出物体的具体位置。拿到这些位置信息之后，就可以进一步通知机器人去进行后续任务的处理，这个是之前传感器无法实现的效果。

        当然现在用3d lidar做slam的开源代码也很多，特别是室外，靠gmapping根本是不可能的。只能靠3d lidar、gps、imu这些传感器去处理，也是未来发展一个很重要的方向。