激光雷达定位与建图-最近邻问题

一、问题引出

最近邻问题:假设有两个点云集合, χ 1 = { x 1 , ⋯ x n } \chi _{1} = \left \{ x_{1},\cdots x_{n} \right \} χ1={x1,xn} χ 2 = { x 1 , ⋯ x n } \chi _{2} = \left \{ x_{1},\cdots x_{n} \right \} χ2={x1,xn} ,求点云集合 χ 2 \chi _{2} χ2中某个点,在点云集合 χ 1 \chi _{1} χ1中与它最近的点是?或者在点云集合 χ 2 \chi _{2} χ2中与它最近的K个点是?

常用解决方法有:暴力最近邻法、栅格与体素方法、二分树与k-d树、四叉树与八叉树和其它树类方法。

二、暴力最近邻搜索

暴力最近邻最朴素的思路:假设待查点point和点云,计算待查点对所有点云的距离,排序后给出最小距离;若求最近K个点云,则排序后,给出最近k个即可。

1.暴力 k 近邻(Brute Force k-Nearest Neighbors,简称 BFKNN)

std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k) {
    struct IndexAndDis {
        IndexAndDis() {}
        IndexAndDis(int index, double dis2) : index_(index), dis2_(dis2) {}
        int index_ = 0;
        double dis2_ = 0;
    };

    std::vector<IndexAndDis> index_and_dis(cloud->size());
    for (int i = 0; i < cloud->size(); ++i) {
        index_and_dis[i] = {i, (cloud->points[i].getVector3fMap() - point).squaredNorm()};
    }
    std::sort(index_and_dis.begin(), index_and_dis.end(),
              [](const auto& d1, const auto& d2) { return d1.dis2_ < d2.dis2_; });
    std::vector<int> ret;
    std::transform(index_and_dis.begin(), index_and_dis.begin() + k, std::back_inserter(ret),
                   [](const auto& d1) { return d1.index_; });
    return ret;
}

void bfnn_cloud_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k) {
    std::vector<size_t> index(cloud2->size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });

    matches.resize(index.size() * k);
    std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {
        auto v = bfnn_point_k(cloud1, ToVec3f(cloud2->points[idx]), k);
        for (int i = 0; i < v.size(); ++i) {
            matches[idx * k + i].first = v[i];
            matches[idx * k + i].second = idx;
        }
    });
}

这两个函数 bfnn_point_kbfnn_cloud_k 实现了暴力 k 近邻(Brute Force k-Nearest Neighbors,简称 BFKNN)算法。下面是对这两个函数的详细解释:

bfnn_point_k 函数

这个函数用于在点云 cloud 中找到与给定点 point 最近的 k 个点的索引。

函数参数:
  • CloudPtr cloud:指向点云的智能指针。
  • const Vec3f& point:查询点的三维坐标。
  • int k:要找到的最近邻点的数量。
函数实现步骤:
  1. 定义结构体 IndexAndDis:用于存储点云中每个点的索引和它到查询点 point 的平方距离。

  2. 初始化 index_and_dis 向量:创建一个 IndexAndDis 类型的向量,大小与点云相同,并初始化每个元素的索引和平方距离。

  3. 计算距离并存储:遍历点云中的每个点,计算它与查询点 point 的平方距离,并将索引和距离存储在 index_and_dis 向量中。

  4. 排序:使用 std::sort 算法对 index_and_dis 向量进行排序,排序依据是每个元素的平方距离 dis2_

  5. 提取最近邻索引:使用 std::transform 算法和 std::back_inserter 迭代器从排序后的 index_and_dis 向量中提取前 k 个元素的索引,并将它们添加到 ret 向量中。

返回值:
  • std::vector<int>:包含 k 个最近邻点的索引。

bfnn_cloud_k 函数

这个函数用于在两个点云 cloud1cloud2 之间找到每个点的 k 个最近邻点。

