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

最近在做一些毕设的东西，做到这里写个笔记记录以下，也为大家提供一点参考。
本次所用的数据是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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145

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