kdtree_search

最新推荐文章于 2024-03-06 09:56:56 发布

ocachenjun

最新推荐文章于 2024-03-06 09:56:56 发布

阅读量235

点赞数

分类专栏： pcl 文章标签： c++

本文链接：https://blog.csdn.net/weixin_39191646/article/details/108382483

版权

pcl 专栏收录该内容

7 篇文章 0 订阅

订阅专栏

原理

kdtree_search(PCL)

kdtree_search.cpp

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>

int main (int argc, char**argv)
{
  srand (time (NULL));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  //点云生成
  cloud->width =1000;
  cloud->height =1;
  cloud->points.resize (cloud->width * cloud->height);
  for (size_t i=0; i< cloud->points.size (); ++i)
  {
    cloud->points[i].x =1024.0f* rand () / (RAND_MAX +1.0f);
    cloud->points[i].y =1024.0f* rand () / (RAND_MAX +1.0f);
    cloud->points[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近邻搜索
  int K =10;
  std::vector<int>pointIdxNKNSearch(K);
  std::vector<float>pointNKNSquaredDistance(K);
  std::cout<<"K nearest neighbor search at ("<<searchPoint.x
           <<" "<<searchPoint.y
           <<" "<<searchPoint.z
           <<") with K="<< K <<std::endl;
  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0 )
  {
    for (size_t i=0; i<pointIdxNKNSearch.size (); ++i)
    {
      std::cout<<"    "<<   cloud->points[ pointIdxNKNSearch[i] ].x 
      <<" "<< cloud->points[pointIdxNKNSearch[i] ].y 
      <<" "<< cloud->points[pointIdxNKNSearch[i] ].z 
      <<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
    }
  }
  // 在半径r内搜索近邻
  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 (size_t i=0; i<pointIdxRadiusSearch.size (); ++i)
    {
      std::cout<<"    "<<   cloud->points[ pointIdxRadiusSearch[i] ].x 
      <<" "<< cloud->points[pointIdxRadiusSearch[i] ].y 
      <<" "<< cloud->points[pointIdxRadiusSearch[i] ].z 
      <<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
    }
  }
  return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 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})