kitti彩色地图拼接<二>、单帧着色

一、数据准备与处理

这里使用的是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 }
View Code

　　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})
View Code

 

 

 

reference：https://blog.csdn.net/kanojoy/article/details/112008354

posted @ 2023-09-30 09:32  佚名12  阅读(80)  评论(0)  编辑  收藏  举报