函数参数:
  • CloudPtr cloud1:指向第一个点云的智能指针,将被搜索以找到最近邻点。
  • CloudPtr cloud2:指向第二个点云的智能指针,其点将作为查询点。
  • std::vector<std::pair<size_t, size_t>>& matches:一个引用,指向一个向量,该向量存储匹配对。每个匹配对包含 cloud1 中最近邻点的索引和 cloud2 中对应点的索引。
  • int k:要找到的最近邻点的数量。
函数实现步骤:
  1. 初始化索引向量:创建一个与 cloud2 大小相同的索引向量 index

  2. 填充索引向量:使用 std::for_each 算法和一个 Lambda 表达式来填充索引向量。

  3. 调整匹配向量的大小:调整 matches 向量的大小以匹配 cloud2 的大小乘以 k,为存储每个点的 k 个最近邻匹配做准备。

  4. 寻找 k 个最近邻点:使用 std::for_each 算法和一个 Lambda 表达式遍历 index 向量。对于每个索引 idx,调用 bfnn_point_k 函数找到 cloud2 中点的 k 个最近邻点的索引,并将它们存储在 matches 向量中。

三、栅格与体素搜索

栅格与体素搜索是将点云在空间中划分为二维栅格或者三维体素,计算出每个点对应在栅格中的坐标位置,再对所有点进行索引。所以在查找最近邻时,可以先计算出被查找点所在的栅格,再在该栅格周围寻找最近邻。和暴力搜索相比,这种方法大大缩小了查找范围,从而提升了效率。

从上述描述看:
1.假设待划分的点云不是均匀分布的(实际也不是),所以需要根据点云的疏密程度,需要预先定义划分栅格的分辨率。假设栅格划得太大,一个格子里会有很多点,导致最近邻计算效率下降;如果划分太小,一方面格子数量多,另一方面,在查找相邻格子时,由于点云过于稀疏,最近邻可能并不在上下左右的格子中,可能找不到真正的最近邻,所以网格划分的分辨率需要根据经验来调整。也可以自适应分辨率,如根据点云的密度动态调整分辨率大小。

2.由于划分的格子是离散的,在查找最近邻时,除了在当前栅格内查找,还应该在它周边进行查找。对于二维栅格,一般可分为:上下4格,或加上四个角8格。三维体素,上下前后左右6个体素,或加上八个对角14个体素。

栅格搜索法示例代码,主要分为三步:
1.定义搜索近邻网格范围;
2.划分网格;
3.在待查找点pt栅格周边寻找最近邻。

1.定义搜索近邻网格范围
template <>
void GridNN<2>::GenerateNearbyGrids() {
    if (nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());
    } else if (nearby_type_ == NearbyType::NEARBY4) {
        nearby_grids_ = {Vec2i(0, 0), Vec2i(-1, 0), Vec2i(1, 0), Vec2i(0, 1), Vec2i(0, -1)};
    } else if (nearby_type_ == NearbyType::NEARBY8) {
        nearby_grids_ = {
            Vec2i(0, 0),   Vec2i(-1, 0), Vec2i(1, 0),  Vec2i(0, 1), Vec2i(0, -1),
            Vec2i(-1, -1), Vec2i(-1, 1), Vec2i(1, -1), Vec2i(1, 1),
        };
    }
}

2.划分网格
template <int dim>
bool GridNN<dim>::SetPointCloud(CloudPtr cloud) {
    std::vector<size_t> index(cloud->size());
    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });

    std::for_each(index.begin(), index.end(), [&cloud, this](const size_t& idx) {
        auto pt = cloud->points[idx];
        auto key = Pos2Grid(ToEigen<float, dim>(pt));
        if (grids_.find(key) == grids_.end()) {
            grids_.insert({key, {idx}});
        } else {
            grids_[key].emplace_back(idx);
        }
    });

    cloud_ = cloud;
    LOG(INFO) << "grids: " << grids_.size();
    return true;
}
template <int dim>
Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
    return pt.array().template round().template cast<int>(); //将每个点云坐标四舍五入、取整,划分为网格
    // Eigen::Matrix<int, dim, 1> ret;
    // for (int i = 0; i < dim; ++i) {
    //     ret(i, 0) = round(pt[i] * inv_resolution_); //根据分辨率,划分网格
    // }
    // return ret;
}

