kitti点云地图拼接

前言：这段时间在学习坐标系变换相关的知识，同时尝试了利用kitti公开点云数据集以及对应的真实位姿，拼接出全局地图，如下图所示，我采用了kitti点云数据集的00序列来测试拼接地图。下面大致记录下点云拼接过程以及基于C++和Matlab的拼接代码，备忘。

1. 关于kitti数据集以及坐标系

kitti数据集是一个为立体，光流，视觉里程计，3D目标检测和3D跟踪等任务而开发采集的基准数据集。它利用了Annieway自动驾驶平台，配备了1个OXTS RT 3003，1台Velodyne HDL-64E激光扫描仪，2台Point Grey Flea 2（FL2-14S3M-C）一百四十万像素灰度相机，2台一百四十万像素彩色摄像头，以及4个4~8mm的可变焦镜头Edmund Optics NT59-917。根据 Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite 这篇文章的描述，相机，激光扫描仪以及定位系统全部都经过了校准和时间同步，且里程计的真值由GPS/IMU定位单元的输出投影到校准后的左侧相机得到。也就是说，在kitti提供的里程计真值是基于相机坐标系的，而根据上面kitti链接中的介绍，相机坐标系为：向前为z，向右为x，向下为y；而velodyne激光扫描仪坐标系则是：向前x，向左y，向上z。（需要注意的是，点云数据是在激光雷达坐标系下，而真值则是在相机坐标系下。）
在从官网上下载的真值位姿具有以下格式（kitti总共有编号为00~20的21个数据集序列，其中只有00~10序列公开了真值，序列11~20仅用来做为算法评估使用）：

1.000000e+00 9.043680e-12 2.326809e-11 5.551115e-17 9.043683e-12 1.000000e+00 2.392370e-10 3.330669e-16 2.326810e-11 2.392370e-10 9.999999e-01 -4.440892e-16
9.999978e-01 5.272628e-04 -2.066935e-03 -4.690294e-02 -5.296506e-04 9.999992e-01 -1.154865e-03 -2.839928e-02 2.066324e-03 1.155958e-03 9.999971e-01 8.586941e-01
9.999910e-01 1.048972e-03 -4.131348e-03 -9.374345e-02 -1.058514e-03 9.999968e-01 -2.308104e-03 -5.676064e-02 4.128913e-03 2.312456e-03 9.999887e-01 1.716275e+00
9.999796e-01 1.566466e-03 -6.198571e-03 -1.406429e-01 -1.587952e-03 9.999927e-01 -3.462706e-03 -8.515762e-02 6.193102e-03 3.472479e-03 9.999747e-01 2.574964e+00

它们每一行有12个数据，它们记录了每一个时刻的位置和方向，对于上面的每一行，$p_0,p_1,p_2,...,p_{11}$; 它们与（旋转+平移）欧式矩阵对应关系如下：
$$T=\left[\begin{array}{cccc}{p}_0& p_1& p_2& p_3\\ p_4& p_5& p_6& p_7\\ p_8& p_9& p_{10}& p_{11}\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0_{1\times 3}& 1\end{array}\right]\text{\hspace{1em}(1.1)}$$
上面提到，这些真值位姿是在相机坐标系下的，而kitti数据集重的点云则是在激光雷达坐标系下，因此，式（1-1）重的$T$并不能直接使用，而要将其转换到激光雷达坐标系下才能实现正确的点云拼接。但是如何将相机坐标系（向前为z，向右为x，向下为y）转换到激光雷达坐标系（向前x，向左y，向上z）下呢？首先，看一下它们之间的对应关系：

转换方法为：将式（1-1）中的旋转矩阵$R_{3\times 3}$转换成四元数:$q_{camera}=(q{x}_c,q{y}_c,q{z}_c,q{w}_c)$，然后做如下变动得到$q_{lidar}=(q{z}_c,-q{x}_c,-q{y}_c,q{w}_c)$，然后再将$q_{lidar}$转换回旋转矩阵$R$，从而得到激光雷达方向下的欧式矩阵：
$$T=\left[\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0_{1\times 3}& 1\end{array}\right]\text{\hspace{1em}(1.2)}$$
有了$T$，我们拼接点云的基础就有了。注意，我这里只是把方向转换到雷达坐标系下，平移没有做任何变动（因为这不影响点云拼接，也就是说，这样拼接得到的点云地图其实是激光雷达在相机所在位置时扫描得到）。

