pcl::octree::OctreePointCloudSearch：pcl点云图临近点搜索使用以及报错处理

最新推荐文章于 2024-04-25 13:42:39 发布

v俊逸

最新推荐文章于 2024-04-25 13:42:39 发布

阅读量2k

点赞数

分类专栏： c++ ROS PCL 文章标签： pcl 概率树临近点搜索 octree

本文链接：https://blog.youkuaiyun.com/me1171115772/article/details/105839239

版权

c++ 同时被 3 个专栏收录

32 篇文章

订阅专栏

ROS

12 篇文章

订阅专栏

PCL

11 篇文章

订阅专栏

Octree pointcloud search class More...

#include <pcl/octree/octree_search.h>

Inheritance diagram for pcl::octree::OctreePointCloudSearch< PointT, LeafT, OctreeT >:

Inheritance graph

Collaboration diagram for pcl::octree::OctreePointCloudSearch< PointT, LeafT, OctreeT >:

Collaboration graph

[legend]

Public Types
typedef boost::shared_ptr < std::vector< int > >	IndicesPtr

typedef boost::shared_ptr < const std::vector< int > >	IndicesConstPtr

typedef pcl::PointCloud< PointT >	PointCloud

typedef boost::shared_ptr < PointCloud >	PointCloudPtr

typedef boost::shared_ptr < const PointCloud >	PointCloudConstPtr

typedef boost::shared_ptr < OctreePointCloudSearch < PointT, LeafContainerT, BranchContainerT > >	Ptr

typedef boost::shared_ptr < const OctreePointCloudSearch < PointT, LeafContainerT, BranchContainerT > >	ConstPtr

typedef std::vector< PointT, Eigen::aligned_allocator < PointT > >	AlignedPointTVector

typedef OctreePointCloud < PointT, LeafContainerT, BranchContainerT >	OctreeT

typedef OctreeT::LeafNode	LeafNode

typedef OctreeT::BranchNode	BranchNode

Public Types inherited from pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >
Public Types inherited from pcl::octree::OctreeBase< LeafContainerT, BranchContainerT >

Public Member Functions
	OctreePointCloudSearch (const double resolution)
	Constructor. More...

virtual	~OctreePointCloudSearch ()
	Empty class constructor. More...

bool	voxelSearch (const PointT &point, std::vector< int > &point_idx_data)
	Search for neighbors within a voxel at given point. More...

bool	voxelSearch (const int index, std::vector< int > &point_idx_data)
	Search for neighbors within a voxel at given point referenced by a point index. More...

int	nearestKSearch (const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
	Search for k-nearest neighbors at the query point. More...

int	nearestKSearch (const PointT &p_q, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
	Search for k-nearest neighbors at given query point. More...

int	nearestKSearch (int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
	Search for k-nearest neighbors at query point. More...

void	approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
	Search for approx. More...

void	approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
	Search for approx. More...

void	approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
	Search for approx. More...

int	radiusSearch (const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
	Search for all neighbors of query point that are within a given radius. More...

int	radiusSearch (const PointT &p_q, const double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
	Search for all neighbors of query point that are within a given radius. More...

int	radiusSearch (int index, const double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
	Search for all neighbors of query point that are within a given radius. More...

int	getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
	Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction). More...

int	getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
	Get indices of all voxels that are intersected by a ray (origin, direction). More...

int	boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
	Search for points within rectangular search area. More...

Public Member Functions inherited from pcl::octree::OctreePointCloud< PointT, LeafContainerT, BranchContainerT >
Public Member Functions inherited from pcl::octree::OctreeBase< LeafContainerT, BranchContainerT >

Protected Member Functions
float	pointSquaredDist (const PointT &point_a, const PointT &point_b) const
	Helper function to calculate the squared distance between two points. More...

void	getNeighborsWithinRadiusRecursive (const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
	Recursive search method that explores the octree and finds neighbors within a given radius. More...

double	getKNearestNeighborRecursive (const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
	Recursive search method that explores the octree and finds the K nearest neighbors. More...

void	approxNearestSearchRecursive (const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
	Recursive search method that explores the octree and finds the approximate nearest neighbor. More...

int	getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
	Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers. More...

void	boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
	Recursive search method that explores the octree and finds points within a rectangular search area. More...

int	getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, std::vector< int > &k_indices, int max_voxel_count) const
	Recursively search the tree for all intersected leaf nodes and return a vector of indices. More...

void	initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
	Initialize raytracing algorithm. More...

int	getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
	Find first child node ray will enter. More...

int	getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
	Get the next visited node given the current node upper bounding box corner. More...

官方文档中的介绍，示例代码：

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>

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

int
main (int argc, char** argv)
{
  srand ((unsigned int) 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->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);
  }

  float resolution = 128.0f;

  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);

  octree.setInputCloud (cloud);
  octree.addPointsFromInputCloud ();

  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);

  // Neighbors within voxel search

  std::vector<int> pointIdxVec;

  if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
    std::cout << "Neighbors within voxel search at (" << searchPoint.x 
     << " " << searchPoint.y 
     << " " << searchPoint.z << ")" 
     << std::endl;
              
    for (std::size_t i = 0; i < pointIdxVec.size (); ++i)
   std::cout << "    " << cloud->points[pointIdxVec[i]].x 
       << " " << cloud->points[pointIdxVec[i]].y 
       << " " << cloud->points[pointIdxVec[i]].z << std::endl;
  }

  // K nearest neighbor search

  int K = 10;

  std::vector<int> pointIdxNKNSearch;
  std::vector<float> pointNKNSquaredDistance;

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

  if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
  {
    for (std::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;
  }

  // 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 (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    for (std::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;
  }

}

在使用过程中，我的程序报错，代码如下：

    constexpr double voxelResolution = 0.1;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr  octree_map;
    //octree_map=new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(voxelResolution);
    octree_map->deleteTree();
    octree_map->setInputCloud(cloud);
    octree_map->addPointsFromInputCloud();
    int result_index=-1;
    //int queay_index=-1;
    float sqr_distance;
    pcl::PointXYZ point(5,6,7);
    octree_map->approxNearestSearch(point , result_index , sqr_distance);
    std::cout<<"Dis="<<sqr_distance<<std::endl;

报错信息：

constshow: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>; typename boost::detail::sp_member_access<T>::type = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>*]: Assertion `px != 0' failed.

从报错中看，是因为这个指针的问题，改成使用类的实例，报错消失，正确代码如下：

    constexpr double voxelResolution = 0.1;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>  octree_map(voxelResolution);
    octree_map.deleteTree();
    octree_map.setInputCloud(cloud);
    octree_map.addPointsFromInputCloud();
    int result_index=-1;
    //int queay_index=-1;
    float sqr_distance;
    pcl::PointXYZ point(5,6,7);
    octree_map.approxNearestSearch(point , result_index , sqr_distance);
    std::cout<<"Dis="<<sqr_distance<<std::endl;