3.在待查找点pt栅格周边寻找最近邻
template <int dim>
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
    std::vector<size_t> idx_to_check;
    auto key = Pos2Grid(ToEigen<float, dim>(pt));

    std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &idx_to_check, this](const KeyType& delta) {
        auto dkey = key + delta;
        auto iter = grids_.find(dkey);
        if (iter != grids_.end()) {
            idx_to_check.insert(idx_to_check.end(), iter->second.begin(), iter->second.end());
        }
    });

    if (idx_to_check.empty()) {
        return false;
    }

    // brute force nn in cloud_[idx]
    CloudPtr nearby_cloud(new PointCloudType);
    std::vector<size_t> nearby_idx;
    for (auto& idx : idx_to_check) {
        nearby_cloud->points.template emplace_back(cloud_->points[idx]);
        nearby_idx.emplace_back(idx);
    }

    size_t closest_point_idx = bfnn_point(nearby_cloud, ToVec3f(pt));
    idx = nearby_idx.at(closest_point_idx);
    closest_pt = cloud_->points[idx];

    return true;
}

这段代码实现了一个基于网格的最近邻搜索框架,用于在点云数据中高效地查找最近邻点。以下是对代码的详细解析:

1. 定义搜索近邻网格范围

GenerateNearbyGrids 函数用于定义查询点周边的网格范围,这些网格将被用于后续的最近邻搜索。

  • nearby_type_ 是一个枚举类型,决定了搜索的范围。
  • NearbyType::CENTER 表示只搜索中心网格。
  • NearbyType::NEARBY4 表示搜索中心网格及其上下左右的4个相邻网格。
  • NearbyType::NEARBY8 表示搜索中心网格及其8个对角相邻网格。

2. 划分网格

SetPointCloud 函数用于将点云数据划分到不同的网格中。

  • 使用 std::for_each 和 Lambda 表达式为点云中的每个点分配一个索引。
  • 通过 Pos2Grid 函数将每个点的坐标转换为网格键。
  • 使用 grids_ 容器(通常是一个哈希表)存储每个网格键及其对应的点索引。

Pos2Grid 函数用于将点的坐标转换为网格索引。

  • 使用 round 函数将点的坐标四舍五入到最近的整数,然后转换为整数类型,作为网格索引。

3. 在待查找点pt栅格周边寻找最近邻

GetClosestPoint 函数用于在查询点 pt 的周边网格中寻找最近邻点。

  • 使用 Pos2Grid 函数将查询点 pt 转换为网格键。

  • 遍历 nearby_grids_ 中定义的网格范围,对于每个网格,检查是否存在对应的点索引。

  • 如果找到对应的点索引,将它们添加到 idx_to_check 向量中。

  • 如果 idx_to_check 向量不为空,使用 bfnn_point 函数在这些点中找到最近的点。

  • bfnn_point 函数是一个暴力搜索算法,它计算查询点与每个候选点之间的距离,并返回最近点的索引。

  • 最后,将最近邻点的索引和坐标存储在 idxclosest_pt 中,并返回 true 表示找到最近邻点。

四、评价

