38 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
39 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
41 #include <pcl/octree/octree_pointcloud_adjacency.h>
44 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
47 ,
OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg)
53 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
57 float minX = std::numeric_limits<float>::max (), minY = std::numeric_limits<float>::max (), minZ = std::numeric_limits<float>::max ();
58 float maxX = -std::numeric_limits<float>::max(), maxY = -std::numeric_limits<float>::max(), maxZ = -std::numeric_limits<float>::max();
60 for (
size_t i = 0; i < input_->size (); ++i)
62 PointT temp (input_->points[i]);
64 transform_func_ (temp);
80 this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
84 LeafContainerT *leaf_container;
86 leaf_vector_.reserve (this->getLeafCount ());
87 for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr)
93 leaf_container->computeData ();
95 computeNeighbors (leaf_key, leaf_container);
97 leaf_vector_.push_back (leaf_container);
100 assert (leaf_vector_.size () == this->getLeafCount ());
104 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
110 transform_func_ (temp);
114 key_arg.
x =
static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
115 key_arg.
y =
static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
116 key_arg.
z =
static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
126 key_arg.
x =
static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
127 key_arg.
y =
static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
128 key_arg.
z =
static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
133 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
138 assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
140 const PointT& point = this->input_->points[pointIdx_arg];
145 this->genOctreeKeyforPoint (point, key);
147 LeafContainerT* container = this->createLeaf(key);
148 container->addPoint (point);
152 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
156 if (key_arg.
x > this->max_key_.x || key_arg.
y > this->max_key_.y || key_arg.
z > this->max_key_.z)
158 PCL_ERROR (
"OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n");
163 int dx_min = (key_arg.
x > 0) ? -1 : 0;
164 int dy_min = (key_arg.
y > 0) ? -1 : 0;
165 int dz_min = (key_arg.
z > 0) ? -1 : 0;
166 int dx_max = (key_arg.
x == this->max_key_.x) ? 0 : 1;
167 int dy_max = (key_arg.
y == this->max_key_.y) ? 0 : 1;
168 int dz_max = (key_arg.
z == this->max_key_.z) ? 0 : 1;
170 for (
int dx = dx_min; dx <= dx_max; ++dx)
172 for (
int dy = dy_min; dy <= dy_max; ++dy)
174 for (
int dz = dz_min; dz <= dz_max; ++dz)
176 neighbor_key.
x =
static_cast<uint32_t
> (key_arg.
x + dx);
177 neighbor_key.
y =
static_cast<uint32_t
> (key_arg.
y + dy);
178 neighbor_key.
z =
static_cast<uint32_t
> (key_arg.
z + dz);
179 LeafContainerT *neighbor = this->findLeaf (neighbor_key);
182 leaf_container->addNeighbor (neighbor);
192 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
194 const PointT& point_arg)
const
197 LeafContainerT* leaf = 0;
199 this->genOctreeKeyforPoint (point_arg, key);
201 leaf = this->findLeaf (key);
207 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
212 voxel_adjacency_graph.clear ();
214 std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map;
217 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
219 this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point);
220 VoxelID node_id = add_vertex (voxel_adjacency_graph);
222 voxel_adjacency_graph[node_id] = centroid_point;
223 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ());
224 leaf_vertex_id_map[leaf_container] = node_id;
228 for (
typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr)
230 typename LeafContainerT::iterator neighbor_itr = (*leaf_itr)->begin ();
231 typename LeafContainerT::iterator neighbor_end = (*leaf_itr)->end ();
232 LeafContainerT* neighbor_container;
233 VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second;
234 PointT p_u = voxel_adjacency_graph[u];
235 for ( ; neighbor_itr != neighbor_end; ++neighbor_itr)
237 neighbor_container = *neighbor_itr;
240 VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second;
241 boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph);
243 PointT p_v = voxel_adjacency_graph[v];
244 float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm ();
245 voxel_adjacency_graph[edge] = dist;
254 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
258 this->genOctreeKeyforPoint (point_arg, key);
260 Eigen::Vector3f sensor(camera_pos.x,
264 Eigen::Vector3f leaf_centroid(static_cast<float> ((static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_),
265 static_cast<float> ((static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_),
266 static_cast<float> ((static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_));
267 Eigen::Vector3f direction = sensor - leaf_centroid;
269 float norm = direction.norm ();
270 direction.normalize ();
271 float precision = 1.0f;
272 const float step_size =
static_cast<const float> (resolution_) * precision;
273 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
277 Eigen::Vector3f p = leaf_centroid;
279 for (
int i = 0; i < nsteps; ++i)
282 p += (direction * step_size);
289 this->genOctreeKeyforPoint (octree_p, key);
292 if ((key == prev_key))
297 LeafContainerT *leaf = this->findLeaf (key);
310 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
const LeafContainer & getLeafContainer() const
Method for retrieving a single leaf container from the octree leaf node.
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
VoxelAdjacencyList::vertex_descriptor VoxelID
Octree leaf node iterator class.
VoxelAdjacencyList::edge_descriptor EdgeID
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided).
const OctreeKey & getCurrentOctreeKey() const
Get octree key for the current iterator octree node.
virtual void addPointIdx(const int point_idx_arg)
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.
A point structure representing Euclidean xyz coordinates.
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.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void addPointsFromInputCloud()
Adds points from cloud to the octree.