基于GT（ground truth）的kitti点云数据拼接建图
在linux系统上通过kitti数据集提供的Ground Truth(00.txt)对点云数据进行拼接，实现一个完成的稠密点云地图。上面是实现的效果图，使用了400帧的点云帧，下面是一些重要部分与完整代码。
点云读取
//读取xxx.bin形式的二进制点云文件到pcl::PointCloud中
bool loadPointCloudBinary(const std::string filePath, pcl::PointCloud::Ptr &point_cloud_ptr){
// Initialization
int32_t num = 1000000; // maximum Number of points to allocate
float* data = (float*) malloc(num * sizeof(float));
float* px = data + 0;
float* py = data + 1;
float* pz = data + 2;
float* pr = data + 3;
// load point cloud from file
std::FILE* stream;
stream = fopen(filePath.c_str(), "rb");
// Save data to variable
num = fread(data, sizeof(float), num, stream)/4;
//
//pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud);
point_cloud_ptr->height = 1;
point_cloud_ptr->width = num;
point_cloud_ptr->points.resize(num);
// Format data as desired
for (int32_t i = 0; i < num; i++) {
point_cloud_ptr->points[i].x = (float)*px;
point_cloud_ptr->points[i].y = (float)*py;
point_cloud_ptr->points[i].z = (float)*pz;
point_cloud_ptr->points[i].intensity = (float)*pr;
px+=4; py+=4; pz+=4; pr+=4;
}
// Close Stream and Free Memory
fclose(stream);
free(data);
// Feedback and return
return true;
} Groundtruth读取
//读取00.txt文件，每一行12个数据，表示一个变换矩阵，用Eigen::Matrix4f存储
vector tras;
string trapath = "../00.txt";
ifstream fin(trapath);
int frameNum = 400;
int tmpN = 0;
while(!fin.eof()&&++tmpN>a00>>a01>>a02>>a03>>a10>>a11>>a12>>a13>>a20>>a21>>a22>>a23;
Eigen::Matrix4f ttra;
ttra< 点云滤波
//对于每一帧点云进行滤波，包括体素滤波和距离滤波，减少误差较大的点云
void pointSample(pcl::PointCloud::Ptr & point, float size = 0.5){
// //体素滤波器
pcl::VoxelGrid voxel;
pcl::PointCloud::Ptr samcloud(new pcl::PointCloud);
voxel.setInputCloud( point );
voxel.setLeafSize(size, size, size);
voxel.filter( *samcloud );
pcl::PointCloud::Ptr cloudDS(new pcl::PointCloud);
for(int i=0;ipoints.size();i++){
float *px_t = &samcloud->points[i].x;
float *py_t = &samcloud->points[i].y;
float *pz_t = &samcloud->points[i].z;
double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t));
if(distance<20){
cloudDS->push_back(samcloud->points[i]);
}
}
*point = *cloudDS;
} 点云坐标转换与拼接
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
KITTI 的 Odometry 数据集中的 calib 文件夹内有 calib.txt 文件
kitti数据集提供的点云文件（例如0000.bin）是在雷达坐标系(velodyne)下的值，而GT（00.txt）里提供的变换是每个时刻相机坐标系（cam0）到世界坐标系的变换，因此整体思路是先将点云从雷达坐标系变换到相机坐标系(cam0)，然后再从相机坐标系变换到世界坐标系
//将点云从雷达坐标系变换到相机坐标系
void transToCam(pcl::PointCloud::Ptr & point){
pcl::PointCloud::Ptr pointCam(new pcl::PointCloud);
//变换矩阵，即图中velo_to_cam,可以从kitti标定文件中得到
cv::Mat RotationMatrix = (cv::Mat_(4,4)<<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.000000000000e+00, 0.000000000000e+00, 1.000000000000e+00, 1.0);
Eigen::Matrix4f transNow;
cv::cv2eigen(RotationMatrix,transNow);
pcl::transformPointCloud(*point, *pointCam, transNow);
*point = *pointCam;
}
void traAndPoint(vector &tras, vector &binNames){
//定义拼接的全局点云
pcl::PointCloud::Ptr gobalPoint(new pcl::PointCloud);
for(int i=0;i::Ptr point(new pcl::PointCloud);
//读取点云
loadPointCloudBinary(binNames[i], point);
//下采样--体素滤波--距离滤波
pointSample(point, 1.0);
//将点云从雷达坐标系变换到相机坐标系（camer_0）
transToCam(point);
//将点云从相机坐标系变换到世界坐标系
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
pcl::transformPointCloud( *point, *cloud, tras[i]);
//进行点云的拼接
*gobalPoint += *cloud;
cout<<"gobalPoint size: "<points.size()<::Ptr gobalPointDS(new pcl::PointCloud);
gobalPoint->is_dense = false; //防止全局点云因为无效值计算出错
voxel.setInputCloud(gobalPoint);
voxel.setLeafSize(0.3,0.3,0.3);
voxel.filter(*gobalPointDS);
cout<<"gobalPoint size: "<points.size()< 完整代码
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
pcl::VoxelGrid voxel;
//点云读取到pcl::PointCloud中
bool loadPointCloudBinary(const std::string filePath, pcl::PointCloud::Ptr &point_cloud_ptr){
// Initialization
int32_t num = 1000000; // maximum Number of points to allocate
float* data = (float*) malloc(num * sizeof(float));
float* px = data + 0;
float* py = data + 1;
float* pz = data + 2;
float* pr = data + 3;
// load point cloud from file
std::FILE* stream;
stream = fopen(filePath.c_str(), "rb");
// Save data to variable
num = fread(data, sizeof(float), num, stream)/4;
//
//pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud);
point_cloud_ptr->height = 1;
point_cloud_ptr->width = num;
point_cloud_ptr->points.resize(num);
// Format data as desired
for (int32_t i = 0; i < num; i++) {
point_cloud_ptr->points[i].x = (float)*px;
point_cloud_ptr->points[i].y = (float)*py;
point_cloud_ptr->points[i].z = (float)*pz;
point_cloud_ptr->points[i].intensity = (float)*pr;
px+=4; py+=4; pz+=4; pr+=4;
}
// Close Stream and Free Memory
fclose(stream);
free(data);
// Feedback and return
return true;
}
void showPoint(pcl::PointCloud::Ptr &point){
// 创建PCLVisualizer
boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
// 设置背景色为灰色（非必须）
viewer->setBackgroundColor (0.05, 0.05, 0.05, 0);
//按照z字段进行染色
pcl::visualization::PointCloudColorHandlerGenericField fildColor(point, "y");
viewer->addPointCloud (point, fildColor, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
// 添加一个0.5倍缩放的坐标系（非必须）
viewer->addCoordinateSystem (0.5);
// 自定义计时
// clock_t start , end;
// start = clock();
// end = clock();
// 直到窗口关闭才结束循环
while (!viewer->wasStopped()) {
// 每次循环调用内部的重绘函数
viewer->spinOnce();
//end = clock();
}
viewer->close();
}
void pointSample(pcl::PointCloud::Ptr & point, float size = 0.5){
// //体素滤波器
pcl::VoxelGrid voxel;
pcl::PointCloud::Ptr samcloud(new pcl::PointCloud);
voxel.setInputCloud( point );
voxel.setLeafSize(size, size, size);
voxel.filter( *samcloud );
//距离滤波
pcl::PointCloud::Ptr cloudDS(new pcl::PointCloud);
for(int i=0;ipoints.size();i++){
float *px_t = &samcloud->points[i].x;
float *py_t = &samcloud->points[i].y;
float *pz_t = &samcloud->points[i].z;
double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t));
if(distance<20){
cloudDS->push_back(samcloud->points[i]);
}
}
*point = *cloudDS;
}
//从雷达坐标系变换到相机坐标系
void transToCam(pcl::PointCloud::Ptr & point){
pcl::PointCloud::Ptr pointCam(new pcl::PointCloud);
cv::Mat RotationMatrix = (cv::Mat_(4,4)<<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.000000000000e+00, 0.000000000000e+00, 1.000000000000e+00, 1.0);
Eigen::Matrix4f transNow;
cv::cv2eigen(RotationMatrix,transNow);
pcl::transformPointCloud(*point, *pointCam, transNow);
*point = *pointCam;
}
//变换与点云拼接
void traAndPoint(vector &tras, vector &binNames){
pcl::PointCloud::Ptr gobalPoint(new pcl::PointCloud);
for(int i=0;i::Ptr point(new pcl::PointCloud);
loadPointCloudBinary(binNames[i], point);
//下采样--体素滤波--距离滤波
pointSample(point, 1.0);
//变换到相机坐标系
transToCam(point);
//变换到世界坐标系
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
pcl::transformPointCloud( *point, *cloud, tras[i]);
*gobalPoint += *cloud;
cout<<"gobalPoint size: "<points.size()<::Ptr gobalPointDS(new pcl::PointCloud);
gobalPoint->is_dense = false;
voxel.setInputCloud(gobalPoint);
voxel.setLeafSize(0.3,0.3,0.3);
voxel.filter(*gobalPointDS);
cout<<"gobalPoint size: "<points.size()< binNames;
vector tras;
//GT文件路径
string trapath = "../00.txt";
ifstream fin(trapath);
int frameNum = 400;
int tmpN = 0;
while(!fin.eof()&&++tmpN>a00>>a01>>a02>>a03>>a10>>a11>>a12>>a13>>a20>>a21>>a22>>a23;
Eigen::Matrix4f ttra;
ttra< stream(boost::filesystem::directory_iterator{binpath}, boost::filesystem::directory_iterator{});
sort(stream.begin(), stream.end());
auto streamIterator = stream.begin();
int j = 0;
cv::Mat pointMat;
while(streamIterator != stream.end()){
string binfile((*streamIterator).string());
binNames.push_back(binfile);
cout<> str;
streamIterator++;
j++;
if(j>frameNum)
break;
}
traAndPoint(tras, binNames);
}