真值数据和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
标签：std,pose,彩色,地图,pcl,kitti,ptr,cloud,data2 From： https://www.cnblogs.com/feiyull/p/15348190.html