ikd-Tree 简单测试

ikd-Tree 简单测试

因为在学习时手写的 KNN 搜索可以用多线程的方式搜索,搜索效率很高,就考虑 PCL 自带的函数是否能够多线程 KNN 搜索,貌似好像是不行的,不过没有深入研究

发现了 ikd-Tree 这个方法,但是阅读论文后发现其 KNN 搜索貌似依然是单线程的,只是在 re-build 部分采用了并行线程的方式

在这里插入图片描述

对其性能进行简单测试,主要是 Nearest_Search 函数的使用

void KD_TREE<PointType>::Nearest_Search(PointType point, int k_nearest, PointVector& Nearest_Points, vector<float> & Point_Distance, double max_dist)

在这里插入图片描述

注意 max_dist 参数可以控制在指定的范围内进行 KNN 搜索

inline bool compare_point(const sad::PointType& p1, const sad::PointType& p2){
    if(p1.x == p2.x && p1.y == p2.y && p1.z == p2.z){
        return true;
    }
    return false;
}

double max_distance = 3.0;

TEST(CH5_TEST, KDTREE_KNN) {
    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);

    if (first->empty() || second->empty()) {
        LOG(ERROR) << "cannot load cloud";
        FAIL();
    }

    const int nearest_search_k = 5;

    // voxel grid 至 0.05
    sad::VoxelGrid(first);
    sad::VoxelGrid(second);

    sad::KdTree kdtree;
    sad::evaluate_and_call([&first, &kdtree]() { kdtree.BuildTree(first); }, "Kd Tree build", 1);

    kdtree.SetEnableANN(true, FLAGS_ANN_alpha);

    LOG(INFO) << "Kd tree leaves: " << kdtree.size() << ", points: " << first->size();

    // 比较 bfnn
    std::vector<std::pair<size_t, size_t>> true_matches;
    sad::bfnn_cloud_mt_k(first, second, true_matches);

    // 对第2个点云执行knn
    std::vector<std::pair<size_t, size_t>> matches;
    sad::evaluate_and_call([&first, &second, &kdtree, &matches]() { kdtree.GetClosestPointMT(second, matches, nearest_search_k); },
                           "Kd Tree 5NN multi-thread", 1);
    EvaluateMatches(true_matches, matches);

    LOG(INFO) << "building kdtree pcl";
    // 对比PCL
    pcl::search::KdTree<sad::PointType> kdtree_pcl;
    sad::evaluate_and_call([&first, &kdtree_pcl]() { kdtree_pcl.setInputCloud(first); }, "Kd Tree build", 1);

    LOG(INFO) << "searching pcl";
    matches.clear();
    std::vector<int> search_indices(second->size());
    for (int i = 0; i < second->points.size(); i++) {
        search_indices[i] = i;
    }

    std::vector<std::vector<int>> result_index;
    std::vector<std::vector<float>> result_distance;
    sad::evaluate_and_call(
        [&]() { kdtree_pcl.nearestKSearch(*second, search_indices, nearest_search_k, result_index, result_distance); },
        "Kd Tree 5NN in PCL", 1);
    for (int i = 0; i < second->points.size(); i++) {
        for (int j = 0; j < result_index[i].size(); ++j) {
            int m = result_index[i][j];
            double d = result_distance[i][j];
            matches.push_back({m, i});
        }
    }
    EvaluateMatches(true_matches, matches);

    LOG(INFO) << "ikd_Tree Test";
    KD_TREE<sad::PointType>::Ptr ikdtree_ptr = make_shared<KD_TREE<sad::PointType>>();
    KD_TREE<sad::PointType>& ikd_Tree = *ikdtree_ptr;
    sad::evaluate_and_call([&first, &ikd_Tree]() { ikd_Tree.Build(first->points); }, "iKd Tree build", 1);
    LOG(INFO) << "ikd-Tree valid points: " << ikd_Tree.validnum();

    LOG(INFO) << "searching ikd-Tree";
    LOG(INFO) << "ikdTree KNN search_distance:" << max_distance;
    vector<PointVector> nearest_points(second->points.size());
    vector<vector<float>> points_distance(second->points.size());
    sad::evaluate_and_call(
        [&]() { for(int i = 0; i < second->points.size(); ++i){
                ikd_Tree.Nearest_Search(second->points[i], nearest_search_k, nearest_points[i], points_distance[i], max_distance);}; },
        "ikd-Tree 5NN Search", 1);
    matches.clear();
    int m = 0;
    for(int i = 0; i < second->points.size(); ++i){
        for(int j = 0; j < nearest_points[i].size(); ++j){
            m = -1;
            for(int k = 0; k < first->points.size(); ++k){
                if(compare_point(first->points[k], nearest_points[i][j])){
                    m = k;
                    break;
                }
            }
            matches.push_back({m, i});
        }
    }
    EvaluateMatches(true_matches, matches);  
    
    LOG(INFO) << "done.";

    SUCCEED();
}

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;

    string envVarName = "ikdTree_search_distance";
    string max_distance_str = getenv(envVarName.data());
    max_distance = stod(max_distance_str);

    testing::InitGoogleTest(&argc, argv);
    google::ParseCommandLineFlags(&argc, &argv, true);
    return RUN_ALL_TESTS();
}

