0
点赞
收藏
分享

微信扫一扫

3d激光雷达开发(kd树)


        kd 树是点云数据的一种管理方式。如果打个比方,就像一串数据一样,可以用数组串起来,也可以用二叉树串起来。用特定的数据结构把数据管理起来,最终还是为了更快更好的查找数据。比如如果我们查找一个点周围有哪些数据,此时没有kd树这样的数据结构的话,就只能一个一个去遍历了,效率非常低。参考的代码网页在这里,​​https://pcl.readthedocs.io/projects/tutorials/en/master/kdtree_search.html#kdtree-search​​

1、编写kdtree_search.cp

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>

int
main ()
{
srand (time (NULL));

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);

for (std::size_t i = 0; i < cloud->size (); ++i)
{
(*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
(*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
(*cloud)[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

kdtree.setInputCloud (cloud);

pcl::PointXYZ searchPoint;

searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);

// K nearest neighbor search

int K = 10;

std::vector<int> pointIdxKNNSearch(K);
std::vector<float> pointKNNSquaredDistance(K);

std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;

if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
{
for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i)
std::cout << " " << (*cloud)[ pointIdxKNNSearch[i] ].x
<< " " << (*cloud)[ pointIdxKNNSearch[i] ].y
<< " " << (*cloud)[ pointIdxKNNSearch[i] ].z
<< " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
}

// Neighbors within radius search

std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;

float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;


if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << (*cloud)[ pointIdxRadiusSearch[i] ].x
<< " " << (*cloud)[ pointIdxRadiusSearch[i] ].y
<< " " << (*cloud)[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}


return 0;
}

2、代码讲解

        整个代码主要分成三个部分。第一,创建一个KdTree的变量;第二,随机生成一个点,查找周围最近的10个数据;第三,给一个半径数据,查找半径之内的所有数据。

3,准备CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(kdtree_search)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (kdtree_search kdtree_search.cpp)
target_link_libraries (kdtree_search ${PCL_LIBRARIES})

4、生成sln文件,直接编译

        按照常规,mkdir build & cd build & cmake ..。

        编译sln之后,不出意外,应该可以看到这样的界面,

3d激光雷达开发(kd树)_网络协议

5、直接运行kdtree_search.exe

        直接运行,可以会失败,因为缺少pcl_kdtree_debug.dll库,补上即可。

        打开一个终端,输入kdtree_search.exe,应该就可以看到这样的打印信息,

3d激光雷达开发(kd树)_数据_02

        数据内容可能不太一样,但是形式肯定是这样的。

举报

相关推荐

0 条评论