40 #include <pcl/common/point_tests.h>
41 #include <pcl/console/print.h>
50 #include <pcl/octree/impl/octree_pointcloud.hpp>
53 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
59 OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
63 template <
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);
96 this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
100 leaf_vector_.reserve(this->getLeafCount());
101 for (
auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end();
103 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
104 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
107 leaf_container->computeData();
109 computeNeighbors(leaf_key, leaf_container);
111 leaf_vector_.push_back(leaf_container);
114 assert(leaf_vector_.size() == this->getLeafCount());
118 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
123 if (transform_func_) {
125 transform_func_(temp);
131 static_cast<unsigned int>((temp.x - this->min_x_) / this->resolution_);
133 static_cast<unsigned int>((temp.y - this->min_y_) / this->resolution_);
135 static_cast<unsigned int>((temp.z - this->min_z_) / this->resolution_);
144 static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
146 static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
148 static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
153 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
160 assert(pointIdx_arg <
static_cast<int>(this->input_->size()));
162 const PointT& point = (*this->input_)[pointIdx_arg];
167 this->genOctreeKeyforPoint(point, key);
169 LeafContainerT* container = this->createLeaf(key);
170 container->addPoint(point);
174 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
180 if (key_arg.
x > this->max_key_.x || key_arg.
y > this->max_key_.y ||
181 key_arg.
z > this->max_key_.z) {
182 PCL_ERROR(
"OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
183 "invalid octree key\n");
188 int dx_min = (key_arg.
x > 0) ? -1 : 0;
189 int dy_min = (key_arg.
y > 0) ? -1 : 0;
190 int dz_min = (key_arg.
z > 0) ? -1 : 0;
191 int dx_max = (key_arg.
x == this->max_key_.x) ? 0 : 1;
192 int dy_max = (key_arg.
y == this->max_key_.y) ? 0 : 1;
193 int dz_max = (key_arg.
z == this->max_key_.z) ? 0 : 1;
195 for (
int dx = dx_min; dx <= dx_max; ++dx) {
196 for (
int dy = dy_min; dy <= dy_max; ++dy) {
197 for (
int dz = dz_min; dz <= dz_max; ++dz) {
198 neighbor_key.
x =
static_cast<std::uint32_t
>(key_arg.
x + dx);
199 neighbor_key.
y =
static_cast<std::uint32_t
>(key_arg.
y + dy);
200 neighbor_key.
z =
static_cast<std::uint32_t
>(key_arg.
z + dz);
201 LeafContainerT* neighbor = this->findLeaf(neighbor_key);
203 leaf_container->addNeighbor(neighbor);
211 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
217 LeafContainerT* leaf =
nullptr;
219 this->genOctreeKeyforPoint(point_arg, key);
221 leaf = this->findLeaf(key);
227 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
234 voxel_adjacency_graph.clear();
236 std::map<LeafContainerT*, VoxelID> leaf_vertex_id_map;
238 this->leaf_depth_begin();
239 leaf_itr != this->leaf_depth_end();
241 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
243 this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point);
244 VoxelID node_id = add_vertex(voxel_adjacency_graph);
246 voxel_adjacency_graph[node_id] = centroid_point;
247 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
248 leaf_vertex_id_map[leaf_container] = node_id;
252 for (
typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin();
253 leaf_itr != leaf_vector_.end();
255 VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second;
256 PointT p_u = voxel_adjacency_graph[u];
257 for (
auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend();
258 neighbor_itr != neighbor_end;
260 LeafContainerT* neighbor_container = *neighbor_itr;
263 VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second;
264 boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph);
266 PointT p_v = voxel_adjacency_graph[v];
267 float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm();
268 voxel_adjacency_graph[edge] = dist;
274 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
280 this->genOctreeKeyforPoint(point_arg, key);
282 Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z);
284 Eigen::Vector3f leaf_centroid(
285 static_cast<float>((
static_cast<double>(key.
x) + 0.5f) * this->resolution_ +
287 static_cast<float>((
static_cast<double>(key.
y) + 0.5f) * this->resolution_ +
289 static_cast<float>((
static_cast<double>(key.
z) + 0.5f) * this->resolution_ +
291 Eigen::Vector3f direction = sensor - leaf_centroid;
293 float norm = direction.norm();
294 direction.normalize();
295 float precision = 1.0f;
296 const float step_size =
static_cast<const float>(resolution_) * precision;
297 const int nsteps = std::max(1,
static_cast<int>(norm / step_size));
301 Eigen::Vector3f p = leaf_centroid;
303 for (
int i = 0; i < nsteps; ++i) {
305 p += (direction * step_size);
312 this->genOctreeKeyforPoint(octree_p, key);
315 if ((key == prev_key))
320 LeafContainerT* leaf = this->findLeaf(key);
331 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
332 template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;