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

最近在做一些毕设的东西，做到这里写个笔记记录以下，也为大家提供一点参考。
本次所用的数据是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_cloudrgb_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;
}

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