KITTI 点云数据集无序点转有序点

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

前言

KITTI 点云数据集 bin 格式转 pcd 格式请参照本人博客文章：《KITTI - 二进制点云数据集》。
KITTI下载点云数据集为 unorganised ，这为计算带来了麻烦，本文将无序点云进行排序生成有序点（organiesd）。

解决方案

KITTI 点云数据集是原始激光雷达点云经过了预处理之后的点云，预处理包括：

将高度为2米以上点过滤（2米为估计，没有考证）
噪点过滤

思路 1：

将垂直方向上的激光束按照64个水平高度格子进行分类
在每一个水平高度上，按照水平角度分辨率计算此排激光束排序
根据水平面上的 x、y 坐标值进行排序

预期问题：

每一束激光的角度对应格子，不一定能正好对上，也就是说，可能存在数据偏差，结果导致某一个格子没有点，某个格子有多个点情况。当然，这种情况在激光雷达生成原始数据中就存在。

重要问题：

1. 怎样判断哪一堆点云属于一束激光扫出来的？或者说，怎样判断在 unorganised 点云中哪里是下一帧点云的分隔标识？通过哪些信息来定义这个标识，从而能保证分隔正确？
思路：
KITTI数据集存储是按照一束激光的所有扫描数据存储完之后再存储下一束激光的数据，所以标识可以通过计算点在水平面上的转角值之差来得到。
解决方法：
因为每一束激光扫描起始点转角为0°附近值，结束点转角为360°附近值，所以在unorganised点云中，当相邻两个点转角值差大于100°（或者更大，值可任意取，但建议不要超过300°）时，可认定为两束激光扫描点云储存在unorganised点云中的分界标识。
2. 如何确定每一排格子数目？怎样将每一束激光点云储存在对应的此排格子中？
思路：
a. 由官方提供水平角分辨率0.08°进行格子数确定
b. 统计每排最小转角 alpha ，以此作为水平角分辨率
解决方法：
根据方法b计算得出alpha值在0.08附近：

0: 0.0766167
1: 0.0766167
2: 0.0766167
3: 0.0791294
4: 0.0815647
5: 0.0740187
6: 0.0815647
7: 0.0766167
8: 0.0791294
9: 0.0766167
10: 0.0791294
11: 0.0815647
12: 0.0815647
13: 0.0815647
14: 0.0791294
15: 0.0766167
16: 0.0815647
17: 0.0791294
18: 0.0791294
19: 0.0791294
20: 0.0815647
21: 0.0815647
22: 0.0815647
23: 0.0791294
24: 0.0791294
25: 0.0740187
26: 0.0766167
27: 0.0766167
28: 0.0766167
29: 0.0766167
30: 0.0766167
31: 0.0766167
32: 0.068528
33: 0.0656106
34: 0.068528
35: 0.0740187
36: 0.0740187
37: 0.0740187
38: 0.0740187
39: 0.0713263
40: 0.0766167
41: 0.0713263
42: 0.0713263
43: 0.0740187
44: 0.0656106
45: 0.0656106
46: 0.0713263
47: 0.0740187
48: 0.0740187
49: 0.0713263
50: 0.0656106
51: 0.0656106
52: 0.0713263
53: 0.0713263
54: 0.0713263
55: 0.068528
56: 0.068528
57: 0.068528
58: 0.068528
59: 0.0656106
60: 0.0559529
61: 0.068528
62: 0.0625573
63: 0.0656106

同时，根据官方给定数据水平角分辨率 0.08° 进行计算，得到最大点格子数为：max col: 4499
因此，本文决定依据官方给定数据水平角分辨率 0.08° 进行计算，结果得到每一束激光扫描得到点个数为：
360° / 0.08° = 4500 （个）

3. PCL中的 Organiesd cloud 属性设置
PCL中通过如下代码，设置初始点云为 organised ：

bool initialKittiOrganiseCloud(const int &row_size, const int &col_size,pcl::PointCloud::Ptr& in_cloud)
{
    in_cloud->resize( row_size * col_size );
    in_cloud->height = row_size;
    in_cloud->width = col_size;
    return true;
}

4. 关于C++：为何在构造函数内进行的对变量的初始化值会在后续函数中发现该函数并未达到初始化的效果？而当将初始化变量函数放到其他函数内，后续函数并不会报错？
具体如下：

