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 的时间，这和论文中是一致的，但总体时间短很多