2. 关于bin格式点云的存储方式

为了节省空间，作者将所有扫描的点云数据以Nx4浮点矩阵的形式存储到二进制文件中：

stream = fopen (dst_file.c_str(),“wb”);
fwrite(data,sizeof(float),4*num,stream);
fclose(stream);

其中，每个激光点数据包含4 * num个值，分别代表x，y，z以及intensity。所有扫描点均以行对齐方式存储，即前4个值对应于第一个（激光点）测量值。由于每次扫描可能具有不同数量的点，因此在读取文件时，必须根据文件大小来确定读取点云的数量：

// allocate 4 MB buffer (only ~13044 KB are needed)
int32_t num = 1000000;
float data = (float)malloc(num*sizeof(float));

// pointers
float *px = data+0;
float *py = data+1;
float *pz = data+2;
float *pr = data+3;

// load point cloud
FILE *stream;
stream = fopen (currFilenameBinary.c_str(),“rb”);
num = fread(data,sizeof(float),num,stream)/4;
for (int32_t i=0; i point_cloud.points.push_back(tPoint(*px,*py,*pz,*pr));
px+=4; py+=4; pz+=4; pr+=4;
}
fclose(stream);

3. 点云拼接

到这里差不多可以贴代码了，基于ROS的C++代码如下：

#include   // pcl::io::savePCDFileBinary
#include   // pcl::transformPointCloud
#include   // voxelGridFilter

#include   // tf::Matrix3x3, tf::createQuaternionMsgFromRollPitchYaw, tf::Quaternion
#include 

#include 
#include 
#include 
#include 
#include 
#include 


typedef pcl::PointXYZI pointType;

static Eigen::Matrix4f tform;
std::vector<Eigen::Matrix4f> tforms;
std::string save_map_name = "output_map.pcd";
std::string bin_path, poses_file;
std::vector<std::string> bin_names;

pcl::PointCloud<pointType>::Ptr source_cloud (new pcl::PointCloud<pointType> ());
pcl::PointCloud<pointType>::Ptr transformed_cloud (new pcl::PointCloud<pointType> ());
pcl::PointCloud<pointType>::Ptr map_cloud (new pcl::PointCloud<pointType> ());


bool vstring_compare(const std::string &x,const std::string &y)  //&符号不能少
{
  return x < y;
}

