kitti彩色地图拼接<三>、构建彩色地图
真值数据和raw data的对应
部分真值的轨迹和raw data的轨迹相同，可以使用真值的数据进行轨迹评估。
（kitti总共有编号为00~20的21个数据集序列，其中只有00~10序列公开了真值，序列11~20仅用来做为算法评估使用）：
Nr. Sequence name Start End
---------------------------------------
00: 2011_10_03_drive_0027 000000 004540
01: 2011_10_03_drive_0042 000000 001100
02: 2011_10_03_drive_0034 000000 004660
03: 2011_09_26_drive_0067 000000 000800
04: 2011_09_30_drive_0016 000000 000270
05: 2011_09_30_drive_0018 000000 002760
06: 2011_09_30_drive_0020 000000 001100
07: 2011_09_30_drive_0027 000000 001100
08: 2011_09_30_drive_0028 001100 005170
09: 2011_09_30_drive_0033 000000 001590
10: 2011_09_30_drive_0034 000000 001200
代码都在这：
1 // 2 // Created by sry on 2021/9/28. 3 // 4 5 /** 6 * 1、两帧匹配看看，配准完直接显示看看 7 * 2、读取pcd image，按顺序读取，一一对应，没有文件则跳过 8 * */ 9 10 #include<pcl/visualization/pcl_visualizer.h> 11 //#include<pcl/common/io.h> 12 //#include<pcl/io/io.h> 13 #include <pcl/io/pcd_io.h> 14 #include <pcl/common/transforms.h> 15 #include <pcl/filters/voxel_grid.h> 16 #include <opencv2/opencv.hpp> 17 #include <Eigen/Core> 18 #include <vector> 19 #include <algorithm> 20 #include <string> 21 22 /** @brief 获取path路径下所有指定格式文件名 23 * @in path： eg:"/home/ros_proj/bin2pcd/input/" 24 * @in extendStr： eg:".bin"格式 25 * @out files： “0000000000.bin” “0000000001.bin” “0000000002.bin”... 26 * note：(具体哪种格式，需要在函数中修改，我这里默认.bin格式) 27 * */ 28 int getFiles(const std::string path, std::vector<std::string>& files, const std::string& extendStr){ 29 30 int iFileCnt = 0; 31 DIR *dirptr = NULL; 32 struct dirent *dirp; 33 34 if((dirptr = opendir(path.c_str())) == NULL)//打开一个目录 35 { 36 std::cout << "errors occurs in open the input file!" <<std::endl; 37 return 0; 38 } 39 while ((dirp = readdir(dirptr)) != NULL) 40 { 41 if ((dirp->d_type == DT_REG) && 0 ==(strcmp(strchr(dirp->d_name, '.'), extendStr.c_str())))//判断是否为文件以及文件后缀名 42 { 43 files.push_back(dirp->d_name); 44 } 45 iFileCnt++; 46 } 47 closedir(dirptr); 48 49 return iFileCnt; 50 } 51 /** @brief 读取pose 52 * @in filePath： eg:"xxxx.txt" 53 * @out transform： eg: R|T 数组 54 * */ 55 void transfromMatReader(std::string& filePath, std::vector<Eigen::Matrix4d> &transform) 56 { 57 ifstream FileIn(filePath); 58 Eigen::Matrix4d pose_data2; 59 Eigen::Matrix4d velo2cam, cam2velo; 60 cam2velo << 0, 0, 1, 0, 61 -1, 0, 0, 0, 62 0, -1, 0, 0.08, 63 0, 0, 0, 1; 64 velo2cam << 0, -1, 0, 0, 65 0, 0, -1, 0, 66 1, 0, 0, -0.08, 67 0, 0, 0, 1; 68 69 while (FileIn >> pose_data2(0, 0) >> pose_data2(0, 1) >> pose_data2(0, 2) >> pose_data2(0, 3) 70 >> pose_data2(1, 0) >> pose_data2(1, 1) >> pose_data2(1, 2) >> pose_data2(1, 3) 71 >> pose_data2(2, 0) >> pose_data2(2, 1) >> pose_data2(2, 2) >> pose_data2(2, 3)) 72 { 73 pose_data2(3, 0) = 0; 74 pose_data2(3, 1) = 0; 75 pose_data2(3, 2) = 0; 76 pose_data2(3, 3) = 1; 77 pose_data2 = cam2velo * pose_data2 * velo2cam; 78 transform.push_back(pose_data2); 79 } 80 FileIn.close(); 81 } 82 /** @brief RGB colour visualisation example 83 * */ 84 boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) 85 { 86 //创建3D窗口并添加点云 87 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 88 viewer->setBackgroundColor(0, 0, 0); 89 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud); 90 viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud"); 91 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud"); 92 viewer->addCoordinateSystem(1.0); 93 viewer->initCameraParameters(); 94 return (viewer); 95 } 96 /** @brief 仿函数 97 * */ 98 bool compareVec(std::string& a, std::string& b) 99 { 100 return a < b; 101 } 102 103 /** @brief 利用彩色图像对点云着色 104 * @in img 105 * @out point_cloud_ptr 106 * */ 107 void coloringPointCloud(const cv::Mat& img, pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr) 108 { 109 if(img.channels() != 3){ 110 cout <<"RGB pics needed." <<endl; 111 return; 112 } 113 cout <<"Pic loaded." <<endl; 114 int rows = img.rows; 115 int cols = img.cols; 116 unsigned char red,green,blue; 117 float p_u,p_v,p_w;//pics_uv1;(u for cols, v for lines!!!) 118 float c_x,c_y,c_z,c_i;//clouds_xyz、intensity; 119 120 // lidar -> 02Camera(left RGB) 121 // 【calib_cam_to_cam】：P_rect_02(3*4) 122 cv::Mat P_rect_02 = (cv::Mat_<float>(3,4) << 7.188560e+02, 0.000000e+00, 6.071928e+02, 4.538225e+01, 123 0.000000e+00, 7.188560e+02, 1.852157e+02, -1.130887e-01, 124 0.000000e+00, 0.000000e+00, 1.000000e+00, 3.779761e-03); 125 // 【calib_cam_to_cam】：R_rect_02(3*3写成：4*4) 126 cv::Mat R_rect_02 = (cv::Mat_<float>(4,4) << 9.999191e-01, 1.228161e-02, -3.316013e-03, 0, 127 -1.228209e-02, 9.999246e-01, -1.245511e-04, 0, 128 3.314233e-03, 1.652686e-04, 9.999945e-01, 0, 129 0,0,0,1); 130 // 【calib_velo_to_cam】：R|T（3*4写成：4*4）note:T的单位应该是米 131 cv::Mat Tr_velo_to_cam = (cv::Mat_<float>(4,4) << 7.967514e-03, -9.999679e-01, -8.462264e-04,-1.377769e-02, 132 -2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02, 133 9.999644e-01, 7.969825e-03, -2.764397e-03,-2.918589e-01, 134 0,0,0,1); 135 136 cv::Mat trans = cv::Mat(3,4,CV_32FC1); 137 trans = P_rect_02 * R_rect_02 * Tr_velo_to_cam; 138 cv::Mat c_tmp = cv::Mat(4,1,CV_32FC1); 139 cv::Mat p_result = cv::Mat(1,3,CV_32FC1); 140 141 cout << "trans = " << trans <<endl; 142 143 for(int nIndex = 0; nIndex < point_cloud_ptr->points.size (); nIndex++) 144 { 145 c_x = point_cloud_ptr->points[nIndex].x; 146 c_y = point_cloud_ptr->points[nIndex].y; 147 c_z = point_cloud_ptr->points[nIndex].z; 148 149 c_tmp = (cv::Mat_<float>(4, 1) << c_x, c_y, c_z, 1); 150 p_result = trans * c_tmp; 151 152 p_w = p_result.at<float>(0,2); 153 p_u = (int)((p_result.at<float>(0,0)) / p_w); 154 p_v = (int)((p_result.at<float>(0,1)) / p_w); 155 if( (p_u<0) || (p_u>cols) || (p_v<0) || (p_v>rows) ||(p_w < 0)){ 156 point_cloud_ptr->points[nIndex].r = 128; 157 point_cloud_ptr->points[nIndex].g = 2; 158 point_cloud_ptr->points[nIndex].b = 64; 159 continue; 160 } 161 point_cloud_ptr->points[nIndex].r = img.at<cv::Vec3b>(p_v,p_u)[2];//not (p_u,p_v)! 162 point_cloud_ptr->points[nIndex].g = img.at<cv::Vec3b>(p_v,p_u)[1]; 163 point_cloud_ptr->points[nIndex].b = img.at<cv::Vec3b>(p_v,p_u)[0]; 164 } 165 } 166 167 int main() 168 { 169 std::vector<std::string> pcdFiles; // xxxxxx.pcd 170 std::vector<std::string> imgFiles; // xxxxxx.png 171 std::vector<Eigen::Matrix4d> transforms; 172 std::string posePath = "../pose/01.txt"; 173 std::string pcdPath = "../pcd"; 174 std::string imgPath = "../image"; 175 // get the names of files 176 int npcd = getFiles(pcdPath, pcdFiles, ".pcd"); // pcd 177 int nimg = getFiles(imgPath, imgFiles, ".png"); // image 178 // get the value of poses 179 transfromMatReader(posePath, transforms); // pose 180 if(npcd != nimg) 181 { 182 cout << "the num of pcds is not the same as the imgs!" << endl; 183 return -1; 184 } 185 if(npcd != (int)transforms.size()) 186 { 187 cout << "the num of pcds is not the same as the poses!" << endl; 188 } 189 std::sort(pcdFiles.begin(), pcdFiles.end(), compareVec); 190 std::sort(imgFiles.begin(), imgFiles.end(), compareVec); 191 // mapping 192 pcl::PointCloud<pcl::PointXYZRGB>::Ptr source(new pcl::PointCloud<pcl::PointXYZRGB>); 193 pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourceCut(new pcl::PointCloud<pcl::PointXYZRGB>); // cut the source 194 pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourceDS(new pcl::PointCloud<pcl::PointXYZRGB>); // subSample the source 195 pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>); 196 pcl::PointCloud<pcl::PointXYZRGB>::Ptr map(new pcl::PointCloud<pcl::PointXYZRGB>); 197 // subSample 198 pcl::VoxelGrid<pcl::PointXYZRGB> downSizeFilter; 199 float r = 0.5; 200 downSizeFilter.setLeafSize(r, r, r); 201 //visualization 202 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; 203 for (size_t i = 0; i < transforms.size(); ++i) 204 // for (size_t i = 300; i < 600; ++i) 205 { 206 cout << i <<endl; 207 pcl::io::loadPCDFile("../pcd/" + pcdFiles[i], *source); 208 cv::Mat img = cv::imread("../image/" + imgFiles[i]); 209 if(img.empty() || source->points.empty()) 210 { 211 cout << "errors in reading the data!" << endl; 212 } 213 // cut 214 for (int j = 0; j < source->points.size(); ++j) 215 { 216 pcl::PointXYZRGB p = source->points[j]; 217 if((std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)) < 400) // 距离<20 218 { 219 sourceCut->points.push_back(p); 220 } 221 } 222 // subSample 223 // downSizeFilter.setInputCloud(source); 224 // downSizeFilter.filter(*sourceDS); 225 // coloring 226 //coloringPointCloud(img, sourceDS); 227 coloringPointCloud(img, sourceCut); 228 // // debug 229 // viewer = rgbVis(sourceDS); 230 // // 主循环 231 // while (!viewer->wasStopped()) 232 // { 233 // viewer->spinOnce(100); 234 // boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 235 // } 236 237 // mapping 238 //pcl::transformPointCloud(*sourceDS, *target, transforms[i]); 239 pcl::transformPointCloud(*sourceCut, *target, transforms[i]); 240 *map = *map + *target; 241 242 // 每50帧存一次 243 if((i+1) % 50 == 0) 244 { 245 pcl::io::savePCDFileBinary("../map" + std::to_string((i+1) / 50) + ".pcd", *map); 246 map->clear(); 247 } 248 249 target->clear(); 250 sourceCut->clear(); 251 sourceDS->clear(); 252 } 253 pcl::io::savePCDFileBinary("../fuck.pcd", *map); 254 255 // viewer = rgbVis(map); 256 // // 主循环 257 // while (!viewer->wasStopped()) 258 // { 259 // viewer->spinOnce(100); 260 // boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 261 // } 262 return 0; 263 }
reference：https://blog.csdn.net/weixin_43846627/article/details/112377660
CV&DL
