kitti数据集激光投影到相机，生成并发布深度图

//
// Created by cai on 2021/8/26.
//

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>

#include<opencv2/opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>

#include<stdio.h>
//#include "fssim_common/Cmd.h"
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/point_cloud_conversion.h>
using namespace Eigen;
using namespace cv;
using namespace std;


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>





static cv::Size imageSize;

static ros::Publisher pub;


//store calibration data in Opencv matrices
cv::Mat P_rect_00(3,4,cv::DataType<double>::type);//3×4 projection matrix after rectification
cv::Mat R_rect_00(4,4,cv::DataType<double>::type);//3×3 rectifying rotation to make image planes co-planar
cv::Mat RT(4,4,cv::DataType<double>::type);//rotation matrix and translation vector






class laser_to_image {

public:
    laser_to_image()
    {
        image_transport::ImageTransport depth_it(nh);
        depth_pub = depth_it.advertise("depth_to_rgb/image_raw", 1);

        cout<<"hahah"<<endl;
        //ros::Subscriber sub = nh.subscribe("/kitti/velo/pointcloud", 3, &laser_to_image::callback,this);

    }
    void pub_depth(cv::Mat depth)
    {
        depth_msg = cv_bridge::CvImage(std_msgs::Header(),  sensor_msgs::image_encodings::TYPE_16UC1, depth).toImageMsg();
        depth_pub.publish(depth_msg);
    }

     void callback(const sensor_msgs::PointCloud2ConstPtr& msg);
    void img_callback(const sensor_msgs::ImageConstPtr &color_msg);

private:
    image_transport::Publisher depth_pub ;
    sensor_msgs::ImagePtr depth_msg;
    ros::NodeHandle nh;
    ros::Publisher pub;
    ros::Subscriber sub;
};

void laser_to_image::img_callback(const sensor_msgs::ImageConstPtr &color_msg)
{
    cout<<"zhong"<<endl;
    cv_bridge::CvImageConstPtr cv_ptrLeft;
    cv_ptrLeft = cv_bridge::toCvShare(color_msg);
    Mat zhong_img;
    zhong_img=cv_ptrLeft->image;

    cv::imwrite("zhong.png",zhong_img);


}

 void laser_to_image::callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{

    sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式
    //对数据进行处理
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    output = *msg;

    pcl::fromROSMsg(output,*cloud);

    cv::Mat tem_=imread("zhong.png");

    cv::Mat visImg=tem_;

    cv::Mat overlay = visImg.clone();

    cv::Mat X(4,1,cv::DataType<double>::type);
    cv::Mat Y(4,1,cv::DataType<double>::type);

    int count=0;
    Mat depthImg(overlay.rows,overlay.cols,CV_16UC1,Scalar(0));
    int maxx=0,maxy=0;
    for(int i=0;i<cloud->size();i++) {
        // convert each 3D point into homogeneous coordinates and store it in a 4D variable X
        X.at<double>(0, 0) = cloud->points[i].x;
        X.at<double>(1, 0) = cloud->points[i].y;
        X.at<double>(2, 0) = cloud->points[i].z;
        X.at<double>(3, 0) = 1;

        //apply the projection equation to map X onto the image plane of the camera. Store the result in Y

        Y=P_rect_00*R_rect_00*RT*X;
        cv::Mat camera_link;
        camera_link=R_rect_00*RT*X;

        // transform Y back into Euclidean coordinates and store the result in the variable pt
        cv::Point pt;
        pt.x = Y.at<double>(0, 0) / Y.at<double>(2, 0);
        pt.y = Y.at<double>(1, 0) / Y.at<double>(2, 0);
        if(pt.x>maxx)
            maxx=pt.x;
        if(pt.y<maxy)
            maxy=pt.y;
        if(pt.x<1240 &&pt.x>0 &&pt.y<375 &&pt.y>0)

            depthImg.ptr<unsigned short>(pt.y)[pt.x]=Y.at<double>(2, 0)*1000.0 ;

        float val = cloud->points[i].x;
        float maxVal = 20.0;
        int red = min(255, (int) (255 * abs((val - maxVal) / maxVal)));
        int green = min(255, (int) (255 * (1 - abs((val - maxVal) / maxVal))));
        cv::circle(overlay, pt, 1, cv::Scalar(0, green, red), cv::FILLED);
        count++;

    }

    float opacity=0.6;
    cv::addWeighted(overlay,opacity,visImg,1-opacity,0,visImg);

    string windowName = "Lidar data on image overlay";
    cv::namedWindow(windowName,3);
    cv::Mat depth_save;

    cv::imshow("深度图",depthImg);
    pub_depth(depthImg);

    cv::imwrite("depth.png",depthImg);
    cv::imwrite("visImg.png",visImg);



    cv::imshow(windowName,visImg);
    cv::waitKey(30);  //wait for key to be pressed

}


