Point Cloud Library (PCL) 1.13.0
octree_pointcloud_adjacency.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012, Jeremie Papon
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
40#include <pcl/common/point_tests.h> // for pcl::isFinite
41#include <pcl/console/print.h>
42
43/*
44 * OctreePointCloudAdjacency is not precompiled, since it's used in other
45 * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
46 * used, octree_pointcloud_adjacency.h includes this file but octree_pointcloud.h
47 * would not include the implementation because it's precompiled. So we need to
48 * include it here "manually".
49 */
50#include <pcl/octree/impl/octree_pointcloud.hpp>
51
52//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53template <typename PointT, typename LeafContainerT, typename BranchContainerT>
55 OctreePointCloudAdjacency(const double resolution_arg)
57 LeafContainerT,
58 BranchContainerT,
59 OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
60{}
61
62////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63template <typename PointT, typename LeafContainerT, typename BranchContainerT>
64void
67{
68 // double t1,t2;
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();
75
76 for (std::size_t i = 0; i < input_->size(); ++i) {
77 PointT temp((*input_)[i]);
78 if (transform_func_) // Search for point with
79 transform_func_(temp);
80 if (!pcl::isFinite(
81 temp)) // Check to make sure transform didn't make point not finite
82 continue;
83 if (temp.x < minX)
84 minX = temp.x;
85 if (temp.y < minY)
86 minY = temp.y;
87 if (temp.z < minZ)
88 minZ = temp.z;
89 if (temp.x > maxX)
90 maxX = temp.x;
91 if (temp.y > maxY)
92 maxY = temp.y;
93 if (temp.z > maxZ)
94 maxZ = temp.z;
95 }
96 this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
97
99
100 leaf_vector_.reserve(this->getLeafCount());
101 for (auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end();
102 ++leaf_itr) {
103 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
104 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
105
106 // Run the leaf's compute function
107 leaf_container->computeData();
108
109 computeNeighbors(leaf_key, leaf_container);
110
111 leaf_vector_.push_back(leaf_container);
112 }
113 // Make sure our leaf vector is correctly sized
114 assert(leaf_vector_.size() == this->getLeafCount());
115}
116
117//////////////////////////////////////////////////////////////////////////////////////////////
118template <typename PointT, typename LeafContainerT, typename BranchContainerT>
119void
121 genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
122{
123 if (transform_func_) {
124 PointT temp(point_arg);
125 transform_func_(temp);
126 // calculate integer key for transformed point coordinates
127 if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it
128 // gets default key
129 {
130 key_arg.x = static_cast<uindex_t>((temp.x - this->min_x_) / this->resolution_);
131 key_arg.y = static_cast<uindex_t>((temp.y - this->min_y_) / this->resolution_);
132 key_arg.z = static_cast<uindex_t>((temp.z - this->min_z_) / this->resolution_);
133 }
134 else {
135 key_arg = OctreeKey();
136 }
137 }
138 else {
139 // calculate integer key for point coordinates
140 key_arg.x = static_cast<uindex_t>((point_arg.x - this->min_x_) / this->resolution_);
141 key_arg.y = static_cast<uindex_t>((point_arg.y - this->min_y_) / this->resolution_);
142 key_arg.z = static_cast<uindex_t>((point_arg.z - this->min_z_) / this->resolution_);
143 }
144}
145
146//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
147template <typename PointT, typename LeafContainerT, typename BranchContainerT>
148void
150 addPointIdx(const uindex_t pointIdx_arg)
151{
152 OctreeKey key;
153
154 assert(pointIdx_arg < this->input_->size());
155
156 const PointT& point = (*this->input_)[pointIdx_arg];
157 if (!pcl::isFinite(point))
158 return;
159
160 // generate key
161 this->genOctreeKeyforPoint(point, key);
162 // add point to octree at key
163 LeafContainerT* container = this->createLeaf(key);
164 container->addPoint(point);
165}
166
167//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
168template <typename PointT, typename LeafContainerT, typename BranchContainerT>
169void
171 computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container)
172{
173 // Make sure requested key is valid
174 if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y ||
175 key_arg.z > this->max_key_.z) {
176 PCL_ERROR("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
177 "invalid octree key\n");
178 return;
179 }
180
181 OctreeKey neighbor_key;
182 int dx_min = (key_arg.x > 0) ? -1 : 0;
183 int dy_min = (key_arg.y > 0) ? -1 : 0;
184 int dz_min = (key_arg.z > 0) ? -1 : 0;
185 int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1;
186 int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1;
187 int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1;
188
189 for (int dx = dx_min; dx <= dx_max; ++dx) {
190 for (int dy = dy_min; dy <= dy_max; ++dy) {
191 for (int dz = dz_min; dz <= dz_max; ++dz) {
192 neighbor_key.x = static_cast<std::uint32_t>(key_arg.x + dx);
193 neighbor_key.y = static_cast<std::uint32_t>(key_arg.y + dy);
194 neighbor_key.z = static_cast<std::uint32_t>(key_arg.z + dz);
195 LeafContainerT* neighbor = this->findLeaf(neighbor_key);
196 if (neighbor) {
197 leaf_container->addNeighbor(neighbor);
198 }
199 }
200 }
201 }
202}
203
204//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
205template <typename PointT, typename LeafContainerT, typename BranchContainerT>
206LeafContainerT*
208 getLeafContainerAtPoint(const PointT& point_arg) const
209{
210 OctreeKey key;
211 LeafContainerT* leaf = nullptr;
212 // generate key
213 this->genOctreeKeyforPoint(point_arg, key);
214
215 leaf = this->findLeaf(key);
216
217 return leaf;
218}
219
220//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
221template <typename PointT, typename LeafContainerT, typename BranchContainerT>
222void
225{
226 // TODO Change this to use leaf centers, not centroids!
227
228 voxel_adjacency_graph.clear();
229 // Add a vertex for each voxel, store ids in map
230 std::map<LeafContainerT*, VoxelID> leaf_vertex_id_map;
231 for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr =
232 this->leaf_depth_begin();
233 leaf_itr != this->leaf_depth_end();
234 ++leaf_itr) {
235 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
236 PointT centroid_point;
237 this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point);
238 VoxelID node_id = add_vertex(voxel_adjacency_graph);
239
240 voxel_adjacency_graph[node_id] = centroid_point;
241 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
242 leaf_vertex_id_map[leaf_container] = node_id;
243 }
244
245 // Iterate through and add edges to adjacency graph
246 for (auto leaf_itr = leaf_vector_.begin(); leaf_itr != leaf_vector_.end();
247 ++leaf_itr) {
248 VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second;
249 PointT p_u = voxel_adjacency_graph[u];
250 for (auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend();
251 neighbor_itr != neighbor_end;
252 ++neighbor_itr) {
253 LeafContainerT* neighbor_container = *neighbor_itr;
254 EdgeID edge;
255 bool edge_added;
256 VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second;
257 boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph);
258
259 PointT p_v = voxel_adjacency_graph[v];
260 float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm();
261 voxel_adjacency_graph[edge] = dist;
262 }
263 }
264}
265
266//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
267template <typename PointT, typename LeafContainerT, typename BranchContainerT>
268bool
270 testForOcclusion(const PointT& point_arg, const PointXYZ& camera_pos)
271{
272 OctreeKey key;
273 this->genOctreeKeyforPoint(point_arg, key);
274 // This code follows the method in Octree::PointCloud
275 Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z);
276
277 Eigen::Vector3f leaf_centroid(
278 static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
279 this->min_x_),
280 static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
281 this->min_y_),
282 static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
283 this->min_z_));
284 Eigen::Vector3f direction = sensor - leaf_centroid;
285
286 float norm = direction.norm();
287 direction.normalize();
288 float precision = 1.0f;
289 const float step_size = static_cast<const float>(resolution_) * precision;
290 const auto nsteps = std::max<std::size_t>(1, norm / step_size);
291
292 OctreeKey prev_key = key;
293 // Walk along the line segment with small steps.
294 Eigen::Vector3f p = leaf_centroid;
295 PointT octree_p;
296 for (std::size_t i = 0; i < nsteps; ++i) {
297 // Start at the leaf voxel, and move back towards sensor.
298 p += (direction * step_size);
299
300 octree_p.x = p.x();
301 octree_p.y = p.y();
302 octree_p.z = p.z();
303 // std::cout << octree_p<< "\n";
304 OctreeKey key;
305 this->genOctreeKeyforPoint(octree_p, key);
306
307 // Not a new key, still the same voxel (starts at self).
308 if ((key == prev_key))
309 continue;
310
311 prev_key = key;
312
313 LeafContainerT* leaf = this->findLeaf(key);
314 // If the voxel is occupied, there is a possible occlusion
315 if (leaf) {
316 return true;
317 }
318 }
319
320 // If we didn't run into a voxel on the way to this camera, it can't be occluded.
321 return false;
322}
323
324#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
325 template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
Octree key class
Definition: octree_key.h:54
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 addPointsFromInputCloud()
Adds points from cloud to the octree.
typename VoxelAdjacencyList::edge_descriptor EdgeID
void addPointIdx(uindex_t point_idx_arg) override
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.
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
Octree pointcloud class
void addPointsFromInputCloud()
Add points from input point cloud to octree.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
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...
Definition: point_tests.h:55
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.