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
65 std::list <std::pair<OctreeKey,LeafContainerT*> > delete_list;
68 LeafContainerT *leaf_container;
70 leaf_vector_.reserve (this->getLeafCount ());
71 for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr)
80 leaf_container->computeData ();
85 computeNeighbors (leaf_key, leaf_container);
88 leaf_vector_.push_back (leaf_container);
92 for (
typename std::list<std::pair<OctreeKey,LeafContainerT*> >::
iterator delete_itr = delete_list.begin (); delete_itr != delete_list.end (); ++delete_itr)
94 leaf_container = delete_itr->second;
96 typename std::set<LeafContainerT*>::iterator neighbor_itr = leaf_container->begin ();
97 typename std::set<LeafContainerT*>::iterator neighbor_end = leaf_container->end ();
98 for ( ; neighbor_itr != neighbor_end; ++neighbor_itr)
101 if (*neighbor_itr != leaf_container)
102 (*neighbor_itr)->removeNeighbor (leaf_container);
104 this->removeLeaf (delete_itr->first);
108 assert (leaf_vector_.size () == this->getLeafCount ());
117 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
123 transform_func_ (temp);
125 key_arg.
x =
static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
126 key_arg.
y =
static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
127 key_arg.
z =
static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
133 key_arg.
x =
static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
134 key_arg.
y =
static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
135 key_arg.
z =
static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
140 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
145 assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
147 const PointT& point = this->input_->points[pointIdx_arg];
154 transform_func_ (temp);
155 this->adoptBoundingBoxToPoint (temp);
158 this->adoptBoundingBoxToPoint (point);
161 this->genOctreeKeyforPoint (point, key);
163 LeafContainerT* container = this->createLeaf(key);
164 container->addPoint (point);
169 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
175 for (
int dx = -1; dx <= 1; ++dx)
177 for (
int dy = -1; dy <= 1; ++dy)
179 for (
int dz = -1; dz <= 1; ++dz)
181 neighbor_key.
x = key_arg.
x + dx;
182 neighbor_key.
y = key_arg.
y + dy;
183 neighbor_key.
z = key_arg.
z + dz;
184 LeafContainerT *neighbor = this->findLeaf (neighbor_key);
187 leaf_container->addNeighbor (neighbor);
197 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
199 const PointT& point_arg)
const
202 LeafContainerT* leaf = 0;
204 this->genOctreeKeyforPoint (point_arg, key);
206 leaf = this->findLeaf (key);
212 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
217 voxel_adjacency_graph.clear ();
219 std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map;
222 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
224 this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point);
225 VoxelID node_id = add_vertex (voxel_adjacency_graph);
227 voxel_adjacency_graph[node_id] = centroid_point;
228 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ());
229 leaf_vertex_id_map[leaf_container] = node_id;
233 for (
typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr)
235 typename LeafContainerT::iterator neighbor_itr = (*leaf_itr)->begin ();
236 typename LeafContainerT::iterator neighbor_end = (*leaf_itr)->end ();
237 LeafContainerT* neighbor_container;
238 VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second;
239 PointT p_u = voxel_adjacency_graph[u];
240 for ( ; neighbor_itr != neighbor_end; ++neighbor_itr)
242 neighbor_container = *neighbor_itr;
245 VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second;
246 boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph);
248 PointT p_v = voxel_adjacency_graph[v];
249 float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm ();
250 voxel_adjacency_graph[edge] = dist;
259 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
263 this->genOctreeKeyforPoint (point_arg, key);
265 Eigen::Vector3f sensor(camera_pos.x,
269 Eigen::Vector3f leaf_centroid(static_cast<float> ((static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_),
270 static_cast<float> ((static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_),
271 static_cast<float> ((static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_));
272 Eigen::Vector3f direction = sensor - leaf_centroid;
274 float norm = direction.norm ();
275 direction.normalize ();
276 float precision = 1.0f;
277 const float step_size =
static_cast<const float> (resolution_) * precision;
278 const int nsteps = std::max (1, static_cast<int> (norm / step_size));
282 Eigen::Vector3f p = leaf_centroid;
284 for (
int i = 0; i < nsteps; ++i)
287 p += (direction * step_size);
294 this->genOctreeKeyforPoint (octree_p, key);
297 if ((key == prev_key))
302 LeafContainerT *leaf = this->findLeaf (key);
315 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
VoxelAdjacencyList::vertex_descriptor VoxelID
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
VoxelAdjacencyList::edge_descriptor EdgeID
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 addPointsFromInputCloud()
Adds points from cloud to the octree.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
virtual void addPointIdx(const int pointIdx_arg)
Add point at index from input pointcloud dataset to octree.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided)
LeafVectorT::iterator iterator
Octree leaf node iterator class.
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
const LeafContainer & getLeafContainer() const
Method for retrieving a single leaf container from the octree leaf node.
const OctreeKey & getCurrentOctreeKey() const
Get octree key for the current iterator octree node.