有几个需要注意的地方:

1、启动程序时从命令行参数中获取环境变量作为 max_dist 参数的值,这样可以避免每次变化max_dist 都要重新编译,编译还是很费时的

		string envVarName = "ikdTree_search_distance";
    string max_distance_str = getenv(envVarName.data());
    max_distance = stod(max_distance_str);

2、添加 ikd-Tree 的建树以及 KNN 搜索测试,对于 second 点云中的每个点找最近邻

		LOG(INFO) << "ikd_Tree Test";
    KD_TREE<sad::PointType>::Ptr ikdtree_ptr = make_shared<KD_TREE<sad::PointType>>();
    KD_TREE<sad::PointType>& ikd_Tree = *ikdtree_ptr;
    sad::evaluate_and_call([&first, &ikd_Tree]() { ikd_Tree.Build(first->points); }, "iKd Tree build", 1);
    LOG(INFO) << "ikd-Tree valid points: " << ikd_Tree.validnum();

    LOG(INFO) << "searching ikd-Tree";
    LOG(INFO) << "ikdTree KNN search_distance:" << max_distance;
    vector<PointVector> nearest_points(second->points.size());
    vector<vector<float>> points_distance(second->points.size());
    sad::evaluate_and_call(
        [&]() { for(int i = 0; i < second->points.size(); ++i){
                ikd_Tree.Nearest_Search(second->points[i], nearest_search_k, nearest_points[i], points_distance[i], max_distance);}; },
        "ikd-Tree 5NN Search", 1);

3、计算准确率和召回率,这里不太好实现的是 ikd-Tree 的 KNN 记录的是点信息而不是点在点云中的索引,而评价函数中用的是索引,因此还需要遍历找点对应的索引,这里的时间复杂度很高,应该进行优化

		matches.clear();
    int m = 0;
    for(int i = 0; i < second->points.size(); ++i){
        for(int j = 0; j < nearest_points[i].size(); ++j){
            m = -1;
            for(int k = 0; k < first->points.size(); ++k){
                if(compare_point(first->points[k], nearest_points[i][j])){
                    m = k;
                    break;
                }
            }
            matches.push_back({m, i});
        }
    }
    EvaluateMatches(true_matches, matches); 

简单进行了几组实验

1、max_dist = 0.5

