将激光点云数据投影到二维图像及对三维点云上色

最近在做一些毕设的东西，做到这里写个笔记记录以下，也为大家提供一点参考。
本次所用的数据是16线的激光点云数据和1080p的usb图像信息，内容涉及到标定，投影两个部分，参考网上大部分都是ros下方进行进一步开发，这里写一个不一样的。

1、相机和激光雷达标定

相机和激光雷达标定使用的是autoware的标定包。需要标定的话可以参考大佬们的博客，内容相差不大，里面有工具安装步骤和标定方法。
https://blog.csdn.net/AdamShan/article/details/81670732

https://blog.csdn.net/zbr794866300/article/details/107109186

2、矩阵参数转置

autoware构建出来的矩阵不能拿来直接使用，原因我就不仔细在这里介绍了。可以参考大佬们的博客。直通车！！！（飞机票）

3、激光和相机之间的投影

这里就直接上全部代码了，关键部分的代码解读，参考这行的飞机票。python版本的点这个链接。

#include<iostream>
#include<opencv2/opencv.hpp>
#include<string>
#include<pcl/io/pcd_io.h>
#include<pcl/common/transforms.h>
#include<pcl/console/parse.h>
#include<pcl/visualization/range_image_visualizer.h>
#include<pcl/common/common_headers.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/visualization/cloud_viewer.h>
using namespace std;
struct fileArg
{
    cv::Mat extrinsic_mat, camera_mat,dist_coeff; //外参矩阵，内参矩阵，畸变矩阵
    cv::Mat rotate_mat,transform_vec; //旋转矩阵，平移向量
};

struct calcuArg
{
    cv::Mat rotate_mat;
    cv::Mat transform_vec;
    cv::Mat rotate_vec;
};



void getMatrixFromFile(cv::String filePath, fileArg& filearg, calcuArg& calarg) {    
	    cv::FileStorage fs(filePath, cv::FileStorage::READ); //打开标定结果文件
        if(!fs.isOpened()) cout<< "open failed"<<endl; 
	    fs["CameraExtrinsicMat"] >> filearg.extrinsic_mat; //从文件里读取4x4外参矩阵
	    fs["CameraMat"] >>filearg.camera_mat; //从文件里读取3x3相机内参矩阵
	    fs["DistCoeff"] >> filearg.dist_coeff; //从文件里读取5x1畸变矩阵
	    fs.release(); //关闭文件
        calarg.rotate_mat=cv::Mat(3, 3, cv::DataType<double>::type); // 将旋转矩阵赋值成3x3矩阵
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                calarg.rotate_mat.at<double>(i,j)=filearg.extrinsic_mat.at<double>(j,i); // 取前三行前三列
            }
        }
        
        //cv::transpose( filearg.camera_mat ,filearg.camera_mat);网上说先做转置，但是转了效果不对
        calarg.rotate_vec = cv::Mat(3, 1, cv::DataType<double>::type); 
        cv::Rodrigues(calarg.rotate_mat, calarg.rotate_vec);

        calarg.transform_vec=cv::Mat(3, 1, cv::DataType<double>::type); //将平移向量赋值成3x1矩阵
        calarg.transform_vec.at<double>(0)=filearg.extrinsic_mat.at<double>(1,3);
        calarg.transform_vec.at<double>(1)=filearg.extrinsic_mat.at<double>(2,3);
        calarg.transform_vec.at<double>(2)=-filearg.extrinsic_mat.at<double>(0,3);
}


