KITTI unorganised cloud to organised cloud

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

前言


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

解决方案


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

  • 将高度为2米以上点过滤(2米为估计,没有考证)
  • 噪点过滤

思路 1:

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

    预期问题:

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

    重要问题:

  • 1. 怎样判断哪一堆点云属于一束激光扫出来的?或者说,怎样判断在 unorganised 点云中哪里是下一帧点云的分隔标识?通过哪些信息来定义这个标识,从而能保证分隔正确?
    思路:
    KITTI数据集存储是按照一束激光的所有扫描数据存储完之后再存储下一束激光的数据,所以标识可以通过计算点在水平面上的转角值之差来得到。
    解决方法:
    因为每一束激光扫描起始点转角为0°附近值,结束点转角为360°附近值,所以在unorganised点云中,当相邻两个点转角值差大于100°(或者更大,值可任意取,但建议不要超过300°)时,可认定为两束激光扫描点云储存在unorganised点云中的分界标识。

  • 2. 如何确定每一排格子数目?怎样将每一束激光点云储存在对应的此排格子中?
    思路:
    a. 由官方提供水平角分辨率0.08°进行格子数确定
    b. 统计每排最小转角 alpha ,以此作为水平角分辨率
    解决方法:
    根据方法b计算得出alpha值在0.08附近:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    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

    1
    2
    3
    4
    5
    6
    7
    bool initialKittiOrganiseCloud(const int &row_size, const int &col_size,pcl::PointCloud<pcl::PointXYZI>::Ptr& in_cloud)
    {
    in_cloud->resize( row_size * col_size );
    in_cloud->height = row_size;
    in_cloud->width = col_size;
    return true;
    }
  • 4. 关于C++:为何在构造函数内进行的对变量的初始化值会在后续函数中发现该函数并未达到初始化的效果?而当将初始化变量函数放到其他函数内,后续函数并不会报错?
    具体如下:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    // 初始化变量函数
    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<PointCloudXYZI> &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<int > ( 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个点?具体如下:
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    bool GroundRemove::arrangePointInOrganise(std::vector<PointCloudXYZI> &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<int > ( 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)此前是否已经有点,这就会导致点的损失。
解决方法:
这种情况无法避免,当同一个格子中时,只能选取更优的点,选取原则:格子中距离雷达最近距离的点。

以上。

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