1.Octree相关原理
2.PCL官方demo
本次运行了官方的基于八叉树的空间划分及搜索操作的demo
代码如下：
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.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>);
// 随机生成1000个无序的点
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);
}
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);
//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 (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;
}
//KNN 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 (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;
}
//radiuSearch
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 (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;
}
}
camkelist.txt的内容如下：
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(octree_search)
find_package(PCL 1.2 REQUIRED)
include_directories(${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable(octree_search octree_search.cpp)
target_link_libraries(octree_search ${
PCL_LIBRARIES})
运行结果如下：
3.源码分析
对于上述官方的demo中提到的函数，我找了这些函数的源码，以共大家更好的理解算法。
/** \brief Provide a pointer to the input data set.
* \param[in] cloud_arg the const boost shared pointer to a PointCloud message
* \param[in] indices_arg the point indices subset that is to be used from \a cloud -
* if 0 the whole point cloud is used
*/
inline void
setInputCloud(const PointCloudConstPtr& cloud_arg,
const IndicesConstPtr& indices_arg = IndicesConstPtr())
{
input_ = cloud_arg;
indices_ = indices_arg;
}
template <typename PointT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
addPointsFromInputCloud()
{
if (indices_) {
//有子集的情况下
for (const int& index : *indices_) {
assert((index >= 0) && (index < static_cast<int>(input_->size())));
if (isFinite((*input_)[index])) {
// add points to octree
this->addPointIdx(index);
}
}
}
else {
//没有子集，加载全部点云
for (std::size_t i = 0; i < input_->size(); i++) {
if (isFinite((*input_)[i])) {
// add points to octree
this->addPointIdx(static_cast<unsigned int>(i));
}
}
}
}
//voxelSearch() ：Search for neighbors within a voxel at given point referenced by a point index.
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
bool
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::voxelSearch(
const PointT& point, std::vector<int>& point_idx_data)
{
assert(isFinite(point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
bool b_success = false;
// generate key
this->genOctreeKeyforPoint(point, key); //找到point在八叉树中间的下x，y，z索引号值
LeafContainerT* leaf = this->findLeaf(key);
if (leaf) {
(*leaf).getPointIndices(point_idx_data); //返回该叶结点所有子节点的索引号
b_success = true;
}
return (b_success);
}
//genOctreeKeyforPoint()：Generate octree key for voxel at a given point.
template <typename PointT,
typename LeafContainerT,
typename BranchContainerT,
typename OctreeT>
void
pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
{
// calculate integer key for point coordinates
key_arg.x =
static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
key_arg.y =
static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
key_arg.z =
static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
assert(key_arg.x <= this->max_key_.x);
assert(key_arg.y <= this->max_key_.y);
assert(key_arg.z <= this->max_key_.z);
}
//findLeaf():
/** \brief Find leaf node
* \param key_arg: octree key addressing a leaf node.
* \return pointer to leaf node. If leaf node is not found, this pointer returns 0.
*/
LeafContainerT*
findLeaf(const OctreeKey& key_arg) const
{
LeafContainerT* result = nullptr;
findLeafRecursive(key_arg, depth_mask_, root_node_, result);
return result;
}
//getPointIndices()
/** \brief Retrieve point indices from container. This container stores only a single
* point index
* \param[out] data_vector_arg vector of point indices to be stored within
* data vector
*/
void
getPointIndices(std::vector<int>& data_vector_arg) const
{
if (data_ >= 0)
data_vector_arg.push_back(data_);
}
//nearestKSearch()
/*
Search for k-nearest neighbors at given query point.
Parameters
[in] p_q the given query point
[in] k the number of neighbors to search for
[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!)
Return numbers of neighbors found
*/
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::nearestKSearch(
const PointT& p_q,
int k,
std::vector<int>& k_indices,
std::vector<float>& k_sqr_distances)
{
assert(this->leaf_count_ > 0);
assert(isFinite(p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
k_indices.clear();
k_sqr_distances.clear();
if (k < 1)
return 0;
prioPointQueueEntry point_entry;
std::vector<prioPointQueueEntry> point_candidates;
OctreeKey key;
key.x = key.y = key.z = 0;
// initialize smallest point distance in search with high value
double smallest_dist = std::numeric_limits<double>::max();
getKNearestNeighborRecursive(
p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
unsigned int result_count = static_cast<unsigned int>(point_candidates.size());
k_indices.resize(result_count);
k_sqr_distances.resize(result_count);
for (unsigned int i = 0; i < result_count; ++i) {
k_indices[i] = point_candidates[i].point_idx_;
k_sqr_distances[i] = point_candidates[i].point_distance_;
}
return static_cast<int>(k_indices.size());
}
//getKNearestNeighborRecursive()
/*
Recursive search method that explores the octree and finds the K nearest neighbors.
Parameters
[in] point query point
[in] K amount of nearest neighbors to be found
[in] node current octree node to be explored
[in] key octree key addressing a leaf node.
[in] tree_depth current depth/level in the octree
[in] squared_search_radius squared search radius distance
[out] point_candidates priority queue of nearest neigbor point candidates
Returns squared search radius based on current point candidate set found */
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
double
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
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
{
std::vector<prioBranchQueueEntry> search_heap;
search_heap.resize(8);
OctreeKey new_key;
double smallest_squared_dist = squared_search_radius;
// get spatial voxel information
double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
if (this->branchHasChild(*node, child_idx)) {
PointT voxel_center;
search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey(
search_heap[child_idx].key, tree_depth, voxel_center);
// generate new priority queue element
search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
}
else {
search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
}
}
std::sort(search_heap.begin(), search_heap.end());
// iterate over all children in priority queue
// check if the distance to search candidate is smaller than the best point distance
// (smallest_squared_dist)
while ((!search_heap.empty()) &&
(search_heap.back().point_distance <
smallest_squared_dist + voxelSquaredDiameter / 4.0 +
sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
const OctreeNode* child_node;
// read from priority queue element
child_node = search_heap.back().node;
new_key = search_heap.back().key;
if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
smallest_squared_dist =
getKNearestNeighborRecursive(point,
K,
static_cast<const BranchNode*>(child_node),
new_key,
tree_depth + 1,
smallest_squared_dist,
point_candidates);
}
else {
// we reached leaf node level
std::vector<int> decoded_point_vector;
const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
// decode leaf node into decoded_point_vector
(*child_leaf)->getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int& point_index : decoded_point_vector) {
const PointT& candidate_point = this->getPointByIndex(point_index);
// calculate point distance to search point
float squared_dist = pointSquaredDist(candidate_point, point);
// check if a closer match is found
if (squared_dist < smallest_squared_dist) {
prioPointQueueEntry point_entry;
point_entry.point_distance_ = squared_dist;
point_entry.point_idx_ = point_index;
point_candidates.push_back(point_entry);
}
}
std::sort(point_candidates.begin(), point_candidates.end());
if (point_candidates.size() > K)
point_candidates.resize(K);
if (point_candidates.size() == K)
smallest_squared_dist = point_candidates.back().point_distance_;
}
// pop element from priority queue
search_heap.pop_back();
}
return (smallest_squared_dist);
}
//radiusSearch()
/*
Search for all neighbors of query point that are within a given radius.
Parameters
[in] p_q the given query point
[in] radius the radius of the sphere bounding all of p_q's neighbors
[out] k_indices the resultant indices of the neighboring points
[out] k_sqr_distances the resultant squared distances to the neighboring points
[in] max_nn if given, bounds the maximum returned neighbors to this value
Returns
number of neighbors found in radius */
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
int
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::radiusSearch(
const PointT& p_q,
const double radius,
std::vector<int>& k_indices,
std::vector<float>& k_sqr_distances,
unsigned int max_nn) const
{
assert(isFinite(p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear();
k_sqr_distances.clear();
getNeighborsWithinRadiusRecursive(p_q,
radius * radius,
this->root_node_,
key,
1,
k_indices,
k_sqr_distances,
max_nn);
return (static_cast<int>(k_indices.size()));
}
//getNeighborsWithinRadiusRecursive()
/*
Recursive search method that explores the octree and finds neighbors within a given radius.
Parameters
[in] point query point
[in] radiusSquared squared search radius
[in] node current octree node to be explored
[in] key octree key addressing a leaf node.
[in] tree_depth current depth/level in the octree
[out] k_indices vector of indices found to be neighbors of query point
[out] k_sqr_distances squared distances of neighbors to query point
[in] max_nn maximum of neighbors to be found
*/
template <typename PointT, typename LeafContainerT, typename BranchContainerT>
void
OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
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
{
// get spatial voxel information
double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
// iterate over all children
for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
if (!this->branchHasChild(*node, child_idx))
continue;
const OctreeNode* child_node;
child_node = this->getBranchChildPtr(*node, child_idx);
OctreeKey new_key;
PointT voxel_center;
float squared_dist;
// generate new key for current branch voxel
new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
// generate voxel center point for voxel at key
this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
// calculate distance to search point
squared_dist = pointSquaredDist(static_cast<const PointT&>(voxel_center), point);
// if distance is smaller than search radius
if (squared_dist + this->epsilon_ <=
voxel_squared_diameter / 4.0 + radiusSquared +
sqrt(voxel_squared_diameter * radiusSquared)) {
if (tree_depth < this->octree_depth_) {
// we have not reached maximum tree depth
getNeighborsWithinRadiusRecursive(point,
radiusSquared,
static_cast<const BranchNode*>(child_node),
new_key,
tree_depth + 1,
k_indices,
k_sqr_distances,
max_nn);
if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
return;
}
else {
// we reached leaf node level
const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
std::vector<int> decoded_point_vector;
// decode leaf node into decoded_point_vector
(*child_leaf)->getPointIndices(decoded_point_vector);
// Linearly iterate over all decoded (unsorted) points
for (const int& index : decoded_point_vector) {
const PointT& candidate_point = this->getPointByIndex(index);
// calculate point distance to search point
squared_dist = pointSquaredDist(candidate_point, point);
// check if a match is found
if (squared_dist > radiusSquared)
continue;
// add point to result vector
k_indices.push_back(index);
k_sqr_distances.push_back(squared_dist);
if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
return;
}
}
}
}
}