void get_bin_names(const std::string& root_path, std::vector<std::string> &names)
{
    names.clear();
    boost::filesystem::path full_path(root_path);
    boost::filesystem::recursive_directory_iterator end_iter;
    for(boost::filesystem::recursive_directory_iterator iter(full_path); iter!=end_iter; ++iter)
    {
        try
        {
            if ( !boost::filesystem::is_directory( *iter ) )
            {
                std::string file = iter->path().string();
                names.push_back(iter->path().string());   // get the golbal full path name.  获取该文件的全局路径
                // boost::filesystem::path file_path(file);
                // names.push_back(file_path.stem().string());   // get the pure name(no suffix) 获取无后缀的文件名称，即000000, 000001, ...
            }
        }
        catch ( const std::exception & ex )
        {
            std::cerr << ex.what() << std::endl;
            continue;
        }
    }   
}
// 解析得到对应的变换矩阵，并将方向转换到lidar所在坐标系下：
void get_transforms(std::string pose_file, std::vector<Eigen::Matrix4f>& tforms)
{
  std::string line;
  std::ifstream ifs;
  ifs.open(pose_file, std::ios::in);
  if (!ifs)
  {
    std::cout << "cannot open file: " << pose_file << std::endl;
    return ;
  }
  while (std::getline(ifs, line) && ifs.good())
  {
    if (line.empty()) return;
    std::stringstream lineStream(line);
    std::string cell;
    std::vector<double> vdata;
    while (std::getline(lineStream, cell, ' '))
    {
      vdata.push_back(std::stod(cell));
    }

    double roll, pitch, yaw;
    Eigen::Matrix4f tform;
    tf::Matrix3x3 tf_mat;
    tf_mat.setValue(vdata[0], vdata[1], vdata[2], vdata[4], vdata[5], vdata[6], vdata[8], vdata[9], vdata[10]);
    tf_mat.getRPY(roll, pitch, yaw);
    geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
    tf_mat.setRotation(tf::Quaternion(quat.z, -quat.x, -quat.y, quat.w));
    tform(0, 0) = tf_mat[0][0]; tform(0, 1) = tf_mat[0][1]; tform(0, 2) = tf_mat[0][2]; tform(0, 3) = vdata[11];
    tform(1, 0) = tf_mat[1][0]; tform(1, 1) = tf_mat[1][1]; tform(1, 2) = tf_mat[1][2]; tform(1, 3) = -vdata[3];
    tform(2, 0) = tf_mat[2][0]; tform(2, 1) = tf_mat[2][1]; tform(2, 2) = tf_mat[2][2]; tform(2, 3) = -vdata[7];
    tform(3, 0) = 0; tform(3, 1) = 0; tform(3, 2) = 0; tform(3, 3) = 1;
    tforms.push_back(tform);
    // static int count = 0;
    // std::cout << count++ << "transform: \n" << tform << std::endl;
  }
}
// 解析.bin格式的点云文件，转换到标准的pcl::PointXYZI的格式：
void parse_bin_cloud(const std::string& bin_file, pcl::PointCloud<pointType>& points)
{
  points.points.clear();
  std::fstream input(bin_file.c_str(), std::ios::in | std::ios::binary);
  if(!input.good())
  {
    std::cerr << "Could not read file: " << bin_file << std::endl;
    exit(EXIT_FAILURE);
  }
  // bin2points:
  input.seekg(0, std::ios::beg);

  for (int i=0; input.good() && !input.eof(); i++)
  {
    pointType point;
    input.read((char *) &point.x, 3*sizeof(float));
    input.read((char *) &point.intensity, sizeof(float));
    points.points.push_back(point);
  }
  input.close();
}
// 对点云进行将采用，减小对系统资源的占用，加快程序运行：
void voxel_grid_filter(const pcl::PointCloud<pointType>::Ptr& source_cloud, pcl::PointCloud<pointType>::Ptr& filtered_cloud, const double& voxel_leaf_size)
{
  pcl::VoxelGrid<pointType> voxel_filter;
  voxel_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
  voxel_filter.setInputCloud(source_cloud);
  voxel_filter.filter(*filtered_cloud);
  return;
}
// 拼接点云地图
void joint_map(const std::vector<std::string>& bin_names, const std::vector<Eigen::Matrix4f>& tforms)
{
  for (int i = 0; i < bin_names.size(); ++i)
  {
    if (!ros::ok()) break;
    // convert kitti lidar data *.bin to pcl pointcloud type:
    parse_bin_cloud(bin_names[i], *source_cloud);
    std::cout << "get points: " << source_cloud->points.size() << std::endl;
    // transform the point cloud:
    pcl::transformPointCloud(*source_cloud, *transformed_cloud, tforms[i]);
    // voxel grid filter the cloud:
    voxel_grid_filter(transformed_cloud, transformed_cloud, 0.3);
    // point cloud merge:
    if (i == 0)
    {
      *map_cloud = *transformed_cloud;
      continue;
    }
    *map_cloud += *transformed_cloud;
    std::cout << i+1 << ", map cloud points: " << map_cloud->points.size() << std::endl;
  }
  // voxel grid filter the output map:
  voxel_grid_filter(map_cloud, map_cloud, 0.9);
  // save pointcloud to pcd:
  pcl::io::savePCDFileBinary(save_map_name, *map_cloud);
  // show save map info:
  std::cout << "the map " << save_map_name << " is saved with " <<  map_cloud->points.size() << " points" << std::endl;
}


int main(int argc, char** argv) 
{
	ros::init(argc, argv, "joint_kitti_bin2point_map");
  ros::NodeHandle nh;

  if (argc < 3)
  {
    std::cout << "Usage: bin_name bin_path pose_name output_map_name(optional). " << std::endl;
    return 0;
  }
  bin_path = argv[1];
  poses_file = argv[2];
  if (argc >= 4)
  {
    save_map_name = argv[3];
  }

  get_bin_names(bin_path, bin_names);   // get the kitti lidar bin file names.
  sort(bin_names.begin(), bin_names.end(), vstring_compare);  // sort the names.（由于get_bin_names函数得到的文件路径是无序的，这里将它按照000000.bin, 000001.bin的顺序排列好）
  get_transforms(poses_file, tforms);  // get the kitti global poses and convert it to transform: Eigen::Matrix4f.
  // for (size_t i = 0; i < bin_names.size(); ++i)  // print the bin name and its affine matrix.
  // {
  //   std::cout << bin_names[i] << std::endl;
  //   std::cout << tforms[i] << std::endl;
  // }
  joint_map(bin_names, tforms);   // joint the map.

	return 0;
}

