KITTI-二进制点云数据集使用笔记

欢迎访问我的个人Blog：zengzeyu.com

前言

官网数据集说明：http://www.cvlibs.net/datasets/kitti/raw_data.php
数据集详细说明论文：http://www.cvlibs.net/publications/Geiger2013IJRR.pdf
KITTI的激光雷达型号为 Velodyne HDL-64E ，具体信息如下：

Velodyne HDL-64E rotating 3D laser scanner
- 10 Hz
- 64 beams
- 0.09 degree angular resolution
- 2 cm distanceaccuracy
- collecting∼1.3 million points/second
- field of view: 360°
- horizontal, 26.8°
- vertical, range: 120 m

针对激光雷达点云数据集使用的信息在 KITTI_README.TXT 中有详细说明，文件下载地址：Code to use the KITTI data set with PCL

Velodyne 3D laser scan data
===========================

The velodyne point clouds are stored in the folder 'velodyne_points'. To
save space, all scans have been stored as Nx4 float matrix into a binary
file using the following code:

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

Here, data contains 4*num values, where the first 3 values correspond to
x,y and z, and the last value is the reflectance information. All scans
are stored row-aligned, meaning that the first 4 values correspond to the
first measurement. Since each scan might potentially have a different
number of points, this must be determined from the file size when reading
the file, where 1e6 is a good enough upper bound on the number of values:

  // allocate 4 MB buffer (only ~130*4*4 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<num; i++) {
    point_cloud.points.push_back(tPoint(*px,*py,*pz,*pr));
    px+=4; py+=4; pz+=4; pr+=4;
  }
  fclose(stream);

x,y and y are stored in metric (m) Velodyne coordinates.

KITTI点云数据集读取与转换

官方源代码解读

Code to use the KITTI data set with PCL下载的源代码文件夹中的src/kitti2pcd.cpp 中这个函数：

void readKittiPclBinData(std::string &in_file, std::string& out_file)
{
    // load point cloud
    std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary);
    if(!input.good()){
        std::cerr << "Could not read file: " << in_file << std::endl;
        exit(EXIT_FAILURE);
    }
    input.seekg(0, std::ios::beg);

    pcl::PointCloud<pcl::PointXYZI>::Ptr points (new pcl::PointCloud<pcl::PointXYZI>);

    int i;
    for (i=0; input.good() && !input.eof(); i++) {
        pcl::PointXYZI point;
        input.read((char *) &point.x, 3*sizeof(float));
        input.read((char *) &point.intensity, sizeof(float));
        points->push_back(point);
    }
    input.close();
//    g_cloud_pub.publish( points );

    std::cout << "Read KTTI point cloud with " << i << " points, writing to " << out_file << std::endl;
    pcl::PCDWriter writer;

    // Save DoN features
    writer.write< pcl::PointXYZI > (out_file, *points, false);
}

这个函数是最重要的从 KITTI 中读取 .bin 文件转 .pcd 文件。

可运行完整代码

下面贴本人完整代码，代码功能：

读取文件夹下.bin 文件
按照文件名进行排序（虽然默认已经排好序）
转为.pcd 文件，并保存
发送到 rviz 进行显示（可选）

//
// Created by zzy on 3/14/18.
//

#include <ctime>
#include "ros/ros.h"
#include "fcn_data_gen/ground_remove.h"

static ros::Publisher g_cloud_pub;
static std::vector<std::string> file_lists;

void read_filelists(const std::string& dir_path,std::vector<std::string>& out_filelsits,std::string type)
{
    struct dirent *ptr;
    DIR *dir;
    dir = opendir(dir_path.c_str());
    out_filelsits.clear();
    while ((ptr = readdir(dir)) != NULL){
        std::string tmp_file = ptr->d_name;
        if (tmp_file[0] == '.')continue;
        if (type.size() <= 0){
            out_filelsits.push_back(ptr->d_name);
        }else{
            if (tmp_file.size() < type.size())continue;
            std::string tmp_cut_type = tmp_file.substr(tmp_file.size() - type.size(),type.size());
            if (tmp_cut_type == type){
                out_filelsits.push_back(ptr->d_name);
            }
        }
    }
}

bool computePairNum(std::string pair1,std::string pair2)
{
    return pair1 < pair2;
}

void sort_filelists(std::vector<std::string>& filists,std::string type)
{
    if (filists.empty())return;

    std::sort(filists.begin(),filists.end(),computePairNum);
}

void readKittiPclBinData(std::string &in_file, std::string& out_file)
{
    // load point cloud
    std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary);
    if(!input.good()){
        std::cerr << "Could not read file: " << in_file << std::endl;
        exit(EXIT_FAILURE);
    }
    input.seekg(0, std::ios::beg);

    pcl::PointCloud<pcl::PointXYZI>::Ptr points (new pcl::PointCloud<pcl::PointXYZI>);

    int i;
    for (i=0; input.good() && !input.eof(); i++) {
        pcl::PointXYZI point;
        input.read((char *) &point.x, 3*sizeof(float));
        input.read((char *) &point.intensity, sizeof(float));
        points->push_back(point);
    }
    input.close();
//    g_cloud_pub.publish( points );

    std::cout << "Read KTTI point cloud with " << i << " points, writing to " << out_file << std::endl;
    pcl::PCDWriter writer;

    // Save DoN features
    writer.write< pcl::PointXYZI > (out_file, *points, false);
}


int main(int argc, char **argv)
{
//    ros::init(argc, argv, "ground_remove_test");
//    ros::NodeHandle n;
//    g_cloud_pub = n.advertise< pcl::PointCloud< pcl::PointXYZI > > ("point_chatter", 1);

    std::string bin_path = "../velodyne/binary/";
    std::string pcd_path = "../velodyne/pcd/";
    read_filelists( bin_path, file_lists, "bin" );
    sort_filelists( file_lists, "bin" );
    for (int i = 0; i < file_lists.size(); ++i)
    {
        std::string bin_file = bin_path + file_lists[i];
        std::string tmp_str = file_lists[i].substr(0, file_lists[i].length() - 4) + ".pcd";
        std::string pcd_file = pcd_path + tmp_str;
        readKittiPclBinData( bin_file, pcd_file );
    }

    return 0;
}

以上。