void projection(const pcl::PointCloud<pcl::PointXYZI>::Ptr&ccloud,  pcl::PointCloud<pcl::PointXYZRGB>::Ptr & rgb_cloud,cv::Mat&img, fileArg& filearg, calcuArg& calarg) {
    vector<cv::Point3f> points3d; //存储点云点的vcector，必须存储成cv::Point3f格式
    points3d.reserve(ccloud->size()+1); //先给points3d分配足够大的内存空间，避免push_back时频繁复制转移
    cv::Point3f point;
    for(int i=0;i<ccloud->size();i++)
    {
        point.x=ccloud->points[i].x;
        point.y=ccloud->points[i].y;
        point.z=ccloud->points[i].z;
        points3d.push_back(point); //逐个插入
    }
    vector<cv::Point2f> projectedPoints; //该vector用来存储投影过后的二维点，三维点投影后变二维

    cv::projectPoints(points3d, calarg.rotate_vec,calarg.transform_vec,filearg.camera_mat,filearg.dist_coeff,projectedPoints);

    //获取点云投影数据，并限制在相机视角内
    vector<cv::Point2f> pointInImg;
    for(int i=0; i<projectedPoints.size(); i++){
        cv::Point2f p = projectedPoints[i];
        float x = p.x;
        float y = p.y;
        if(x>=0 && x<=1920 && y>=0 && y<=1080) { //这里的相机分辨率是1920*1080的，所以选择区域时要填自己相机的分辨率
            pointInImg.push_back(p);
        }
    }

    pcl::PointXYZRGB point_rgb;
    //pcl::PointCloud<pcl::PointXYZRGB>::Ptr  point_rgb (new pcl::PointCloud<pcl::PointXYZRGB> );
	//遍历投影结果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2f p = projectedPoints[i];
		// 由于图像尺寸为1920x1080，所以投影后坐标不在图像范围内的点不保存
        if (p.y<1080&&p.y>=0&&p.x<1920&&p.x>=0  && ccloud->points[i].x>0) 
        {
            point_rgb.x=ccloud->points[i].x;
            point_rgb.y=ccloud->points[i].y;
            point_rgb.z=ccloud->points[i].z;           
            point_rgb.r=int(img.at<cv::Vec3b>(p.y,p.x)[2]); //读取像素点的rgb值
            point_rgb.g=int(img.at<cv::Vec3b>(p.y,p.x)[1]);
            point_rgb.b=int(img.at<cv::Vec3b>(p.y,p.x)[0]);
            //对于投影后在图像中的点进行染色后加入点云rgb_cloud
            rgb_cloud->push_back(point_rgb); 
        }
        
    }

    for(int i=0; i<pointInImg.size(); i++) { //在图像上画实心圆点
        cv::circle(img, pointInImg[i], 3, cv::Scalar(255,0,0), -1);
    }
}

int main(int argc, char** argv) {
    cv::String argpathfile = "xxxx.yaml"; //这里输入标定参数的yaml信息

    string pcdPath = argv[1]; //命令行第一个参数时pcd的。
    cv::String imgPath = argv[2];//第二个参数是jpg文件的。
    fileArg fileinfo;
    calcuArg calout;

    pcl::PointCloud<pcl::PointXYZI>::Ptr  cloud_tmp (new pcl::PointCloud<pcl::PointXYZI> );
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  rgb_cloud (new pcl::PointCloud<pcl::PointXYZRGB> );
    if(pcl::io::loadPCDFile("./data/" + pcdPath,  *cloud_tmp)<0) { //打开pcd图像
        PCL_ERROR("Error loading cloud %s.\n", "pcdPath");
        return -1;
    }
 
    cv::Mat img = cv::imread("./data/" + imgPath, CV_LOAD_IMAGE_UNCHANGED); //打开jpg图像
    //cv::imshow("Img", img);
    //获取矩阵信息
    getMatrixFromFile(argpathfile, fileinfo, calout);
    //将点云信息投影到图像上
    projection(cloud_tmp, rgb_cloud ,img, fileinfo, calout);
	
    cv::imshow("Img", img);
    cv::waitKey(0);
   //点云可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3d Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(rgb_cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(rgb_cloud,rgb,"sample cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();

    while(!viewer->wasStopped()) {
        viewer->spinOnce();
    }
    
    
    return 0;
}

这里直接放相机视角下的点云信息。程序还可以生成投影了点云的图像信息，这个信息和标定的结果相关。