40#include <pcl/common/point_tests.h>
41#include <pcl/console/print.h>
50#include <pcl/octree/impl/octree_pointcloud.hpp>
53template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
63template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
69 float minX = std::numeric_limits<float>::max(),
70 minY = std::numeric_limits<float>::max(),
71 minZ = std::numeric_limits<float>::max();
72 float maxX = -std::numeric_limits<float>::max(),
73 maxY = -std::numeric_limits<float>::max(),
74 maxZ = -std::numeric_limits<float>::max();
76 for (std::size_t i = 0; i < input_->size(); ++i) {
79 transform_func_(
temp);
100 leaf_vector_.reserve(this->getLeafCount());
101 for (
auto leaf_itr = this->leaf_depth_begin();
leaf_itr != this->leaf_depth_end();
114 assert(leaf_vector_.size() ==
this->getLeafCount());
118template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
123 if (transform_func_) {
125 transform_func_(
temp);
147template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
161 this->genOctreeKeyforPoint(point, key);
163 LeafContainerT*
container = this->createLeaf(key);
168template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
176 PCL_ERROR(
"OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
177 "invalid octree key\n");
195 LeafContainerT* neighbor = this->findLeaf(
neighbor_key);
205template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
211 LeafContainerT*
leaf =
nullptr;
213 this->genOctreeKeyforPoint(
point_arg, key);
215 leaf = this->findLeaf(key);
221template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
231 for (
typename OctreeAdjacencyT::LeafNodeDepthFirstIterator
leaf_itr =
232 this->leaf_depth_begin();
246 for (
typename std::vector<LeafContainerT*>::iterator
leaf_itr = leaf_vector_.begin();
261 float dist = (
p_v.getVector3fMap() -
p_u.getVector3fMap()).norm();
268template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
274 this->genOctreeKeyforPoint(
point_arg, key);
279 static_cast<float>((
static_cast<double>(key.
x) + 0.5f) *
this->resolution_ +
281 static_cast<float>((
static_cast<double>(key.
y) + 0.5f) *
this->resolution_ +
283 static_cast<float>((
static_cast<double>(key.
z) + 0.5f) *
this->resolution_ +
287 float norm = direction.norm();
288 direction.normalize();
290 const float step_size =
static_cast<const float>(resolution_) *
precision;
291 const auto nsteps = std::max<std::size_t>(1, norm / step_size);
297 for (std::size_t i = 0; i <
nsteps; ++i) {
299 p += (direction * step_size);
306 this->genOctreeKeyforPoint(
octree_p, key);
314 LeafContainerT*
leaf = this->findLeaf(key);
325#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
326 template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
typename VoxelAdjacencyList::vertex_descriptor VoxelID
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided).
bool testForOcclusion(const PointT &point_arg, const PointXYZ &camera_pos=PointXYZ(0, 0, 0))
Tests whether input point is occluded from specified camera point by other voxels.
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
void addPointsFromInputCloud()
Adds points from cloud to the octree.
typename VoxelAdjacencyList::edge_descriptor EdgeID
boost:: adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
void addPointIdx(uindex_t point_idx_arg) override
Add point at index from input pointcloud dataset to octree.
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.