更多干货请关注公众号[3D视觉工坊]~
前言
最近要在ROS下做激光雷达和相机的数据融合,而且要同步,搜了网上一大堆,没有找到特别明确的答案,最终,还是搞出来啦~~~
这里主要是完成雷达和相机同步映射,需要相机的内参和雷达相机标定的外参。关于雷达和相机的标定请参考我的另一篇博文:雷达和相机的联合标定
代码
把要处理的数据声明为类的私有变量,data_fusion()为数据融合函数~,这里相机的更新频率要低于雷达,相机大约为3fps,而雷达为10Hz,所以暂时把data_fusion放在相机的回调函数中进行处理,后期会通过加锁来保证数据的同步性。
#include <ros/ros.h>
#include <std_msgs/String.h>
#include<car_sensor/image_detect.h>
#include<string>
#include <boost/thread.hpp>
#include <sstream>
#include<sensor_msgs/PointCloud2.h>
#include<opencv2/opencv.hpp>
#include<cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
//pcl and pointcloud2 to handle the lidar_point data
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
using namespace std;
using namespace cv;
class SubscribeAndPublish
{
private:
cv::Mat image;
ros::NodeHandle n;
//ros::Publisher pub_;
ros::Subscriber sub_camera;
ros::Subscriber sub_lidar;
//std_msgs::String output;
//int count;
std::vector<string> roi_label;
std::vector<int> roi_leftX;
std::vector<int> roi_leftY;
std::vector<int&