雷达相机融合（五）--点云与像素关联及ros订阅发布opencv图像

思路：建一个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;
}