在 KITTI 数据集中利用车辆位姿真值拼接 pcd 点云并滤波,得到一个准确的点云地图:
全部代码如下(C++):
#include "stdlib.h"
#include <unistd.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/common/projection_matrix.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <fstream>
#include <cassert>
#include <string>
#include <iostream>
#include <sys/types.h>
#include <dirent.h>
#include <vector>
#include <string.h>
using namespace std;
// ubuntu 系统下读取指定目录下的所有文件名及其绝对路径
void GetFileNames(string path,vector<string>& filenames)
{
DIR *pDir;
struct dirent* ptr;
if(!(pDir = opendir(path.c_str())))
return;
while((ptr = readdir(pDir))!=0) {
if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
filenames.push_back(path + "/" + ptr->d_name);
}
closedir(pDir);
}
int main()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr allcloud (new pcl::PointCloud<pcl::PointXYZRGB>); // 创建点云(指针)
pcl::visualization::CloudViewer viewer("viewer"); // 用来显示拼接后的点云
vector<Eigen::Isometry3d> Trans = {Eigen::Isometry3d::Identity()};
Eigen::Isometry3d T12 = Eigen::Isometry3d::Identity();
/*T12(0,0) = 1 ;T12(0,1) = 0 ;T12(0,2) = 0 ;
T12(1,0) = 0 ;T12(1,1) = 0 ;T12(1,2) = -1 ;
T12(2,0) = 0 ;T12(2,1) = 1 ;T12(2,2) = 0 ;*/
Eigen::Isometry3d T_out = Eigen::Isometry3d::Identity();
//从激光到相机坐标系的外参,从 KITTI 的 Odometry 数据集中的 calib 文件夹下的 sequence_01 序列得到
T_out(0,0)= 4.2e-04 ;T_out(0,1) = -9.99e-01 ;T_out(0,2) = -8.08e-03 ;T_out(0,3) = -1.198e-02 ;
T_out(1,0) = -7.21e-03 ;T_out(1,1) = 8.08e-03 ;T_out(1,2) = -9.999e-01; T_out(1,3) = -5.40e-02 ;
T_out(2,0) = 9.9997e-01 ;T_out(2,1) =4.8594e-04 ;T_out(2,2) = -7.20e-03 ;T_out(2,3) = -2.92e-01;
cout<<"T12 = "<<T12.matrix()<<endl;
//读取GroundTruth文件(车辆位姿真值文件),在 KITTI 的 Odometry 数据集中的 poses 文件夹内得到,和 01 序列对应
ifstream fin("/home/clzx/ZhangYong/01.txt");
vector<string> filename;
string path = "/home/clzx/ZhangYong/rgbd"; // 存放待拼接的 pcd 点云文件的目录
GetFileNames(path, filename);
string feature;
float feat_onepoint;
vector<float>lines;
for(int i = 0; i < filename.size(); i++)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Isometry3d transformation = Eigen::Isometry3d::Identity();
Eigen::Isometry3d transformation2 = Eigen::Isometry3d::Identity();
getline(fin,feature); //一次读取一行数据
stringstream stringin(feature); //使用串流实现对string的输入输出操作
lines.clear();
while(stringin >> feat_onepoint) //按空格一次读取一个数据存入feat_onePoint
lines.push_back(feat_onepoint); //存储每行按空格分开的数据
//cout<<"lines = "<<lines[0]<<endl;
for(int m=0;m<3;m++)
for(int n=0;n<4;n++)
transformation(m,n) = lines[m*4+n];
cout<<"第"<<i<<"帧"<<endl;
//cout<<" 原来的transformation = "<<transformation.matrix()<<endl;
// 因为我的点云文件已经转换到了参考相机(00 相机)坐标系下,所以不需要再乘以 T_out(从激光坐标系到参考相机(00 相机)坐标系的外参)了,如果没有转换的则要乘以外参。
//transformation2 = transformation*T_out;
transformation2 = transformation;
//cout<<" 转换后的transformation = "<<transformation2.matrix()<<endl;
pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename[i], *pointcloud);
pcl::transformPointCloud( *pointcloud, *outcloud, transformation2.matrix() );
//体素滤波
pcl::VoxelGrid<pcl::PointXYZRGB> downSampled; //创建滤波对象
downSampled.setInputCloud (outcloud); //设置需要过滤的点云给滤波对象
downSampled.setLeafSize (0.2f,0.2f, 0.2f); //设置滤波时创建的体素体积为1cm的立方体,三个参数表示体素栅格叶大小,分别表示体素在XYZ方向的尺寸
downSampled.setDownsampleAllData(true); //设置是否对所有的字段进行下采样
downSampled.filter (*downcloud); //执行滤波处理,存储输出
*allcloud = *allcloud + *downcloud;
viewer.showCloud(allcloud); // 刷新显示点云
}
pcl::io::savePCDFile("/home/clzx/ZhangYong/result.pcd", *allcloud); //拼接后的pcd文件保存目录
cout<<"Final result saved."<<endl;
while( !viewer.wasStopped() )
{
}
}
注意!!
1、KITTI 中的 Odometry 数据集中的 11 个序列与 KITTI 中的 raw data 数据集具有对应关系,对应关系可以参考:1、kitti数据集 Raw Data与00-10 Ground Truth的对应关系,2、KITTI 00 ~ KITTI10 下载地址,3、KITTI中的pose数据集使用
2、KITTI 的 Odometry 数据集中的 calib 文件夹内有 calib.txt 文件,
P0: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 0.000000000000e+00 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P1: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.861448000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00
P2: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 4.538225000000e+01 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 -1.130887000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.779761000000e-03
P3: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.372877000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 2.369057000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.915215000000e-03
Tr: 4.276802385584e-04 -9.999672484946e-01 -8.084491683471e-03 -1.198459927713e-02 -7.210626507497e-03 8.081198471645e-03 -9.999413164504e-01 -5.403984729748e-02 9.999738645903e-01 4.859485810390e-04 -7.206933692422e-03 -2.921968648686e-01
其中 0,1,2,3 代表相机的编号,0表示左边灰度相机,1右边灰度相机,2左边彩色相机,3右边彩色相机,P0、P1、P2、P3分别是四个相机的内参。Tr 表示从激光坐标系到参考相机坐标系的经过校正后的外参,参考相机坐标系就是 0 相机(左边灰度相机)坐标系。
P0 表示把激光点从参考相机坐标系投影到编号为 0 的相机采集的图像中,P1 表示把激光点从参考相机坐标系投影到编号为 0 的相机采集的图像中,P2 表示把激光点从参考相机坐标系投影到编号为 2 的相机采集的图像中,P3 表示把激光点从参考相机坐标系投影到编号为 3 的相机采集的图像中。
KITTI 的 raw 数据集中的 calib 文件夹内有 calib_velo_to_cam.txt 文件和 calib_cam_to_cam.txt 文件,
calib_velo_to_cam.txt 文件中的 R 和 T 分别代表从激光坐标系到参考相机(0相机)坐标系的旋转矩阵和平移向量,delta_f 和 delta_c 已废弃。
calib_time: 15-Mar-2012 11:45:23
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
T: -1.377769e-02 -5.542117e-02 -2.918589e-01
delta_f: 0.000000e+00 0.000000e+00
delta_c: 0.000000e+00 0.000000e+00
calib_cam_to_cam.txt 文件:
calib_time: 09-Jan-2012 14:00:15
corner_dist: 9.950000e-02
S_00: 1.392000e+03 5.120000e+02
K_00: 9.799200e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.741183e+02 2.486443e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_00: -3.745594e-01 2.049385e-01 1.110145e-03 1.379375e-03 -7.084798e-02
R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
T_00: -9.251859e-17 8.326673e-17 -7.401487e-17
S_rect_00: 1.241000e+03 3.760000e+02
R_rect_00: 9.999454e-01 7.259129e-03 -7.519551e-03 -7.292213e-03 9.999638e-01 -4.381729e-03 7.487471e-03 4.436324e-03 9.999621e-01
P_rect_00: 7.188560e+02 0.000000e+00 6.071928e+02 0.000000e+00 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_01: 1.392000e+03 5.120000e+02
K_01: 9.903522e+02 0.000000e+00 7.020000e+02 0.000000e+00 9.855674e+02 2.607319e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_01: -3.712084e-01 1.978723e-01 -3.709831e-05 -3.440494e-04 -6.724045e-02
R_01: 9.993440e-01 1.814887e-02 -3.134011e-02 -1.842595e-02 9.997935e-01 -8.575221e-03 3.117801e-02 9.147067e-03 9.994720e-01
T_01: -5.370000e-01 5.964270e-03 -1.274584e-02
S_rect_01: 1.241000e+03 3.760000e+02
R_rect_01: 9.996568e-01 -1.110284e-02 2.372712e-02 1.099810e-02 9.999292e-01 4.539964e-03 -2.377585e-02 -4.277453e-03 9.997082e-01
P_rect_01: 7.188560e+02 0.000000e+00 6.071928e+02 -3.861448e+02 0.000000e+00 7.188560e+02 1.852157e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S_02: 1.392000e+03 5.120000e+02
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
D_02: -3.685917e-01 1.928022e-01 4.069233e-04 7.247536e-04 -6.276909e-02
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
T_02: 5.954406e-02 -7.675338e-04 3.582565e-03
S_rect_02: 1.241000e+03 3.760000e+02
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
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_03: 1.392000e+03 5.120000e+02
K_03: 9.049931e+02 0.000000e+00 6.957698e+02 0.000000e+00 9.004945e+02 2.389820e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_03: -3.735725e-01 2.066816e-01 -6.133284e-04 -1.193269e-04 -7.600861e-02
R_03: 9.995578e-01 1.656369e-02 -2.469315e-02 -1.663353e-02 9.998582e-01 -2.625576e-03 2.464616e-02 3.035149e-03 9.996916e-01
T_03: -4.738786e-01 5.991982e-03 -3.215069e-03
S_rect_03: 1.241000e+03 3.760000e+02
R_rect_03: 9.998092e-01 -9.354781e-03 1.714961e-02 9.382303e-03 9.999548e-01 -1.525064e-03 -1.713457e-02 1.685675e-03 9.998518e-01
P_rect_03: 7.188560e+02 0.000000e+00 6.071928e+02 -3.372877e+02 0.000000e+00 7.188560e+02 1.852157e+02 2.369057e+00 0.000000e+00 0.000000e+00 1.000000e+00 4.915215e-03
如果想把雷达点云投影到 02 相机(左侧彩色相机)拍摄的图片上,需要的参数有:
- calib_velo_to_cam.txt 中的 R(旋转矩阵)
- calib_velo_to_cam.txt 中的 T(平移向量)
- calib_cam_to_cam.txt 中的 R_rect_00(3x3 纠正旋转矩阵(使图像平面共面,kitti特有的))
- calib_cam_to_cam.txt 中的 P_rect_02(02 相机内参,可以把激光点从参考相机坐标系投影到编号为 2 的相机采集的图像中)
公式:
- RT_velo_to_cam * x :是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机(参考相机)坐标系中
- R_rect00 *RT_velo_to_cam * x :是将 Velodyne 坐标中的点 x 投影到编号为0的相机(参考相机)坐标系中, 再以参考相机 0 为基础进行图像共面对齐修正(这是使用KITTI数据集的进行3D投影的必要操作)
- P_rect_00 * R_rect00 *RT_velo_to_cam * x :是将 Velodyne 坐标中的点 x 投影到编号为0的相机(参考相机)坐标系中, 再以参考相机 0 为基础进行图像共面对齐修正, 然后投影到相机 0 的像素坐标系中. 如果将 P_rect_00 改成 P_rect_02, 就是从参考相机 0 坐标系投影到相机 2 的像素坐标系中(其他相机相对与相机0有偏移b(i)).
其中,R_rect00 * RT_velo_to_cam 的结果与 KITTI 的 Odometry 数据集中的 calib 文件夹内的 calib.txt 文件的 Tr 矩阵一模一样!!
而 P_rect_00、P_rect_01、P_rect_02、P_rect_03 也分别与 KITTI 的 Odometry 数据集中的 calib 文件夹内的 calib.txt 文件的 P0、P1、P2、P3 一模一样!!
参考文献:
1、python_kitti_激光点云变换到图像平面并保存成int16灰度图
2、KITTI数据集 激光雷达-图像坐标系转换关系
3、KITTI数据集测试 - 3 calib 相机参数