基于matlab的kitti点云拼接代码如下：

close all; clear all; clc

addpath('../../mylibrary');  % 其中包含了parse_kitti_transform和parse_kitti_bin两个基本函数。

%% kitti format
bin_path = 'E:/data/kitti_lidar/00/velodyne'; 	% kitti点云数据序列路径
poses_file = 'E:/data/kitti_lidar/dataset/poses/00.txt' 	% 真实位姿所在路径
files = dir(fullfile(bin_path, '*.bin'));
poses = load(poses_file);


for fn = 1 : length(files)
    tform = parse_kitti_transform(poses(fn, :));  % 坐标变换
    bin_file = [files(fn).folder '\' files(fn).name] 	% 得到每个bin文件的全局路径
    source_cloud = parse_kitti_bin(bin_file); 	% 将bin格式点云数据解析成matlab下的标准点云类型
    source_cloud = pctransform(source_cloud, tform);
    source_cloud = pcdownsample(source_cloud,'gridAverage',0.3);  % 将采用，加速算法过程，同时减少对系统资源的占用
    if fn == 1
        points_map = source_cloud;
        continue;
    end
    points_map = pcmerge(points_map, source_cloud, 0.9);
end

pcshow(points_map); 	% 点云地图显示
pcwrite(points_map, 'output_map.pcd'); 	点云地图保存

parse_kitti_bin函数，解析bin格式的二进制编码的点云，将其转换成matlab下的标准点云数据类型。

function [ptCloud] = parse_kitti_bin(bin_name)
%parse_kitti_bin 用于解析 kitti 点云数据集中 bin 格式的点云二进制文件
%   参数 bin_name 表示 bin 格式的点云文件的路径

fid = fopen(bin_name,'rb');
velo = fread(fid, [inf], 'single');  % x,y,z,intensity,x,y,z,intensity,...
velo = reshape(velo, 4, length(velo)/4)';
fclose(fid);
ptCloud = pointCloud(velo(:, 1:3));
ptCloud.Intensity = velo(:, 4);
end

parse_kitti_transform函数，将真值坐标方向转换到激光雷达所对应的方向下，对应式（1.1）到（1.2）的转化：

function [tform] = parse_kitti_transform(pose)
    tform = affine3d();
    tmp = reshape(pose, 4, 3)';
    R = tmp(1:3, 1:3);
    q = rotm2quat(R);
    R = quat2rotm([q(1), q(4), -q(2), -q(3)]);
    ts = tmp(1:3, 4);
    T(1:3, 1:3) = R';
    T(1:3, 4) = 0;
    T(4, 1:3) = [ts(3), -ts(1), -ts(2)];
    T(4, 4) = 1;
    tform.T = T;
    tform.T
end

这里有两点需要注意一下：1）四元数在ROS和matlab中的表示顺序不同。其中的表达顺序是（qx, qy, qz, qw），而它在matlab中的表达顺序则是（qw, qx, qy, qz），这里需要注意一下；2）在pcl中的pcl::transformPointCloud(*source_cloud, *transformed_cloud, tform)函数与matlab中的pctransform(source_cloud, tform)函数中，它们的tform是不一样的：

----------- pcl::transformPointCloud函数中的tform：------------------
0.998834  -0.0458878  -0.0150214     96.9615
0.0457509    0.998909 -0.00933175     5.58393
0.0154332  0.00863363    0.999844     3.56276
0           0           0           1
----------------matlab的pctransform函数中的tform: -------------------
0.9988    0.0458    0.0154         0
-0.0459    0.9989    0.0086         0
-0.0150   -0.0093    0.9998         0
96.9615    5.5839    3.5628    1.0000

利用上面的C++和matlab代码均可拼接得到全局地图，从真值位姿中任意选取一个位姿，得到它们各自对应的转换矩阵，如上面所示（它们值不完全一样只是因为打印时的显示精度不一致而已）。可以看到，pcl和matlab点云转换函数的转换矩阵参数形式不同，它们互成转置关系，这里需要注意一下。