38 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
39 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_
41 #include <pcl/console/print.h>
50 #include <pcl/octree/impl/octree_pointcloud.hpp>
53 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
56 ,
OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg)
62 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
66 float minX = std::numeric_limits<float>::max (), minY = std::numeric_limits<float>::max (), minZ = std::numeric_limits<float>::max ();
67 float maxX = -std::numeric_limits<float>::max(), maxY = -std::numeric_limits<float>::max(), maxZ = -std::numeric_limits<float>::max();
69 for (std::size_t i = 0; i < input_->size (); ++i)
71 PointT temp (input_->points[i]);
73 transform_func_ (temp);
89 this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
93 leaf_vector_.reserve (this->getLeafCount ());
94 for (
auto leaf_itr = this->leaf_depth_begin () ; leaf_itr != this->leaf_depth_end (); ++leaf_itr)
96 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
97 LeafContainerT *leaf_container = &(leaf_itr.getLeafContainer ());
100 leaf_container->computeData ();
102 computeNeighbors (leaf_key, leaf_container);
104 leaf_vector_.push_back (leaf_container);
107 assert (leaf_vector_.size () == this->getLeafCount ());
111 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
117 transform_func_ (temp);
121 key_arg.
x =
static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
122 key_arg.
y =
static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
123 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];
152 this->genOctreeKeyforPoint (point, key);
154 LeafContainerT* container = this->createLeaf(key);
155 container->addPoint (point);
159 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
163 if (key_arg.
x > this->max_key_.x || key_arg.
y > this->max_key_.y || key_arg.
z > this->max_key_.z)
165 PCL_ERROR (
"OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n");
170 int dx_min = (key_arg.
x > 0) ? -1 : 0;
171 int dy_min = (key_arg.
y > 0) ? -1 : 0;
172 int dz_min = (key_arg.
z > 0) ? -1 : 0;
173 int dx_max = (key_arg.
x == this->max_key_.x) ? 0 : 1;
174 int dy_max = (key_arg.
y == this->max_key_.y) ? 0 : 1;
175 int dz_max = (key_arg.
z == this->max_key_.z) ? 0 : 1;
177 for (
int dx = dx_min; dx <= dx_max; ++dx)
179 for (
int dy = dy_min; dy <= dy_max; ++dy)
181 for (
int dz = dz_min; dz <= dz_max; ++dz)
186 LeafContainerT *neighbor = this->findLeaf (neighbor_key);
189 leaf_container->addNeighbor (neighbor);
199 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
201 const PointT& point_arg)
const
204 LeafContainerT* leaf =
nullptr;
206 this->genOctreeKeyforPoint (point_arg, key);
208 leaf = this->findLeaf (key);
214 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
219 voxel_adjacency_graph.clear ();
221 std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map;
224 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
226 this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point);
227 VoxelID node_id = add_vertex (voxel_adjacency_graph);
229 voxel_adjacency_graph[node_id] = centroid_point;
230 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ());
231 leaf_vertex_id_map[leaf_container] = node_id;
235 for (
typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr)
237 VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second;
238 PointT p_u = voxel_adjacency_graph[u];
239 for (
auto neighbor_itr = (*leaf_itr)->cbegin (), neighbor_end = (*leaf_itr)->cend (); neighbor_itr != neighbor_end; ++neighbor_itr)
241 LeafContainerT* neighbor_container = *neighbor_itr;
244 VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second;
245 boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph);
247 PointT p_v = voxel_adjacency_graph[v];
248 float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm ();
249 voxel_adjacency_graph[edge] = dist;
258 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
262 this->genOctreeKeyforPoint (point_arg, key);
264 Eigen::Vector3f sensor(camera_pos.x,
268 Eigen::Vector3f leaf_centroid(
static_cast<float> ((
static_cast<double> (key.
x) + 0.5f) * this->resolution_ + this->min_x_),
269 static_cast<float> ((
static_cast<double> (key.
y) + 0.5f) * this->resolution_ + this->min_y_),
270 static_cast<float> ((
static_cast<double> (key.
z) + 0.5f) * this->resolution_ + this->min_z_));
271 Eigen::Vector3f direction = sensor - leaf_centroid;
273 float norm = direction.norm ();
274 direction.normalize ();
275 float precision = 1.0f;
276 const float step_size =
static_cast<const float> (resolution_) * precision;
277 const int nsteps = std::max (1,
static_cast<int> (norm / step_size));
281 Eigen::Vector3f p = leaf_centroid;
283 for (
int i = 0; i < nsteps; ++i)
286 p += (direction * step_size);
293 this->genOctreeKeyforPoint (octree_p, key);
296 if ((key == prev_key))
301 LeafContainerT *leaf = this->findLeaf (key);
314 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;