41 #include <pcl/common/point_tests.h>
42 #include <pcl/console/print.h>
51 #include <pcl/octree/impl/octree_pointcloud.hpp>
54 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
60 OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
64 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
70 float minX = std::numeric_limits<float>::max(),
71 minY = std::numeric_limits<float>::max(),
72 minZ = std::numeric_limits<float>::max();
73 float maxX = -std::numeric_limits<float>::max(),
74 maxY = -std::numeric_limits<float>::max(),
75 maxZ = -std::numeric_limits<float>::max();
77 for (std::size_t i = 0; i < input_->size(); ++i) {
78 PointT temp(input_->points[i]);
80 transform_func_(temp);
97 this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
101 leaf_vector_.reserve(this->getLeafCount());
102 for (
auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end();
104 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
105 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
108 leaf_container->computeData();
110 computeNeighbors(leaf_key, leaf_container);
112 leaf_vector_.push_back(leaf_container);
115 assert(leaf_vector_.size() == this->getLeafCount());
119 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
124 if (transform_func_) {
126 transform_func_(temp);
132 static_cast<unsigned int>((temp.x - this->min_x_) / this->resolution_);
134 static_cast<unsigned int>((temp.y - this->min_y_) / this->resolution_);
136 static_cast<unsigned int>((temp.z - this->min_z_) / this->resolution_);
145 static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
147 static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
149 static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
154 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
161 assert(pointIdx_arg <
static_cast<int>(this->input_->points.size()));
163 const PointT& point = this->input_->points[pointIdx_arg];
168 this->genOctreeKeyforPoint(point, key);
170 LeafContainerT* container = this->createLeaf(key);
171 container->addPoint(point);
175 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
181 if (key_arg.
x > this->max_key_.x || key_arg.
y > this->max_key_.y ||
182 key_arg.
z > this->max_key_.z) {
183 PCL_ERROR(
"OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
184 "invalid octree key\n");
189 int dx_min = (key_arg.
x > 0) ? -1 : 0;
190 int dy_min = (key_arg.
y > 0) ? -1 : 0;
191 int dz_min = (key_arg.
z > 0) ? -1 : 0;
192 int dx_max = (key_arg.
x == this->max_key_.x) ? 0 : 1;
193 int dy_max = (key_arg.
y == this->max_key_.y) ? 0 : 1;
194 int dz_max = (key_arg.
z == this->max_key_.z) ? 0 : 1;
196 for (
int dx = dx_min; dx <= dx_max; ++dx) {
197 for (
int dy = dy_min; dy <= dy_max; ++dy) {
198 for (
int dz = dz_min; dz <= dz_max; ++dz) {
202 LeafContainerT* neighbor = this->findLeaf(neighbor_key);
204 leaf_container->addNeighbor(neighbor);
212 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
218 LeafContainerT* leaf =
nullptr;
220 this->genOctreeKeyforPoint(point_arg, key);
222 leaf = this->findLeaf(key);
228 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
235 voxel_adjacency_graph.clear();
237 std::map<LeafContainerT*, VoxelID> leaf_vertex_id_map;
239 this->leaf_depth_begin();
240 leaf_itr != this->leaf_depth_end();
242 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
244 this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point);
245 VoxelID node_id = add_vertex(voxel_adjacency_graph);
247 voxel_adjacency_graph[node_id] = centroid_point;
248 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
249 leaf_vertex_id_map[leaf_container] = node_id;
253 for (
typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin();
254 leaf_itr != leaf_vector_.end();
256 VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second;
257 PointT p_u = voxel_adjacency_graph[u];
258 for (
auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend();
259 neighbor_itr != neighbor_end;
261 LeafContainerT* neighbor_container = *neighbor_itr;
264 VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second;
265 boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph);
267 PointT p_v = voxel_adjacency_graph[v];
268 float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm();
269 voxel_adjacency_graph[edge] = dist;
275 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
281 this->genOctreeKeyforPoint(point_arg, key);
283 Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z);
285 Eigen::Vector3f leaf_centroid(
286 static_cast<float>((
static_cast<double>(key.
x) + 0.5f) * this->resolution_ +
288 static_cast<float>((
static_cast<double>(key.
y) + 0.5f) * this->resolution_ +
290 static_cast<float>((
static_cast<double>(key.
z) + 0.5f) * this->resolution_ +
292 Eigen::Vector3f direction = sensor - leaf_centroid;
294 float norm = direction.norm();
295 direction.normalize();
296 float precision = 1.0f;
297 const float step_size =
static_cast<const float>(resolution_) * precision;
298 const int nsteps = std::max(1,
static_cast<int>(norm / step_size));
302 Eigen::Vector3f p = leaf_centroid;
304 for (
int i = 0; i < nsteps; ++i) {
306 p += (direction * step_size);
313 this->genOctreeKeyforPoint(octree_p, key);
316 if ((key == prev_key))
321 LeafContainerT* leaf = this->findLeaf(key);
332 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
333 template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
Octree leaf node iterator class.
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 addPointIdx(const int point_idx_arg) override
Add point at index from input pointcloud dataset to octree.
void addPointsFromInputCloud()
Adds points from cloud to the octree.
typename VoxelAdjacencyList::edge_descriptor EdgeID
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Defines some geometrical functions and utility functions.
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...
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.