Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012, Jeremie Papon 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ 00039 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ 00040 00041 #include <pcl/octree/octree_pointcloud_adjacency.h> 00042 00043 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00044 template<typename PointT, typename LeafContainerT, typename BranchContainerT> 00045 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::OctreePointCloudAdjacency (const double resolution_arg) 00046 : OctreePointCloud<PointT, LeafContainerT, BranchContainerT 00047 , OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg) 00048 { 00049 00050 } 00051 00052 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00053 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void 00054 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud () 00055 { 00056 //double t1,t2; 00057 00058 //t1 = timer_.getTime (); 00059 OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::addPointsFromInputCloud (); 00060 00061 00062 //t2 = timer_.getTime (); 00063 //std::cout << "Add Points:"<<t2-t1<<" ms Num leaves ="<<this->getLeafCount ()<<"\n"; 00064 00065 std::list <std::pair<OctreeKey,LeafContainerT*> > delete_list; 00066 //double t_temp, t_neigh, t_compute, t_getLeaf; 00067 //t_neigh = t_compute = t_getLeaf = 0; 00068 LeafContainerT *leaf_container; 00069 typename OctreeAdjacencyT::LeafNodeIterator leaf_itr; 00070 leaf_vector_.reserve (this->getLeafCount ()); 00071 for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr) 00072 { 00073 //t_temp = timer_.getTime (); 00074 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); 00075 leaf_container = &(leaf_itr.getLeafContainer ()); 00076 //t_getLeaf += timer_.getTime () - t_temp; 00077 00078 //t_temp = timer_.getTime (); 00079 //Run the leaf's compute function 00080 leaf_container->computeData (); 00081 //t_compute += timer_.getTime () - t_temp; 00082 00083 //t_temp = timer_.getTime (); 00084 // std::cout << "Computing neighbors\n"; 00085 computeNeighbors (leaf_key, leaf_container); 00086 //t_neigh += timer_.getTime () - t_temp; 00087 00088 leaf_vector_.push_back (leaf_container); 00089 00090 } 00091 //Go through and delete voxels scheduled 00092 for (typename std::list<std::pair<OctreeKey,LeafContainerT*> >::iterator delete_itr = delete_list.begin (); delete_itr != delete_list.end (); ++delete_itr) 00093 { 00094 leaf_container = delete_itr->second; 00095 //Remove pointer to it from all neighbors 00096 typename std::set<LeafContainerT*>::iterator neighbor_itr = leaf_container->begin (); 00097 typename std::set<LeafContainerT*>::iterator neighbor_end = leaf_container->end (); 00098 for ( ; neighbor_itr != neighbor_end; ++neighbor_itr) 00099 { 00100 //Don't delete self neighbor 00101 if (*neighbor_itr != leaf_container) 00102 (*neighbor_itr)->removeNeighbor (leaf_container); 00103 } 00104 this->removeLeaf (delete_itr->first); 00105 } 00106 00107 //Make sure our leaf vector is correctly sized 00108 assert (leaf_vector_.size () == this->getLeafCount ()); 00109 00110 // std::cout << "Time spent getting leaves ="<<t_getLeaf<<"\n"; 00111 // std::cout << "Time spent computing data in leaves="<<t_compute<<"\n"; 00112 // std::cout << "Time spent computing neighbors="<<t_neigh<<"\n"; 00113 00114 } 00115 00116 ////////////////////////////////////////////////////////////////////////////////////////////// 00117 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void 00118 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const 00119 { 00120 if (transform_func_) 00121 { 00122 PointT temp (point_arg); 00123 transform_func_ (temp); 00124 // calculate integer key for transformed point coordinates 00125 key_arg.x = static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_); 00126 key_arg.y = static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_); 00127 key_arg.z = static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_); 00128 00129 } 00130 else 00131 { 00132 // calculate integer key for point coordinates 00133 key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_); 00134 key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_); 00135 key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_); 00136 } 00137 } 00138 00139 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00140 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void 00141 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::addPointIdx (const int pointIdx_arg) 00142 { 00143 OctreeKey key; 00144 00145 assert (pointIdx_arg < static_cast<int> (this->input_->points.size ())); 00146 00147 const PointT& point = this->input_->points[pointIdx_arg]; 00148 if (!pcl::isFinite (point)) 00149 return; 00150 00151 if (transform_func_) 00152 { 00153 PointT temp (point); 00154 transform_func_ (temp); 00155 this->adoptBoundingBoxToPoint (temp); 00156 } 00157 else 00158 this->adoptBoundingBoxToPoint (point); 00159 00160 // generate key 00161 this->genOctreeKeyforPoint (point, key); 00162 // add point to octree at key 00163 LeafContainerT* container = this->createLeaf(key); 00164 container->addPoint (point); 00165 } 00166 00167 00168 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00169 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void 00170 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container) 00171 { 00172 00173 OctreeKey neighbor_key; 00174 00175 for (int dx = -1; dx <= 1; ++dx) 00176 { 00177 for (int dy = -1; dy <= 1; ++dy) 00178 { 00179 for (int dz = -1; dz <= 1; ++dz) 00180 { 00181 neighbor_key.x = key_arg.x + dx; 00182 neighbor_key.y = key_arg.y + dy; 00183 neighbor_key.z = key_arg.z + dz; 00184 LeafContainerT *neighbor = this->findLeaf (neighbor_key); 00185 if (neighbor) 00186 { 00187 leaf_container->addNeighbor (neighbor); 00188 } 00189 } 00190 } 00191 } 00192 } 00193 00194 00195 00196 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00197 template<typename PointT, typename LeafContainerT, typename BranchContainerT> LeafContainerT* 00198 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::getLeafContainerAtPoint ( 00199 const PointT& point_arg) const 00200 { 00201 OctreeKey key; 00202 LeafContainerT* leaf = 0; 00203 // generate key 00204 this->genOctreeKeyforPoint (point_arg, key); 00205 00206 leaf = this->findLeaf (key); 00207 00208 return leaf; 00209 } 00210 00211 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00212 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void 00213 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph) 00214 { 00215 //TODO Change this to use leaf centers, not centroids! 00216 00217 voxel_adjacency_graph.clear (); 00218 //Add a vertex for each voxel, store ids in map 00219 std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map; 00220 for (typename OctreeAdjacencyT::LeafNodeIterator leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr) 00221 { 00222 OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); 00223 PointT centroid_point; 00224 this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point); 00225 VoxelID node_id = add_vertex (voxel_adjacency_graph); 00226 00227 voxel_adjacency_graph[node_id] = centroid_point; 00228 LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ()); 00229 leaf_vertex_id_map[leaf_container] = node_id; 00230 } 00231 00232 //Iterate through and add edges to adjacency graph 00233 for ( typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr) 00234 { 00235 typename LeafContainerT::iterator neighbor_itr = (*leaf_itr)->begin (); 00236 typename LeafContainerT::iterator neighbor_end = (*leaf_itr)->end (); 00237 LeafContainerT* neighbor_container; 00238 VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second; 00239 PointT p_u = voxel_adjacency_graph[u]; 00240 for ( ; neighbor_itr != neighbor_end; ++neighbor_itr) 00241 { 00242 neighbor_container = *neighbor_itr; 00243 EdgeID edge; 00244 bool edge_added; 00245 VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second; 00246 boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph); 00247 00248 PointT p_v = voxel_adjacency_graph[v]; 00249 float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm (); 00250 voxel_adjacency_graph[edge] = dist; 00251 00252 } 00253 00254 } 00255 00256 } 00257 00258 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00259 template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool 00260 pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>::testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos) 00261 { 00262 OctreeKey key; 00263 this->genOctreeKeyforPoint (point_arg, key); 00264 // This code follows the method in Octree::PointCloud 00265 Eigen::Vector3f sensor(camera_pos.x, 00266 camera_pos.y, 00267 camera_pos.z); 00268 00269 Eigen::Vector3f leaf_centroid(static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_), 00270 static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_), 00271 static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_)); 00272 Eigen::Vector3f direction = sensor - leaf_centroid; 00273 00274 float norm = direction.norm (); 00275 direction.normalize (); 00276 float precision = 1.0f; 00277 const float step_size = static_cast<const float> (resolution_) * precision; 00278 const int nsteps = std::max (1, static_cast<int> (norm / step_size)); 00279 00280 OctreeKey prev_key = key; 00281 // Walk along the line segment with small steps. 00282 Eigen::Vector3f p = leaf_centroid; 00283 PointT octree_p; 00284 for (int i = 0; i < nsteps; ++i) 00285 { 00286 //Start at the leaf voxel, and move back towards sensor. 00287 p += (direction * step_size); 00288 00289 octree_p.x = p.x (); 00290 octree_p.y = p.y (); 00291 octree_p.z = p.z (); 00292 // std::cout << octree_p<< "\n"; 00293 OctreeKey key; 00294 this->genOctreeKeyforPoint (octree_p, key); 00295 00296 // Not a new key, still the same voxel (starts at self). 00297 if ((key == prev_key)) 00298 continue; 00299 00300 prev_key = key; 00301 00302 LeafContainerT *leaf = this->findLeaf (key); 00303 //If the voxel is occupied, there is a possible occlusion 00304 if (leaf) 00305 { 00306 return true; 00307 } 00308 } 00309 00310 //If we didn't run into a voxel on the way to this camera, it can't be occluded. 00311 return false; 00312 00313 } 00314 00315 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>; 00316 00317 #endif 00318