int main(int argc, char* argv[])
{
    ros::init(argc, argv, "points2image");
    ros::NodeHandle n;





    RT.at<double>(0,0) = 7.533745e-03;RT.at<double>(0,1) = -9.999714e-01;RT.at<double>(0,2) = -6.166020e-04;RT.at<double>(0,2) = -4.069766e-03;
    RT.at<double>(1,0) = 1.480249e-02;RT.at<double>(1,1) = 7.280733e-04;RT.at<double>(1,2) = -9.998902e-01;RT.at<double>(1,3) = -7.631618e-02;
    RT.at<double>(2,0) = 9.998621e-01;RT.at<double>(2,1) = 7.523790e-03;RT.at<double>(2,2) = 1.480755e-02;RT.at<double>(2,3) = -2.717806e-01;
    RT.at<double>(3,0) = 0.0;RT.at<double>(3,1) = 0.0;RT.at<double>(3,2) = 0.0;RT.at<double>(3,3) = 1.0;

    R_rect_00.at<double>(0,0) = 9.999239e-01;R_rect_00.at<double>(0,1) = 9.837760e-03;R_rect_00.at<double>(0,2) = -7.445048e-03;R_rect_00.at<double>(0,3) = 0.0;
    R_rect_00.at<double>(1,0) = -9.869795e-03;R_rect_00.at<double>(1,1) = 9.999421e-01;R_rect_00.at<double>(1,2) = -4.278459e-03;R_rect_00.at<double>(1,3) = 0.0;
    R_rect_00.at<double>(2,0) = 7.402527e-03;R_rect_00.at<double>(2,1) = 4.351614e-03;R_rect_00.at<double>(2,2) = 9.999631e-01;R_rect_00.at<double>(2,3) = 0.0;
    R_rect_00.at<double>(3,0) = 0.0;R_rect_00.at<double>(3,1) = 0.0;R_rect_00.at<double>(3,2) = 0.0;R_rect_00.at<double>(3,3) = 1.0;

    P_rect_00.at<double>(0,0) = 7.215377e+02;P_rect_00.at<double>(0,1) = 0.000000e+00;P_rect_00.at<double>(0,2) = 6.095593e+02;P_rect_00.at<double>(0,3) = 0.000000e+00;
    P_rect_00.at<double>(1,0) = 0.000000e+00;P_rect_00.at<double>(1,1) = 7.215377e+02;P_rect_00.at<double>(1,2) = 1.728540e+02;P_rect_00.at<double>(1,3) = 0.000000e+00;
    P_rect_00.at<double>(2,0) = 0.000000e+00;P_rect_00.at<double>(2,1) = 0.000000e+00;P_rect_00.at<double>(2,2) = 1.000000e+00;P_rect_00.at<double>(2,3) = 0.000000e+00;


    ROS_INFO("[points2image]Publishing to... ");



    laser_to_image new_laser_to_image;
    ros::Subscriber rgb_img = n.subscribe("/kitti/camera_color_left/image_raw", 10, &laser_to_image::img_callback, &new_laser_to_image);
    ros::Subscriber sub = n.subscribe("/kitti/velo/pointcloud", 10, &laser_to_image::callback, &new_laser_to_image);
    ros::spin();
    return 0;
}

cmakelist

cmake_minimum_required(VERSION 2.4.6)
include_directories("/usr/include/eigen3")
set(CMAKE_BUILD_TYPE "Debug")
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
  roscpp
  rospy
  sensor_msgs
  std_msgs
        tf
        pcl_conversions
        tf2
        tf2_ros
        tf2_geometry_msgs
)
find_package(OpenCV REQUIRED)
find_package( PCL 1.8 REQUIRED)
include_directories(include ${catkin_INCLUDE_DIRS})
include_directories(include ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
 


add_executable(project_pub src/project_pub.cpp)
target_link_libraries(project_pub ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}  ${PCL_LIBRARIES})