思路:建一个CV_64FC3的矩阵,用于存对应像素处的点云
注意:要通过Scalar()使初始矩阵全部为0;
点云与像素关联
cv::Mat cloudImg(row,col,CV_64FC3,Scalar());
for (int i = 0; i <cloud_tmp->size(); i++)
{
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << cloud_tmp->points[i].x, cloud_tmp->points[i].y, cloud_tmp->points[i].z, 1);
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)
if (0 <= px && px < col && 0 <= py && py < row)
{
cloudImg.at<Vec3d>(py,px)[0]= point.at<double>(0);
cloudImg.at<Vec3d>(py,px)[1]= point.at<double>(1);
cloudImg.at<Vec3d>(py,px)[2]= point.at<double>(2);
}
}
ros发布opencv图像
需要通过cv_bridge来发布
注意这里的“ cvi.encoding = “64FC3”; ”必须准确
可查看:http://docs.ros.org/en/melodic/api/sensor_msgs/html/image__encodings_8h.html
cv_bridge::CvImage cvi;
cvi.header.stamp = time;
cvi.header.frame_id = "cloud_image";
cvi.encoding = "64FC3";
cvi.image = cloudImg;
sensor_msgs::Image msg;
cvi.toImageMsg(msg);
cloud_img_pub.publish(msg);
最常用的一般是:const std::string sensor_msgs::image_encodings::RGB8 = “rgb8”
ros订阅opencv图像
//从ros消息中提取opencv图像
cv_bridge::CvImagePtr cv_ptr1 = cv_bridge::toCvCopy(cloud_img_msg, sensor_msgs::image_encodings::TYPE_64FC3 );
cv::Mat cloud_img = cv_ptr1 -> image;
cv_bridge::CvImagePtr cv_ptr2 = cv_bridge::toCvCopy(image_fire_msg, sensor_msgs::image_encodings::BGR8);
cv::Mat image_fire = cv_ptr2 -> image;
完整代码
发布:
//*************************************************************************************************
// 获取密集点云cloud_cut,发布64FC3图像:cloud_img
// source ~/catkin_ws/devel/setup.bash && rosrun my_lidar_cam_fusion cloud_img
//
//****************************************************************************************************
#include "ros/ros.h"
#include <math.h>
#include <iostream>
#include <iomanip>
#include <cmath>
#include "std_msgs/String.h"
#include <boost/thread.hpp>
#include <geometry_msgs/TransformStamped.h>
#include "tf/transform_datatypes.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h> //ROS图象类型的编码函数
#include <image_transport/image_transport.h> //在ROS系统中的话题上发布和订阅图象消息
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
using namespace sensor_msgs;
using namespace std;
using namespace cv;
class cloud_img
{
public:
cloud_img()
{
cout<< "获取密集点云,迭代发布:cloud_dense "<<endl;
row = 480; //行
col = 640;
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;
//cloud_img_pub = transport.advertise("cloud_img", 1);
cloud_img_pub = n.advertise<sensor_msgs::Image>("cloud_img", 1);//发布
sub_cloud = n.subscribe<sensor_msgs::PointCloud2>("cloud_cut", 10, &cloud_img::chatterCallback,this,ros::TransportHints().tcpNoDelay());
}
void chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);
void Publishe_msg(cv::Mat cloudImg);
private:
ros::NodeHandle n;
ros::Subscriber sub_cloud;
ros::Publisher cloud_img_pub;
//image_transport::ImageTransport transport(n);
//image_transport::Publisher cloud_img_pub; //ROS图象发布
ros::Time time;
cv::Mat M;
cv::Mat RT;
int row; //行
int col;
};
// 回调函数
void cloud_img::chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
// cout<< "xx"<<endl;
cv::Mat cloudImg(row,col,CV_64FC3,Scalar());
time = cloudmsg->header.stamp;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//点云对象
pcl::fromROSMsg (*cloudmsg, *cloud);
for (int i = 0; i <cloud->size(); i++)
{
if(3<cloud->points[i].x || cloud->points[i].x<-3)
{
if(-20<cloud->points[i].y && cloud->points[i].y<20)
{
double x=cloud->points[i].x;
double y=cloud->points[i].y;
double z=cloud->points[i].z;
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << x, y, z, 1);
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);
//注意:image.at<Vec3b>(row,rol) 但是像素坐标为(x,y),即(u,v),即(rol,row)
if (0 <= px && px < col && 0 <= py && py < row)
{
cloudImg.at<Vec3d>(py,px)[0]=x;
cloudImg.at<Vec3d>(py,px)[1]=y;
cloudImg.at<Vec3d>(py,px)[2]=z;
}
}//if
}//if
}//for
Publishe_msg( cloudImg);
}
void cloud_img::Publishe_msg(cv::Mat cloudImg)
{
cv_bridge::CvImage cvi;
cvi.header.stamp = time;
cvi.header.frame_id = "cloud_image";
cvi.encoding = "64FC3";
cvi.image = cloudImg;
sensor_msgs::Image msg;
cvi.toImageMsg(msg);
cloud_img_pub.publish(msg);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cloud_img");
cloud_img ci;
ros::MultiThreadedSpinner spinner(2);
spinner.spin();
//ros::spin();
return 0;
}
订阅:
//*************************************************************************************************
// 融合 image_fire /cloud_img
// source ~/catkin_ws/devel/setup.bash && rosrun my_lidar_cam_fusion img_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/io/pcd_io.h>
#include <pcl/surface/mls.h>
#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;
class img_cam_fusion
{
public:
img_cam_fusion()
{
cout<< "发布:image_color_cloud "<<endl;
cloud_pub = n.advertise<sensor_msgs::PointCloud2>("image_color_cloud", 1);//拼接后的点云发布
row = 480; //行
col = 640;
sub1.subscribe(n,"cloud_img",100);
sub2.subscribe(n,"image_fire",300);
sync.connectInput( sub1, sub2);
sync.registerCallback( boost::bind(&img_cam_fusion::chatterCallback, this,_1, _2) );
}
void chatterCallback(const boost::shared_ptr<const sensor_msgs::Image>& cloud_img,const boost::shared_ptr<const sensor_msgs::Image>& image_fire );
void Publishe_msg(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color );
bool point_fitting(cv::Mat cloud_img,int r,int c);
private:
ros::NodeHandle n;
message_filters::Subscriber<sensor_msgs::Image> sub1;
message_filters::Subscriber<sensor_msgs::Image> sub2;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync{MySyncPolicy(2000)};
ros::Publisher cloud_pub;
ros::Time time;
pcl::PointXYZ fit_point;
int row; //行
int col;
};
void img_cam_fusion::chatterCallback(const boost::shared_ptr<const sensor_msgs::Image>& cloud_img_msg,const boost::shared_ptr<const sensor_msgs::Image>& image_fire_msg )
{
time=cloud_img_msg->header.stamp;
//从ros消息中提取opencv图像
cv_bridge::CvImagePtr cv_ptr1 = cv_bridge::toCvCopy(cloud_img_msg, sensor_msgs::image_encodings::TYPE_64FC3 );
cv::Mat cloud_img = cv_ptr1 -> image;
cv_bridge::CvImagePtr cv_ptr2 = cv_bridge::toCvCopy(image_fire_msg, sensor_msgs::image_encodings::BGR8);
cv::Mat image_fire = cv_ptr2 -> image;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int r=0;r<row;r++)
{
for(int c=0;c<col;c++)
{
if(cloud_img.at<Vec3d>(r, c)[0]>5 && (-20)<cloud_img.at<Vec3d>(r, c)[1] && cloud_img.at<Vec3d>(r, c)[1]<20)
{
pcl::PointXYZRGB colorPoint;
colorPoint.x=cloud_img.at<Vec3d>(r, c)[0];
colorPoint.y=cloud_img.at<Vec3d>(r, c)[1];
colorPoint.z=cloud_img.at<Vec3d>(r, c)[2];
colorPoint.b=image_fire.at<Vec3b>(r, c)[0];
colorPoint.g=image_fire.at<Vec3b>(r, c)[1];
colorPoint.r=image_fire.at<Vec3b>(r, c)[2];
cloud_color->push_back(colorPoint);
}
else if(image_fire.at<Vec3b>(r, c)[2]==255)
{
if(point_fitting(cloud_img,r,c))
{
pcl::PointXYZRGB colorPoint;
colorPoint.x=fit_point.x;
colorPoint.y=fit_point.y;
colorPoint.z=fit_point.z;
colorPoint.b=0;
colorPoint.g=0;
colorPoint.r=255;
cloud_color->push_back(colorPoint);
}
}
//if(cloud_img.at<Vec3d>(r, c)[0]<3 && cloud_img.at<Vec3d>(r, c)[0]!=0)
//{
// cout<< "cloud_img.at<Vec3d>(r, c)[0]= "<<cloud_img.at<Vec3d>(r, c)[0]<<endl;
//}
}
}
Publishe_msg(cloud_color );
}
bool img_cam_fusion::point_fitting(cv::Mat cloud_img,int r,int c)
{
//cout<< "r= "<<r <<"c= "<<c<<endl;
pcl::PointXYZ point1;
pcl::PointXYZ point2;
pcl::PointXYZ point3;
pcl::PointXYZ point4;
if(r<3 || r>477 || c<3 || c>637)
return false;
for(int i=r;i>=0;i--)
{
if(cloud_img.at<Vec3d>(i, c)[0]>0)
{
point1.x=cloud_img.at<Vec3d>(i, c)[0];
point1.y=cloud_img.at<Vec3d>(i, c)[1];
point1.z=cloud_img.at<Vec3d>(i, c)[2];
break;
}
}
for(int i=r;i<=480;i++)
{
if(cloud_img.at<Vec3d>(i, c)[0]>0)
{
point2.x=cloud_img.at<Vec3d>(i, c)[0];
point2.y=cloud_img.at<Vec3d>(i, c)[1];
point2.z=cloud_img.at<Vec3d>(i, c)[2];
break;
}
}
//if((point1.x-point2.x)<-2 ||(point1.x-point2.x)>2 )
// return false;
for(int j=c;j>=0;j--)
{
if(cloud_img.at<Vec3d>(r, j)[0]>0)
{
point3.x=cloud_img.at<Vec3d>(r, j)[0];
point3.y=cloud_img.at<Vec3d>(r, j)[1];
point3.z=cloud_img.at<Vec3d>(r, j)[2];
break;
}
}
//if((point1.x-point3.x)<-2 ||(point1.x-point3.x)>2 )
// return false;
for(int j=c;j<=640;j++)
{
if(cloud_img.at<Vec3d>(r, j)[0]>0)
{
point4.x=cloud_img.at<Vec3d>(r, j)[0];
point4.y=cloud_img.at<Vec3d>(r, j)[1];
point4.z=cloud_img.at<Vec3d>(r, j)[2];
break;
}
}
// if((point1.x-point4.x)<-2 ||(point1.x-point4.x)>2 )
// return false;
if(point1.x==0 || point2.x==0 ||point3.x==0 || point4.x==0 )
return false;
fit_point.x=(point1.x+point2.x+point3.x+point4.x)/4;
fit_point.y=(point1.y+point2.y+point3.y+point4.y)/4;
fit_point.z=(point1.z+point2.z+point3.z+point4.z)/4;
return true;
}
void img_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";
cloud_pub.publish(output);
}
int main(int argc,char** argv)
{
ros::init (argc, argv, "img_cam_fusion");
img_cam_fusion icf;
ros::MultiThreadedSpinner spinner(2);
spinner.spin();
//ros::spin();
return 0;
}