深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第五章作业

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第五章作业

第一题

• 在三维体素中定义 NEARBY14，实现 14 格最近邻的查找。
• 本次作业选择下图 14 格体素, 形状为立体十字。

• 修改 gridnn.hpp 与 test_nn.cc 内容如下，center3维坐标为（0，0，0）。

    enum class NearbyType {
        CENTER,  // 只考虑中心
        // for 2D
        NEARBY4,  // 上下左右
        NEARBY8,  // 上下左右+四角

        // for 3D
        NEARBY6,  // 上下左右前后
        NEARBY14,  // 立体十字
    };

    // check dim and nearby
    if (dim == 2 && (nearby_type_ != NearbyType::CENTER && nearby_type_ != NearbyType::NEARBY4 && nearby_type_ != NearbyType::NEARBY8)) {
        LOG(INFO) << "2D grid does not support nearby6/14, using nearby4 instead.";
        nearby_type_ = NearbyType::NEARBY4;
    } else if (dim == 3 && (nearby_type_ != NearbyType::CENTER && nearby_type_ != NearbyType::NEARBY6 && nearby_type_ != NearbyType::NEARBY14)) {
        LOG(INFO) << "3D grid does not support nearby4/8, using nearby6 instead.";
        nearby_type_ = NearbyType::NEARBY6;
    }

    template <>
    void GridNN<3>::GenerateNearbyGrids() {
    if (nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());
    } else if (nearby_type_ == NearbyType::NEARBY6) {
        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
    } else if (nearby_type_ == NearbyType::NEARBY14) {
        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1), KeyType(-1, 0, 1),
                         KeyType(-1, 0, -1), KeyType(1, 0, 1), KeyType(1, 0, -1), KeyType(0, 1, 1),
                         KeyType(0, 1, -1), KeyType(0, -1, 1), KeyType(0, -1, -1)};
    }
}

    LOG(INFO) << "===================";
    sad::evaluate_and_call(
        [&first, &second, &grid14, &matches]() { grid14.GetClosestPointForCloud(first, second, matches); },
        "Grid 3D_14 单线程", 10);
    EvaluateMatches(truth_matches, matches);

    LOG(INFO) << "===================";
    sad::evaluate_and_call(
        [&first, &second, &grid14, &matches]() { grid14.GetClosestPointForCloudMT(first, second, matches); },
        "Grid 3D_14 多线程", 10);
    EvaluateMatches(truth_matches, matches);

• 分辨率设为 0.5 的条件下，测试结果如下图，Grid 3D 为 NearbyType::NEARBY6，Grid 3D_14 为 NearbyType::NEARBY14
• 可见14体素相比6体素，查找速度都更慢，但是由于在中心体素周围新增了更多的体素，准确率与召回率都有相应提高，这是符合规律的。

第二题

第三题

• 根据 first 点云构建 nano kd tree， 并测试其建树速度。

// nano kdtree
    std::vector<size_t> nanoindex(first->size());
    std::for_each(nanoindex.begin(), nanoindex.end(), [idx = 0](size_t& i) mutable { i = idx++; });

    PointCloud<float> nano_cloud;
    nano_cloud.pts.resize(first->size());

    // 给nano点云赋值
    std::for_each(nanoindex.begin(), nanoindex.end(), [this, &first, &nano_cloud](const size_t& idx) {
        nano_cloud.pts[idx].x = first->points[idx].x;
        nano_cloud.pts[idx].y = first->points[idx].y;
        nano_cloud.pts[idx].z = first->points[idx].z;
      });

    // 构建nano kdtree
    using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<float, PointCloud<float>>,
    PointCloud<float>, 3 /* dim */
    >;
    
    // 其中 {10} 为nano_kdtree中一个节点最多多少个叶子
    my_kd_tree_t nano_kdtree(3, nano_cloud, {10});
    sad::evaluate_and_call([&nano_cloud]() { my_kd_tree_t nano_kdtree(3, nano_cloud, {10}); },
                           "nano_kd_tree build", 1);

• nano_kd_tree 5 KNN 多线程实现与验证

    // nano_kd_tree
    std::vector<std::pair<size_t, size_t>> matches;
    size_t                num_results = 5;
    sad::evaluate_and_call([&first, &second, &nano_kdtree, &matches, &num_results]() mutable {
        // 并行时，matches有点云大小的num_results倍
        matches.resize(second->size() * num_results);
        std::vector<size_t> index(second->size());
        std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&first, &second, &nano_kdtree, &num_results, &matches](size_t& i) mutable {
            // 适配 knnSearch() 接口
            const float query_pt[3] = {second->points[i].x, second->points[i].y, second->points[i].z}; 
            std::vector<uint32_t> ret_index(num_results);  // 最近邻索引容器
            std::vector<float>    out_dist_sqr(num_results);  // 最近邻对应距离容器
            num_results = nano_kdtree.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);
            ret_index.resize(num_results);
            // 给 matches 赋值，（first中最近邻， second中索引）
            // 由于 nano_kdtree 不适配AAN的比例因子，不会出现匹配失败的情况
            for (size_t j = 0; j < num_results; j++) {
                matches[i * num_results + j].second = i;
                matches[i * num_results + j].first = ret_index[j];
            }
            });
        },
        "nano_kd_tree 5NN 多线程", 1);
    EvaluateMatches(true_matches, matches);

• nano_kd_tree 的建树以及多线程knn的测试结果如下图
• 建树时间自定义kdtree速度最慢，nano_kd_tree与pcl_kd_tree相当
• 5NN搜索速度 nano_kd_tree 最快， 自定义kdtree其次，pcl_kd_tree 最慢