lei@lei:~/slam_in_autonomous_driving$ ikdTree_search_distance=0.5 ./bin/test_nn --gtest_filter=CH5_TEST.KDTREE_KNN
Note: Google Test filter = CH5_TEST.KDTREE_KNN
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from CH5_TEST
[ RUN      ] CH5_TEST.KDTREE_KNN
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
I0831 09:19:16.993024  3630 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 6.07747/1 毫秒.
I0831 09:19:16.993263  3630 test_nn.cc:242] Kd tree leaves: 18869, points: 18869
I0831 09:19:19.454218  3630 sys_utils.h:32] 方法 Kd Tree 5NN multi-thread 平均调用时间/次数: 3.67089/1 毫秒.
I0831 09:19:19.454253  3630 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:19:22.478986  3630 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:19:22.479022  3630 test_nn.cc:254] building kdtree pcl
I0831 09:19:22.481360  3630 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 2.31427/1 毫秒.
I0831 09:19:22.481376  3630 test_nn.cc:259] searching pcl
I0831 09:19:22.500319  3630 sys_utils.h:32] 方法 Kd Tree 5NN in PCL 平均调用时间/次数: 18.9071/1 毫秒.
I0831 09:19:22.500790  3630 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:19:25.495046  3630 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:19:25.495080  3630 test_nn.cc:280] ikd_Tree Test
Multi thread started 
I0831 09:19:25.516258  3630 sys_utils.h:32] 方法 iKd Tree build 平均调用时间/次数: 7.09316/1 毫秒.
I0831 09:19:25.516289  3630 test_nn.cc:284] ikd-Tree valid points: 18869
I0831 09:19:25.516294  3630 test_nn.cc:286] searching ikd-Tree
I0831 09:19:25.516294  3630 test_nn.cc:287] ikdTree KNN search_distance:0.5
I0831 09:19:25.550645  3630 sys_utils.h:32] 方法 ikd-Tree 5NN Search 平均调用时间/次数: 34.1594/1 毫秒.
I0831 09:19:26.320165  3630 test_nn.cc:69] truth: 93895, esti: 80229
I0831 09:19:29.010622  3630 test_nn.cc:95] precision: 1, recall: 0.854454, fp: 0, fn: 13666
I0831 09:19:29.010658  3630 test_nn.cc:310] done.
Rebuild thread terminated normally
[       OK ] CH5_TEST.KDTREE_KNN (12039 ms)
[----------] 1 test from CH5_TEST (12039 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (12040 ms total)
[  PASSED  ] 1 test.

2、max_dist = 1.0

lei@lei:~/slam_in_autonomous_driving$ ikdTree_search_distance=1.0 ./bin/test_nn --gtest_filter=CH5_TEST.KDTREE_KNN
Note: Google Test filter = CH5_TEST.KDTREE_KNN
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from CH5_TEST
[ RUN      ] CH5_TEST.KDTREE_KNN
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
I0831 09:19:44.656121  3637 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 5.99205/1 毫秒.
I0831 09:19:44.656301  3637 test_nn.cc:242] Kd tree leaves: 18869, points: 18869
I0831 09:19:47.107566  3637 sys_utils.h:32] 方法 Kd Tree 5NN multi-thread 平均调用时间/次数: 4.01058/1 毫秒.
I0831 09:19:47.107602  3637 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:19:50.210033  3637 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:19:50.210068  3637 test_nn.cc:254] building kdtree pcl
I0831 09:19:50.212378  3637 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 2.28724/1 毫秒.
I0831 09:19:50.212393  3637 test_nn.cc:259] searching pcl
I0831 09:19:50.231775  3637 sys_utils.h:32] 方法 Kd Tree 5NN in PCL 平均调用时间/次数: 19.3455/1 毫秒.
I0831 09:19:50.232239  3637 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:19:53.198168  3637 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:19:53.198204  3637 test_nn.cc:280] ikd_Tree Test
Multi thread started 
I0831 09:19:53.219951  3637 sys_utils.h:32] 方法 iKd Tree build 平均调用时间/次数: 6.79115/1 毫秒.
I0831 09:19:53.219980  3637 test_nn.cc:284] ikd-Tree valid points: 18869
I0831 09:19:53.219987  3637 test_nn.cc:286] searching ikd-Tree
I0831 09:19:53.219990  3637 test_nn.cc:287] ikdTree KNN search_distance:1
I0831 09:19:53.255913  3637 sys_utils.h:32] 方法 ikd-Tree 5NN Search 平均调用时间/次数: 35.78/1 毫秒.
I0831 09:19:54.156787  3637 test_nn.cc:69] truth: 93895, esti: 89684
I0831 09:19:56.939265  3637 test_nn.cc:95] precision: 1, recall: 0.955152, fp: 0, fn: 4211
I0831 09:19:56.939299  3637 test_nn.cc:310] done.
Rebuild thread terminated normally
[       OK ] CH5_TEST.KDTREE_KNN (12308 ms)
[----------] 1 test from CH5_TEST (12308 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (12308 ms total)
[  PASSED  ] 1 test.

3、max_dist = 1.5

lei@lei:~/slam_in_autonomous_driving$ ikdTree_search_distance=1.5 ./bin/test_nn --gtest_filter=CH5_TEST.KDTREE_KNN
Note: Google Test filter = CH5_TEST.KDTREE_KNN
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from CH5_TEST
[ RUN      ] CH5_TEST.KDTREE_KNN
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
I0831 09:43:12.231748  4167 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 6.1671/1 毫秒.
I0831 09:43:12.251842  4167 test_nn.cc:242] Kd tree leaves: 18869, points: 18869
I0831 09:43:14.705106  4167 sys_utils.h:32] 方法 Kd Tree 5NN multi-thread 平均调用时间/次数: 4.15197/1 毫秒.
I0831 09:43:14.705140  4167 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:43:17.634130  4167 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:43:17.634166  4167 test_nn.cc:254] building kdtree pcl
I0831 09:43:17.636514  4167 sys_utils.h:32] 方法 Kd Tree build 平均调用时间/次数: 2.32538/1 毫秒.
I0831 09:43:17.636538  4167 test_nn.cc:259] searching pcl
I0831 09:43:17.655293  4167 sys_utils.h:32] 方法 Kd Tree 5NN in PCL 平均调用时间/次数: 18.7018/1 毫秒.
I0831 09:43:17.655751  4167 test_nn.cc:69] truth: 93895, esti: 93895
I0831 09:43:20.850251  4167 test_nn.cc:95] precision: 1, recall: 1, fp: 0, fn: 0
I0831 09:43:20.850288  4167 test_nn.cc:280] ikd_Tree Test
Multi thread started 
I0831 09:43:20.870407  4167 sys_utils.h:32] 方法 iKd Tree build 平均调用时间/次数: 5.79657/1 毫秒.
I0831 09:43:20.870435  4167 test_nn.cc:284] ikd-Tree valid points: 18869
I0831 09:43:20.870441  4167 test_nn.cc:286] searching ikd-Tree
I0831 09:43:20.870443  4167 test_nn.cc:287] ikdTree KNN search_distance:1.5
I0831 09:43:20.911399  4167 sys_utils.h:32] 方法 ikd-Tree 5NN Search 平均调用时间/次数: 40.8256/1 毫秒.
I0831 09:43:22.000274  4167 test_nn.cc:69] truth: 93895, esti: 91513
I0831 09:43:25.069787  4167 test_nn.cc:95] precision: 1, recall: 0.974631, fp: 0, fn: 2382
I0831 09:43:25.069824  4167 test_nn.cc:310] done.
Rebuild thread terminated normally
[       OK ] CH5_TEST.KDTREE_KNN (12858 ms)
[----------] 1 test from CH5_TEST (12858 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (12858 ms total)
[  PASSED  ] 1 test.

经观察可以发现随着 max_dist 的增大,KNN 搜索的时间在增大,但召回率(recall)也在增大,说明搜索效率和准确率确实是矛盾的

另外 ikd-Tree KNN 的时间确实要大于 PCL KNN 的时间,这和论文中是一致的,但总体时间短很多

  • 1
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Prejudices

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值