雷达相机融合（四）--点云着色

源码一

//*************************************************************************************************
//         融合  image_fire  /cloud_cut  发布：image_color_cloud
//    source ~/catkin_ws/devel/setup.bash &&  rosrun my_lidar_cam_fusion lidar_cam_fusion
//
//****************************************************************************************************#include <ros/ros.h>
#include <boost/thread.hpp>
#include <iomanip>
#include <thread>
#include <mutex>
#include <unistd.h>#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>using namespace sensor_msgs;
using namespace std;
using namespace cv;class lidar_cam_fusion
{public:lidar_cam_fusion(){ROS_INFO_STREAM("Image and dense point cloud fusion, Publisher: image_color_cloud");M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);cout << "M=" << M << endl;RT = (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );cout << "RT=" << RT << endl;row = 480; //行col = 640;colorCloud_pub = n.advertise<sensor_msgs::PointCloud2>("image_color_cloud", 1);//着色点云发布//  points_raw   image_raw cloud_cut  /image_firesub1 = n.subscribe<sensor_msgs::PointCloud2>("cloud_cut", 10, &lidar_cam_fusion::myCallback1,this); //,ros::TransportHints().tcpNoDelay());sub2 = n.subscribe<sensor_msgs::Image>("image_fire", 1000, &lidar_cam_fusion::myCallback2,this );}void myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);void myCallback2(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg);bool cloud_Queue(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);void syn_image_lidar();bool get_image();void cloud_Color(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);void Publishe_msg(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color);private:ros::NodeHandle n;ros::Subscriber sub1;ros::Subscriber sub2;ros::Publisher colorCloud_pub;std::mutex cloudLock;std::mutex imageLock;std::deque<sensor_msgs::Image> imageQueue;std::deque<sensor_msgs::PointCloud2> cloudQueue;sensor_msgs::PointCloud2 currentCloudMsg;sensor_msgs::Image currentImageMsg;ros::Time time;double currentCloudtime;double currentImagetime;cv::Mat M;cv::Mat RT;cv::Mat image ;int row; //行int col;};void lidar_cam_fusion::myCallback1(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{if (!cloud_Queue(cloudmsg))return;if (!get_image())return;syn_image_lidar();
}
void lidar_cam_fusion::myCallback2(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg)
{std::lock_guard<std::mutex> lock2(imageLock);imageQueue.push_back(*imagemsg);
}bool lidar_cam_fusion::cloud_Queue(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{std::lock_guard<std::mutex> lock1(cloudLock);cloudQueue.push_back(*cloudmsg);if (cloudQueue.size() <= 2)return false;currentCloudMsg = std::move(cloudQueue.front());cloudQueue.pop_front();time = currentCloudMsg.header.stamp;currentCloudtime= currentCloudMsg.header.stamp.toSec();return true;
}bool lidar_cam_fusion::get_image()
{std::lock_guard<std::mutex> lock2(imageLock);while (!imageQueue.empty()){if (imageQueue.front().header.stamp.toSec() < currentCloudtime - 0.03)imageQueue.pop_front();elsebreak;}for (int i = 0; i < (int)imageQueue.size(); ++i){sensor_msgs::Image thisImageMsg = imageQueue[i];double currentImageTime = thisImageMsg.header.stamp.toSec();if (currentImageTime <= currentCloudtime){currentImageMsg = std::move(imageQueue.front());currentImagetime=currentImageMsg.header.stamp.toSec();return true;}if (currentImageTime > currentCloudtime + 0.03)break;}return false;}void lidar_cam_fusion::syn_image_lidar()
{//cout<<"currentCloudtime="<<setprecision(16)<<currentCloudtime<<endl;//cout<<"currentImagetime="<<setprecision(16)<<currentImagetime<<endl;//从ros消息中提取opencv图像与pcl点云cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(currentImageMsg, sensor_msgs::image_encodings::BGR8);image= cv_ptr -> image;//cv::imshow("FIRE", image);//cv::waitKey(1);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//点云pcl::fromROSMsg (currentCloudMsg, *cloud);cloud_Color(cloud);}void lidar_cam_fusion::cloud_Color(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);//遍历点云for (int i = 0; i < cloud->points.size(); i++){Mat point = (Mat_<double>(4, 1) << cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, 1);//cout << "point=" << aa[i][0]<<"  "<< aa[i][1]<<"  "<< aa[i][2] << endl;Mat uv(3, 1, CV_64F);uv = M * RT*point;if (uv.at<double>(2) == 0){cout << "uv.at<double>(2)=0" << endl;break;}double u = uv.at<double>(0) / uv.at<double>(2);double v = uv.at<double>(1) / uv.at<double>(2);int px = int(u + 0.5);int py = int(v + 0.5);//cout << "(u,v)=" << px << "  "<< py << endl;//注意：image.at<Vec3b>(row,rol)  但是像素坐标为(x,y)，即(u,v),即(rol,row)pcl::PointXYZRGB colorPoint;if (0 <= px && px < col && 0 <= py && py < row){colorPoint.x=cloud->points[i].x;colorPoint.y=cloud->points[i].y;colorPoint.z=cloud->points[i].z;colorPoint.b=image.at<Vec3b>(py, px)[0];//{r,g,b};colorPoint.g=image.at<Vec3b>(py, px)[1];colorPoint.r=image.at<Vec3b>(py, px)[2];cloud_color->push_back(colorPoint);}}Publishe_msg(cloud_color);}void lidar_cam_fusion::Publishe_msg(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color)
{sensor_msgs::PointCloud2 output;//发布点云话题消息pcl::toROSMsg(*cloud_color, output);output.header.stamp = time;output.header.frame_id = "map";colorCloud_pub.publish(output);}int main(int argc,char** argv)
{ros::init (argc, argv, "lidar_cam_fusion");lidar_cam_fusion lcf;ros::MultiThreadedSpinner spinner(2);spinner.spin();//ros::spin();return 0;
}

源码二

和源码1功能一样，写法略有不同,可读性更强

//*************************************************************************************************
//         融合  image_fire  /cloud_cut
//    source ~/catkin_ws/devel/setup.bash &&  rosrun my_lidar_cam_fusion my_lidar_cam_fusion
//
//****************************************************************************************************#include <ros/ros.h>
#include <boost/thread.hpp>#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>using namespace sensor_msgs;
using namespace std;
using namespace cv;cv::Mat M;
cv::Mat RT;
int i_=0;
ros::Publisher cloud_pub;void myCallback(const  boost::shared_ptr<const sensor_msgs::Image>& imagemsg ,const  boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{//输入相机内参及雷达与相机间的投影矩阵if(i_==0){M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);cout << "M=" << M << endl;RT = (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );cout << "RT=" << RT << endl;i_++;}//从ros消息中提取opencv图像与pcl点云cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(imagemsg, sensor_msgs::image_encodings::BGR8);cv::Mat image  = cv_ptr -> image;int row = image.rows; //行int col = image.cols;pcl::PointCloud<pcl::PointXYZ> cloud;//点云pcl::fromROSMsg (*cloudmsg, cloud);pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);//遍历点云for (int i = 0; i < cloud.points.size(); i++){Mat point = (Mat_<double>(4, 1) << cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, 1);//cout << "point=" << aa[i][0]<<"  "<< aa[i][1]<<"  "<< aa[i][2] << endl;Mat uv(3, 1, CV_64F);uv = M * RT*point;if (uv.at<double>(2) == 0){cout << "uv.at<double>(2)=0" << endl;break;}double u = uv.at<double>(0) / uv.at<double>(2);double v = uv.at<double>(1) / uv.at<double>(2);int px = int(u + 0.5);int py = int(v + 0.5);//cout << "(u,v)=" << px << "  "<< py << endl;//注意：image.at<Vec3b>(row,rol)  但是像素坐标为(x,y)，即(u,v),即(rol,row)pcl::PointXYZRGB colorPoint;if (0 <= px && px < col && 0 <= py && py < row){colorPoint.x=cloud.points[i].x;colorPoint.y=cloud.points[i].y;colorPoint.z=cloud.points[i].z;colorPoint.b=image.at<Vec3b>(py, px)[0];//{r,g,b};colorPoint.g=image.at<Vec3b>(py, px)[1];colorPoint.r=image.at<Vec3b>(py, px)[2];cloud_color->push_back(colorPoint);}}// 发布点云消息//cout<< j <<endl;sensor_msgs::PointCloud2 output;//发布点云话题消息pcl::toROSMsg(*cloud_color, output);output.header.stamp = cloudmsg->header.stamp;output.header.frame_id = "map";cloud_pub.publish(output);}int main(int argc,char** argv)
{ros::init (argc, argv, "my_lidar_cam_fusion");ros::NodeHandle nh;cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/image_color_cloud", 1);//彩色点云发布message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/image_fire", 1000); //  /image_raw  /image_firemessage_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh,"/cloud_cut",1000);//订阅激光雷达  /points_raw /cloud_lidar /cloud_cuttypedef  message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::PointCloud2> MySyncPolicy;message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(2000),image_sub,lidar_sub);    // 同步sync.registerCallback(boost::bind(&myCallback, _1, _2));                   // 回调ros::MultiThreadedSpinner spinner(2);spinner.spin();//ros::spin();return 0;
}

效果

由于是红外图像与点云融合，所以视觉效果不太好。

原图：

融合后

雷达相机融合（四）--点云着色相关推荐

Airsim雷达相机融合生成彩色点云
代码,cmake,launch等都在网盘链接中链接: https://pan.baidu.com/s/14hWBDnRZC41xkqAk6FirtA 提取码: 1een --来自百度网盘超级会员v4 ...
雷达相机融合（七）--显示鼠标位置处的实际空间位置
目录思路鼠标位置处的雷达坐标鼠标位置处的GPS 效果思路通过鼠标回调函数获取鼠标所在位置处的像素坐标,从而得出鼠标所在像素坐标处的点云坐标,由于点云稀疏,如果该像素没有对应雷达点,则遍历,寻 ...
【radar】毫米波雷达-相机-激光雷达融合相关论文汇总（特征融合、RPN融合、弱监督融合、决策融合、深度估计、跟踪）（5）
[radar]毫米波雷达-相机-激光雷达融合相关论文汇总(特征融合.RPN融合.弱监督融合.决策融合.深度估计.跟踪)(5) Radar Camera Fusion Feature-level Fus ...
NeurIPS 2022 | 北大阿里提出BEVFusion：激光雷达-相机融合新框架
点击下方卡片,关注"CVer"公众号 AI/CV重磅干货,第一时间送达作者:考验 | 已授权转载(源:知乎)编辑:CVer https://zhuanlan.zhihu.com ...
【自动驾驶】视觉与毫米波雷达数据融合技术
文章目录一.相机介绍二.毫米波雷达介绍三.为什么要做传感器融合四.相机与毫米波雷达的融合方式五.雷达与相机联合标定 (1)毫米波雷达坐标系至世界坐标系 (2)世界坐标系至图像像素坐标系六. ...
毫米波雷达视觉融合方案综述（数据级/决策级/特征级融合）
摘要:本论文详细介绍了基于毫米波雷达和视觉融合的障碍物检测方法,从任务介绍.评估标准和数据集三方面展开. 转载自:自动驾驶之心原文地址:毫米波雷达视觉融合方案综述(数据级/决策级/特征级融合) 自动 ...
AEB落地：摄像头与毫米波雷达的融合
☛ 我们的生活中,总有各种场合需要证明自己. 内心不够坚定的时候,总是活在不断证明自己的循环中.新人刚入职,会努力证明自己是有能力的:遇到心动的男神,会努力证明自己值得被爱:受到质疑否定,会努力证明我 ...
自动驾驶之心：毫米波雷达-视觉融合感知方法（前融合/特征级融合/数据级融合）
毫米波雷达-视觉融合感知方法(前融合/特征级融合/数据级融合) 分享一个自动驾驶之心的报告:毫米波雷达与视觉融合目标检测. 作者主页为:https://www.zhihu.com/people/nac ...
5G+云网融合，移动云带领开发者释放边缘计算的力量
在5G浪潮的驱动下,智能设备.自动驾驶.VR/AR等对于实时性.本地性有着较强需求的场景日益丰富,边缘计算应运而生,有效提升了用户体验. 众所周知,边缘计算技术的突破,意味着许多控制将通过本地设备实现 ...

雷达相机融合（四）--点云着色

目录

源码一

源码二

效果

雷达相机融合（四）--点云着色相关推荐

最新文章

热门文章