​从第三步栅格与体素划分可知,如果栅格划分太小,一方面格子数量多,另一方面,在查找相邻格子时,由于点云过于稀疏,最近邻可能并不在上下左右的格子中,可能找不到真正的最近邻。因此有必要对划分网格方法进行准确性评价。
栅格法最近邻可能存在两种错误的情况:
1.假阳性
栅格法检测出的最近邻,实际上并不是最近邻,这种情况称为假阳性,记作FP。
2.假阴性
实际中的某个最近邻,在栅格法中并没有检测到,这种情况称为假阴性,记作FN。
利用FP和FN的定义,可以定义算法的准确率和召回率。
假设近邻算法中总共计算了m次最近邻,而真值共给出了n个最近邻,那么准确率和召回率定位为:
准确率 = 1 - FP/m,召回率 = 1 - FN/n
暴力搜索匹配可以做到准确率和召回率都为100%,故使用暴力匹配结果作为真值。
示例代码:

//truth:为暴力搜索匹配结果
//esti:为栅格法搜索匹配结果
//以truth为真值,计算esti的准确率和召回率
void EvaluateMatches(const std::vector<std::pair<size_t, size_t>>& truth,
                     const std::vector<std::pair<size_t, size_t>>& esti) {
    int fp = 0;  // false-positive,esti存在但truth中不存在
    int fn = 0;  // false-negative, truth存在但esti不存在

    LOG(INFO) << "truth: " << truth.size() << ", esti: " << esti.size();

    /// 检查某个匹配在另一个容器中存不存在
    auto exist = [](const std::pair<size_t, size_t>& data, const std::vector<std::pair<size_t, size_t>>& vec) -> bool {
        return std::find(vec.begin(), vec.end(), data) != vec.end();
    };

    int effective_esti = 0;
    for (const auto& d : esti) {
        if (d.first != sad::math::kINVALID_ID && d.second != sad::math::kINVALID_ID) {
            effective_esti++;

            if (!exist(d, truth)) {
                fp++;
            }
        }
    }

    for (const auto& d : truth) {
        if (!exist(d, esti)) {
            fn++;
        }
    }

    float precision = 1.0 - float(fp) / effective_esti;
    float recall = 1.0 - float(fn) / truth.size();
    LOG(INFO) << "precision: " << precision << ", recall: " << recall << ", fp: " << fp << ", fn: " << fn;
}

上述定义了一个名为 EvaluateMatches 的函数,它用于评估两种匹配结果的准确率(precision)和召回率(recall)。这里的 “truth” 为暴力搜索匹配结果,而 “esti” 为栅格法搜索得到的匹配结果。该函数通过比较这两种结果来评估后者的性能。

函数参数:

  • const std::vector<std::pair<size_t, size_t>>& truth:为暴力搜索匹配结果。
  • const std::vector<std::pair<size_t, size_t>>& esti:为栅格法搜索得到的匹配结果。

函数实现步骤:

  1. 初始化计数器:初始化假阳性(false-positive, fp)和假阴性(false-negative, fn)计数器。

  2. 记录真实和估计的匹配数量:使用 LOG(INFO) 记录真实匹配和估计匹配的数量。

  3. 定义辅助函数 exist:定义一个 Lambda 表达式 exist,用于检查一个匹配对是否存在于给定的匹配对集合中。这个函数使用 std::find 算法来查找特定的匹配对。

  4. 计算假阳性(FP):遍历估计的匹配结果 esti,对于每个有效的匹配对(即两个索引都不是无效值),检查它是否存在于真实匹配结果 truth 中。如果不存在,则增加 fp 计数器。

  5. 计算假阴性(FN):遍历真实匹配结果 truth,对于每个匹配对,检查它是否存在于估计匹配结果 esti 中。如果不存在,则增加 fn 计数器。

  6. 计算准确率和召回率

    • 准确率(Precision):正确匹配的数量除以估计匹配的总数(有效匹配加上假阳性)。
    • 召回率(Recall):正确匹配的数量除以真实匹配的总数(真实匹配减去假阴性)。
  7. 记录结果:使用 LOG(INFO) 记录准确率、召回率以及假阳性和假阴性的数量。

五、参考

<<自动驾驶与机器人中的SLAM技术从理论到实践>>

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值