南柯一梦
0%

Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners 论文阅读与复现

发表于 更新于 分类于

解决的问题

解决使用旋转卡尺或者PCA分解生成错误heading角的问题。

论文基本思路

  1. 将90度n等分，旋转坐标系，计算所有点在新的方向上形成点的坐标
  2. 计算这些坐标值按照目标函数给出得分
  3. 选定得分最高的方向最为航向角方向

实测效果

论文中针对拟合得分，有三种代价函数，最小面积，最小距离，最小方差。自己实测下来最小距离在速度型效果上最优。

主干部分

论文原文给的伪代码：

自己的C++的复现 代码核心逻辑部分： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 前提 输入点云cloud为XYZI的PCL点云格式
const auto &data = cloud->getMatrixMap(2, 8, 0).transpose();
Eigen::Vector2f e1, e2;
float best_score, best_heading;
for (int i = 0; i <= n; i++)
{
    //生成当前迭代角度
    float theta = i * MP_PI_/ n;
    e1 << cos(theta) , sin(theta);
    e2 << -sin(theta) , cos(theta);
    //计算投影点
    Eigen::VectorXf c1 = data * e1;
    Eigen::VectorXf c2 = data * e2;
    // 生成得分
    float socre = ClosenessCriterion(c1,c2);
    // 比较当前得分是否为历史最佳得分 更新float best_score, best_heading
    ...
}

代价函数部分

论文原文给的伪代码：

自己的C++的复现 代码核心逻辑部分： 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
float ClosenessCriterion(c1, c2)
{
    constexpr float d0 = 0.1;
    // 这里不用Eigen的函数接口自己迭代会更快 少两轮迭代
    float c1_max = c1.maxCoeff();
    float c1_min = c1.minCoeff();
    float c2_max = c2.maxCoeff();
    float c2_min = c2.minCoeff();
    Eigen::VectorXf v1 = c1_max - c1.array();
    Eigen::VectorXf v2 = c1.array() - c1_min;
    Eigen::VectorXf d1 = v1.norm() < v2.norm() ? v1 : v2;
    v1 = c2_max - c2.array();
    v2 = c2.array() - c2_min;
    Eigen::VectorXf d2 = v2.norm() < v2.norm() ? v1 : v2;
    Eigen::VectorXf b = 1 / d1.cwiseMin(d2).cwiseMax(d0).array();
    return b.sum();
}

一些注意事项

  • PCL点云使用getMatrixMap()直接映射成Eigen矩阵，减少点云拷贝。
  • 项目代码中 避免Eigen自动内存管理可以手动管理空间，用Eigen的Map方法将矩阵映射到自己分配的空间中，在乘法中使用.noalias()方法让矩阵乘法的结果直接生成在预先分配好的内存地址上。