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) 2010-2012, Willow Garage, Inc. 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 Willow Garage, Inc. 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 00039 #ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ 00040 #define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ 00041 00042 template<class T, typename REAL> inline void 00043 pcl::recognition::VoxelStructure<T,REAL>::build (const REAL bounds[6], int num_of_voxels[3]) 00044 { 00045 this->clear(); 00046 00047 // Copy the bounds 00048 bounds_[0] = bounds[0]; 00049 bounds_[1] = bounds[1]; 00050 bounds_[2] = bounds[2]; 00051 bounds_[3] = bounds[3]; 00052 bounds_[4] = bounds[4]; 00053 bounds_[5] = bounds[5]; 00054 00055 num_of_voxels_[0] = num_of_voxels[0]; 00056 num_of_voxels_[1] = num_of_voxels[1]; 00057 num_of_voxels_[2] = num_of_voxels[2]; 00058 num_of_voxels_xy_plane_ = num_of_voxels[0]*num_of_voxels[1]; 00059 total_num_of_voxels_ = num_of_voxels_xy_plane_*num_of_voxels[2]; 00060 00061 // Allocate memory for the voxels 00062 voxels_ = new T[total_num_of_voxels_]; 00063 00064 // Compute the spacing between the voxels in x, y and z direction 00065 spacing_[0] = (bounds[1]-bounds[0])/static_cast<REAL> (num_of_voxels[0]); 00066 spacing_[1] = (bounds[3]-bounds[2])/static_cast<REAL> (num_of_voxels[1]); 00067 spacing_[2] = (bounds[5]-bounds[4])/static_cast<REAL> (num_of_voxels[2]); 00068 00069 // Compute the center of the voxel with integer coordinates (0, 0, 0) 00070 min_center_[0] = bounds_[0] + static_cast<REAL> (0.5)*spacing_[0]; 00071 min_center_[1] = bounds_[2] + static_cast<REAL> (0.5)*spacing_[1]; 00072 min_center_[2] = bounds_[4] + static_cast<REAL> (0.5)*spacing_[2]; 00073 } 00074 00075 //================================================================================================================================ 00076 00077 template<class T, typename REAL> inline T* 00078 pcl::recognition::VoxelStructure<T,REAL>::getVoxel (const REAL p[3]) 00079 { 00080 if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) 00081 return NULL; 00082 00083 int x = static_cast<int> ((p[0] - bounds_[0])/spacing_[0]); 00084 int y = static_cast<int> ((p[1] - bounds_[2])/spacing_[1]); 00085 int z = static_cast<int> ((p[2] - bounds_[4])/spacing_[2]); 00086 00087 return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; 00088 } 00089 00090 //================================================================================================================================ 00091 00092 template<class T, typename REAL> inline T* 00093 pcl::recognition::VoxelStructure<T,REAL>::getVoxel (int x, int y, int z) const 00094 { 00095 if ( x < 0 || x >= num_of_voxels_[0] ) return NULL; 00096 if ( y < 0 || y >= num_of_voxels_[1] ) return NULL; 00097 if ( z < 0 || z >= num_of_voxels_[2] ) return NULL; 00098 00099 return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; 00100 } 00101 00102 //================================================================================================================================ 00103 00104 template<class T, typename REAL> inline int 00105 pcl::recognition::VoxelStructure<T,REAL>::getNeighbors (const REAL* p, T **neighs) const 00106 { 00107 if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) 00108 return 0; 00109 00110 const int x = static_cast<int> ((p[0] - bounds_[0])/spacing_[0]); 00111 const int y = static_cast<int> ((p[1] - bounds_[2])/spacing_[1]); 00112 const int z = static_cast<int> ((p[2] - bounds_[4])/spacing_[2]); 00113 00114 const int x_m1 = x-1, x_p1 = x+1; 00115 const int y_m1 = y-1, y_p1 = y+1; 00116 const int z_m1 = z-1, z_p1 = z+1; 00117 00118 T* voxel; 00119 int num_neighs = 0; 00120 00121 voxel = this->getVoxel (x_p1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00122 voxel = this->getVoxel (x_p1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00123 voxel = this->getVoxel (x_p1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00124 voxel = this->getVoxel (x_p1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00125 voxel = this->getVoxel (x_p1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; 00126 voxel = this->getVoxel (x_p1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00127 voxel = this->getVoxel (x_p1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00128 voxel = this->getVoxel (x_p1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00129 voxel = this->getVoxel (x_p1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00130 00131 voxel = this->getVoxel (x , y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00132 voxel = this->getVoxel (x , y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00133 voxel = this->getVoxel (x , y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00134 voxel = this->getVoxel (x , y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00135 voxel = this->getVoxel (x , y , z ); if ( voxel ) neighs[num_neighs++] = voxel; 00136 voxel = this->getVoxel (x , y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00137 voxel = this->getVoxel (x , y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00138 voxel = this->getVoxel (x , y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00139 voxel = this->getVoxel (x , y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00140 00141 voxel = this->getVoxel (x_m1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00142 voxel = this->getVoxel (x_m1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00143 voxel = this->getVoxel (x_m1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00144 voxel = this->getVoxel (x_m1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00145 voxel = this->getVoxel (x_m1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel; 00146 voxel = this->getVoxel (x_m1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00147 voxel = this->getVoxel (x_m1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel; 00148 voxel = this->getVoxel (x_m1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel; 00149 voxel = this->getVoxel (x_m1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel; 00150 00151 return num_neighs; 00152 } 00153 00154 //================================================================================================================================ 00155 00156 #endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_