// 初始化变量函数
bool GroundRemove::initialKittiOrganiseCloud(const int &row_size, const int &col_size)
{
    kitti_organised_cloud_ptr_->height = row_size;
    kitti_organised_cloud_ptr_->width = col_size;
    kitti_organised_cloud_ptr_->resize( row_size * col_size );
    for (int i = 0; i < kitti_organised_cloud_ptr_->height; ++i)
    {
        for (int j = 0; j < kitti_organised_cloud_ptr_->width; ++j)
        {
            kitti_organised_cloud_ptr_->at( j,i ).x = NAN;
            kitti_organised_cloud_ptr_->at( j,i ).y = NAN;
            kitti_organised_cloud_ptr_->at( j,i ).z = NAN;
        }
    }
    return true;
 }

//构造函数
GroundRemove::GroundRemove()
{
    kitti_organised_cloud_ptr_.reset( new PointCloudXYZI );
    this->initialKittiOrganiseCloud(64, 4500)
}

//后续函数
bool GroundRemove::arrangePointInOrganise(std::vector &in_cloud,
                                                 PointCloudXYZI::Ptr &out_cloud)
{
    if ( in_cloud.empty() )
    {
        std::cerr << "Input cloud vector is EMPTY!" << std::endl;
        return false;
    }
    else if ( !out_cloud->isOrganized() )
    {
        std::cerr << "Input point cloud is UNORGANISED!" << std::endl;
        return false;
    }

    float angle = 0.0;
    float distance = 0.0;
    int col_num = 0;
    int tmp_vec_point = 0;
    for (int i = 0; i < in_cloud.size(); ++i)
    {
        for (int j = 0; j < in_cloud[i].size(); ++j)
        {
             angle = this->computeHorResoluteAngle( in_cloud[i].at(j) ) / M_PI * 180.0f;
             col_num = static_cast ( angle / velodyne_angle_res );
             out_cloud->at(col_num,i) = in_cloud[i].at(j)
         }
    }
    return true;
}

当把 initialKittiOrganiseCloud()函数放在构造函数中时，arrangePointInOrganise()内在检测会发生Input point cloud is UNORGANISED!输出，而当把initialKittiOrganiseCloud()函数放在其他函数中时，该检测会通过。
思路：
猜测与构造函数的机制有关，也有可能与PCL有关。
解决方法：
如果非要在构造函数中使用该函数，尚未找到解决办法。记录于此，待解决。

5. 为何每次储存点云相对于原数据会少300个点？具体如下：

        bool GroundRemove::arrangePointInOrganise(std::vector &in_cloud,
                                                 PointCloudXYZI::Ptr &out_cloud)
        {
            if ( in_cloud.empty() )
            {
                std::cerr << "Input cloud vector is EMPTY!" << std::endl;
                return false;
            }
            else if ( !out_cloud->isOrganized() )
            {
                std::cerr << "Input point cloud is UNORGANISED!" << std::endl;
                return false;
            }

            float angle = 0.0f;
            float distance = 0.0f;
            int col_num = 0;
            int tmp_vec_point = 0;
            for (int i = 0; i < in_cloud.size(); ++i)
            {
                for (int j = 0; j < in_cloud[i].size(); ++j)
                {
                    angle = this->computeHorResoluteAngle( in_cloud[i].at(j) ) / M_PI * 180.0f;
                    col_num = static_cast ( angle / velodyne_angle_res );
                    out_cloud->at(col_num,i) = in_cloud[i].at(j);
                }
                tmp_vec_point += in_cloud[i].size();
            }

            int tmp_pt_point = 0;
            int tmp_nan_point = 0;
            for (int k = 0; k < out_cloud->size(); ++k)
            {
                if (isnanf(out_cloud->at(k).x))
                    tmp_nan_point ++;
                else
                    tmp_pt_point ++;
            }

            std::cout << "origin point size: " << origin_cloud_ptr_->size() << std::endl;
            std::cout << "tmp_vec_point: " << tmp_vec_point << std::endl;
            std::cout << "tmp_pt_point: " << tmp_pt_point << std::endl;
            std::cout << "num diff: " << tmp_vec_point - tmp_pt_point << std::endl;
            std::cout << "nan point: " << tmp_nan_point + tmp_pt_point << std::endl;

            return true;
        }

        // 某一帧输出
        origin point size: 120805
        tmp_vec_point: 120804
        tmp_pt_point: 120482
        num diff: 322
        nan point: 288000

思路：
在out_cloud->at(col_num,i) = in_cloud[i].at(j);这行代码中，有这样的逻辑：
只管填入点，不管out_cloud->at(col_num,i)此前是否已经有点，这就会导致点的损失。
解决方法：
这种情况无法避免，当同一个格子中时，只能选取更优的点，选取原则：格子中距离雷达最近距离的点。

以上。