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

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