一、数据准备与处理
这里使用的是kitti数据集中：2011_10_03_drive_0047_sync.zip、2011_10_03_calib.zip。
直接在命令行解压上述两个压缩包：
1 unzip 2011_10_03_calib.zip 2 unzip 2011_10_03_drive_0047_sync.zip
解压后效果如下：
三个txt文件分别是相关外参数，即：
- 相机到相机
- imu到雷达
- 雷达到相机
上述2011_10_03_drive_0047_sync文件夹中效果图：
文件夹对应硬件分别是：
- image_00：左灰度相机
- image_01：右灰度相机
- image_02：左彩色相机
- image_03：右彩色相机
- oxts：IMU/GPS
- velodyne_points：雷达点云
二、3D点到2D平面映射
根据kitti数据集采集时标定的坐标转换公式，来计算由点云坐标中的点x(x, y, z, 1)到图像坐标中的y(u, v, 1)的变换：
其中，T_velo_cam为坐标变换旋转平移矩阵；R_rect_02为各摄像头到摄像头0的修正矩阵；P_rect_0i为各摄像头的内参矩阵(本文中i = 2)；
注意：R_rect_02为3*3旋转矩阵，P_rect_0i为3*4矩阵(包含相机i的3*3内参矩阵、和一个3*1位移)，该位移应该和前面R_rect_02对应起来，组成一个仿射矩阵，有点反人类。
在工程计算时需对各矩阵进行行列扩展（注意中间的旋转矩阵）：
打开文件calib_velo_to_cam.txt：
1 calib_time: 15-Mar-2012 11:45:23 2 R: 7.967514e-03 -9.999679e-01 -8.462264e-04 -2.771053e-03 8.241710e-04 -9.999958e-01 9.999644e-01 7.969825e-03 -2.764397e-03 3 T: -1.377769e-02 -5.542117e-02 -2.918589e-01 4 delta_f: 0.000000e+00 0.000000e+00 5 delta_c: 0.000000e+00 0.000000e+00
文件中R|T可以组成4*4矩阵T_velo_cam
打开文件calib_cam_to_cam.txt(下面只贴一部分)：
1 S_02: 1.392000e+03 5.120000e+02 2 K_02: 9.601149e+02 0.000000e+00 6.947923e+02 0.000000e+00 9.548911e+02 2.403547e+02 0.000000e+00 0.000000e+00 1.000000e+00 3 D_02: -3.685917e-01 1.928022e-01 4.069233e-04 7.247536e-04 -6.276909e-02 4 R_02: 9.999788e-01 -5.008404e-03 -4.151018e-03 4.990516e-03 9.999783e-01 -4.308488e-03 4.172506e-03 4.287682e-03 9.999821e-01 5 T_02: 5.954406e-02 -7.675338e-04 3.582565e-03 6 S_rect_02: 1.241000e+03 3.760000e+02 7 R_rect_02: 9.999191e-01 1.228161e-02 -3.316013e-03 -1.228209e-02 9.999246e-01 -1.245511e-04 3.314233e-03 1.652686e-04 9.999945e-01 8 P_rect_02: 7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03
- S_xx：1x2 矫正前的图像xx的大小
- K_xx：3x3 摄像机xx的校准矩阵
- D_xx：1x5 矫正前摄像头xx的失真向量
- R_xx：3x3 （外部）的旋转矩阵(从相机0到相机xx)
- T_xx：3x1 （外部）的平移矢量(从相机0到相机xx)
- S_rect_xx：1x2 矫正后的图像xx的大小
- R_rect_xx：3x3 纠正旋转矩阵(使图像平面共面)
- P_rect_xx：3x4 矫正后的投影矩阵 （内参矩阵，前面第一行为前面四个数据，依次三行）
这里将P_rect_02写开，第四列就是一个位移了，对应R_rect_02，不多比比。可以看到内参矩阵中，cx = 607.1928，cy = 185.2157 ；由S_rect_02:可知，校正后图像分辨率为：1241*376，差不多是cx、cy两倍，符合情理。
7.188560e+02 0.000000e+00 6.071928e+02 4.538225e+01 0.000000e+00 7.188560e+02 1.852157e+02 -1.130887e-01 0.000000e+00 0.000000e+00 1.000000e+00 3.779761e-03
好了，我们已经拿到了相关参数，下面是代码
1 // lidar -> 02Camera(left RGB) 2 // 【calib_cam_to_cam】：P_rect_02(3*4) 3 Mat P_rect_02 = (Mat_<float>(3,4) << 7.188560e+02, 0.000000e+00, 6.071928e+02, 4.538225e+01, 4 0.000000e+00, 7.188560e+02, 1.852157e+02, -1.130887e-01, 5 0.000000e+00, 0.000000e+00, 1.000000e+00, 3.779761e-03); 6 // 【calib_cam_to_cam】：R_rect_02(3*3写成：4*4) 7 Mat R_rect_02 = (Mat_<float>(4,4) << 9.999191e-01, 1.228161e-02, -3.316013e-03, 0, 8 -1.228209e-02, 9.999246e-01, -1.245511e-04, 0, 9 3.314233e-03, 1.652686e-04, 9.999945e-01, 0, 10 0,0,0,1); 11 // 【calib_velo_to_cam】：R|T（3*4写成：4*4）note:T的单位应该是米 12 Mat Tr_velo_to_cam = (Mat_<float>(4,4) << 7.967514e-03, -9.999679e-01, -8.462264e-04,-1.377769e-02, 13 -2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02, 14 9.999644e-01, 7.969825e-03, -2.764397e-03,-2.918589e-01, 15 0,0,0,1);
提前计算出P * R * T，用于下一步的坐标变换：
1 Mat trans = Mat(3,4,CV_32FC1); 2 trans = P_rect_02 * R_rect_02 * Tr_velo_to_cam;
三、代码测试
总的代码:
kitti_color.cpp
1 // 2 // Created by sry on 2021/9/28. 3 // 4 // 5 // Created by sry on 2021/9/27. 6 // 7 8 //./kitti-color ./kitti/1.pcd ./000001.png 9 #include <iostream> 10 #include<vector> 11 #include<string> 12 #include<algorithm> 13 #include <boost/thread/thread.hpp> 14 #include <pcl/common/common_headers.h> 15 #include <pcl/common/common_headers.h> 16 #include <pcl/features/normal_3d.h> 17 #include <pcl/io/pcd_io.h> 18 #include <pcl/visualization/pcl_visualizer.h> 19 #include <pcl/console/parse.h> 20 21 #include "opencv2/highgui.hpp" 22 using namespace cv; 23 using namespace std; 24 //RGB colour visualisation example 25 boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) 26 { 27 //创建3D窗口并添加点云 28 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 29 viewer->setBackgroundColor(0, 0, 0); 30 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud); 31 viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud"); 32 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud"); 33 viewer->addCoordinateSystem(1.0); 34 viewer->initCameraParameters(); 35 return (viewer); 36 } 37 38 void printVec(const vector<char>& str); 39 void reName(std::string& inStr, std::string& outStr); 40 41 // -----Main----- 42 int main() 43 { 44 pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); 45 pcl::io::loadPCDFile ("../pcd/0000000003.pcd", *point_cloud_ptr); 46 47 Mat img = imread("../image/0000000003.png"); 48 if(img.channels() != 3){ 49 cout <<"RGB pics needed." <<endl; 50 return 0; 51 } 52 cout <<"Pic loaded." <<endl; 53 int rows = img.rows; 54 int cols = img.cols; 55 unsigned char red,green,blue; 56 float p_u,p_v,p_w;//pics_uv1;(u for cols, v for lines!!!) 57 float c_x,c_y,c_z,c_i;//clouds_xyz、intensity; 58 59 // Mat P_rect_02 = (Mat_<float>(3,4) << 7.215377000000e+02,0.000000000000e+00,6.095593000000e+02,4.485728000000e+01,0.000000000000e+00,7.215377000000e+02,1.728540000000e+02,2.163791000000e-01,0.000000000000e+00,0.000000000000e+00,1.000000000000e+00,2.745884000000e-03); 60 // Mat R_rect_02 = (Mat_<float>(4,4) << 9.999239000000e-01,9.837760000000e-03,-7.445048000000e-03,0,-9.869795000000e-03,9.999421000000e-01,-4.278459000000e-03,0,7.402527000000e-03,4.351614000000e-03,9.999631000000e-01,0, 0,0,0,1); 61 // Mat Tr_velo_to_cam = (Mat_<float>(4,4) << 7.533745000000e-03,-9.999714000000e-01,-6.166020000000e-04,-4.069766000000e-03,1.480249000000e-02,7.280733000000e-04,-9.998902000000e-01,-7.631618000000e-02,9.998621000000e-01,7.523790000000e-03,1.480755000000e-02,-2.717806000000e-01, 0,0,0,1); 62 // lidar -> 02Camera(left RGB) 63 // 【calib_cam_to_cam】：P_rect_02(3*4) 64 Mat P_rect_02 = (Mat_<float>(3,4) << 7.188560e+02, 0.000000e+00, 6.071928e+02, 4.538225e+01, 65 0.000000e+00, 7.188560e+02, 1.852157e+02, -1.130887e-01, 66 0.000000e+00, 0.000000e+00, 1.000000e+00, 3.779761e-03); 67 // 【calib_cam_to_cam】：R_rect_02(3*3写成：4*4) 68 Mat R_rect_02 = (Mat_<float>(4,4) << 9.999191e-01, 1.228161e-02, -3.316013e-03, 0, 69 -1.228209e-02, 9.999246e-01, -1.245511e-04, 0, 70 3.314233e-03, 1.652686e-04, 9.999945e-01, 0, 71 0,0,0,1); 72 // 【calib_velo_to_cam】：R|T（3*4写成：4*4）note:T的单位应该是米 73 Mat Tr_velo_to_cam = (Mat_<float>(4,4) << 7.967514e-03, -9.999679e-01, -8.462264e-04,-1.377769e-02, 74 -2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02, 75 9.999644e-01, 7.969825e-03, -2.764397e-03,-2.918589e-01, 76 0,0,0,1); 77 78 Mat trans = Mat(3,4,CV_32FC1); 79 trans = P_rect_02 * R_rect_02 * Tr_velo_to_cam; 80 Mat c_tmp = Mat(4,1,CV_32FC1); 81 Mat p_result = Mat(1,3,CV_32FC1); 82 83 cout << "trans = " << trans <<endl; 84 85 for(int nIndex = 0; nIndex < point_cloud_ptr->points.size (); nIndex++) 86 { 87 c_x = point_cloud_ptr->points[nIndex].x; 88 c_y = point_cloud_ptr->points[nIndex].y; 89 c_z = point_cloud_ptr->points[nIndex].z; 90 91 c_tmp = (Mat_<float>(4, 1) << c_x, c_y, c_z, 1); 92 p_result = trans * c_tmp; 93 94 p_w = p_result.at<float>(0,2); 95 p_u = (int)((p_result.at<float>(0,0)) / p_w); 96 p_v = (int)((p_result.at<float>(0,1)) / p_w); 97 if( (p_u<0) || (p_u>cols) || (p_v<0) || (p_v>rows) ||(p_w < 0)){ 98 point_cloud_ptr->points[nIndex].r = 128; 99 point_cloud_ptr->points[nIndex].g = 2; 100 point_cloud_ptr->points[nIndex].b = 64; 101 continue; 102 } 103 point_cloud_ptr->points[nIndex].r = img.at<Vec3b>(p_v,p_u)[2];//not (p_u,p_v)! 104 point_cloud_ptr->points[nIndex].g = img.at<Vec3b>(p_v,p_u)[1]; 105 point_cloud_ptr->points[nIndex].b = img.at<Vec3b>(p_v,p_u)[0]; 106 } 107 108 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; 109 viewer = rgbVis(point_cloud_ptr); 110 // 主循环 111 while (!viewer->wasStopped()) 112 { 113 viewer->spinOnce(100); 114 boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 115 } 116 return 0; 117 }
CMakeLists:
1 cmake_minimum_required(VERSION 2.8.3) 2 project(kitti_color_map) 3 4 #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 5 6 find_package(PCL REQUIRED QUIET) 7 find_package(OpenCV REQUIRED QUIET) 8 9 include_directories( 10 ${PCL_INCLUDE_DIRS} 11 ${OpenCV_INCLUDE_DIRS}) 12 13 link_directories( 14 ${OpenCV_LIBRARY_DIRS} 15 ${PCL_LIBRARY_DIRS} 16 ) 17 18 #add_executable(kitti_color_map src/mian.cpp) 19 add_executable(kitti_color_map src/kitti_color.cpp) 20 target_link_libraries(kitti_color_map ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
reference：https://blog.csdn.net/kanojoy/article/details/112008354