激光雷达与相机融合（五）-------ros实时版点云投影到图像平面

基于第一部分在单帧中将点云投影到图像的基础上，现将上述代码改写为ros实时处理的版本。以后还可以在此基础上进行一步一步的扩展。将原先的离线代码改写为ros在线版本，需要将原本的代码写成package的形式，并配置好相应的依赖包、环境等，处理好数据接口。

1.程序包结构

本部分对数据处理较为简单，整个包仅包括两个c++文件，project.cpp和project.h文件，另外还有一个配置文件initial_params.txt，存放在cfg文件夹下。配置文件的内容如下，可根据需求修改：

/kitti/camera_gray_left/image_raw  //节点接收的图片话题
/kitti/velo/pointcloud				//点云话题
9.999239e-01 9.837760e-03 -7.445048e-03 0.0
-9.869795e-03 9.999421e-01 -4.278459e-03 0.0
7.402527e-03 4.351614e-03 9.999631e-01 0.0
0 0 0 1									//相机与相机外参矩阵
7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00
0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00
0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 //相机内参矩阵
7.533745e-03 -9.999714e-01 -6.166020e-04 -4.069766e-03
1.480249e-02 7.280733e-04 -9.998902e-01 -7.631618e-02
9.998621e-01 7.523790e-03 1.480755e-02 -2.717806e-01
0 0 0 1											//相机与激光雷达之间的外参矩阵

此处应用的标定参数是kitti数据集的标定参数，最后是将点云投影到左侧灰度摄像头图像平面上。关于标定参数的介绍参照之前的博客。

2.数据接口

接收数据
利用 message_filters类定义了图像和点云数据的接受者，并利用时间同步的函数 message_filters::Synchronizer 对两者进行时间同步过滤，只有当点云和图像的时间戳相同才会被接收进行处理。

 message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, i_params.camera_topic, 5);
 message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh,i_params.lidar_topic, 5);
 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
 message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), image_sub, pcl_sub);

处理数据
接收到数据后调用回调函数projector::projection_callback进行投影处理
处理流程和离线是一样的，只是将数据输入与输出和ros连接起来了。最后投影的结果转换为图片数据利用ros节点发送出去。

 sync.registerCallback(boost::bind(&projector::projection_callback, this, _1, _2)