Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 */ 00035 00036 #ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_ 00037 #define PCL_SURFACE_IMPL_MARCHING_CUBES_H_ 00038 00039 #include "pcl/surface/marching_cubes.h" 00040 #include <pcl/common/common.h> 00041 #include <pcl/common/vector_average.h> 00042 #include <pcl/Vertices.h> 00043 #include <pcl/kdtree/kdtree_flann.h> 00044 00046 template <typename PointNT> 00047 pcl::MarchingCubes<PointNT>::MarchingCubes () 00048 {} 00049 00051 template <typename PointNT> 00052 pcl::MarchingCubes<PointNT>::~MarchingCubes () 00053 { 00054 00055 } 00056 00058 template <typename PointNT> void 00059 pcl::MarchingCubes<PointNT>::getBoundingBox () 00060 { 00061 pcl::getMinMax3D (*input_, min_p_, max_p_); 00062 00063 Eigen::Vector4f bounding_box_size = max_p_ - min_p_; 00064 00065 bounding_box_size = max_p_ - min_p_; 00066 PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n", 00067 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ()); 00068 double max_size = 00069 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), 00070 bounding_box_size.z ()); 00071 00072 data_size_ = max_size / leaf_size_; 00073 PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Lower left point is [%f, %f, %f]\n", 00074 min_p_.x (), min_p_.y (), min_p_.z ()); 00075 PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Upper left point is [%f, %f, %f]\n", 00076 max_p_.x (), max_p_.y (), max_p_.z ()); 00077 PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Padding size: %d\n", padding_size_); 00078 PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Leaf size: %f\n", leaf_size_); 00079 00080 gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0); 00081 } 00082 00084 template <typename PointNT> void 00085 pcl::MarchingCubes<PointNT>::interpolateEdge(float iso_level, Eigen::Vector3f &p1, Eigen::Vector3f &p2, float val_p1, float val_p2, Eigen::Vector3f &output) 00086 { 00087 if(iso_level - val_p1 < 0.00001) 00088 output = p1; 00089 if(iso_level - val_p2 < 0.00001) 00090 output = p2; 00091 if(val_p1 - val_p2 < 0.00001) 00092 output = p1; 00093 00094 float mu = (iso_level - val_p1) / (val_p2-val_p1); 00095 output[0] = p1[0] + mu * (p2[0] - p1[0]); 00096 output[1] = p1[1] + mu * (p2[1] - p1[1]); 00097 output[2] = p1[2] + mu * (p2[2] - p1[2]); 00098 } 00099 00101 template <typename PointNT> void 00102 pcl::MarchingCubes<PointNT>::createSurface (Leaf leaf_node, Eigen::Vector3i &index_3d, pcl::PointCloud<pcl::PointXYZ> &cloud, float iso_level) 00103 { 00104 int cubeindex = 0; 00105 Eigen::Vector3f vertex_list[12]; 00106 if (leaf_node.vertex[0] < iso_level) cubeindex |= 1; 00107 if (leaf_node.vertex[1] < iso_level) cubeindex |= 2; 00108 if (leaf_node.vertex[2] < iso_level) cubeindex |= 4; 00109 if (leaf_node.vertex[3] < iso_level) cubeindex |= 8; 00110 if (leaf_node.vertex[4] < iso_level) cubeindex |= 16; 00111 if (leaf_node.vertex[5] < iso_level) cubeindex |= 32; 00112 if (leaf_node.vertex[6] < iso_level) cubeindex |= 64; 00113 if (leaf_node.vertex[7] < iso_level) cubeindex |= 128; 00114 00115 // Cube is entirely in/out of the surface 00116 if (edgeTable[cubeindex] == 0) 00117 return; 00118 00119 std::vector<Eigen::Vector3f> p; 00120 p.resize(8); 00121 Eigen::Vector4f center; 00122 getCellCenterFromIndex(index_3d, center); 00123 for( int i = 0; i < 8; ++i ) 00124 { 00125 Eigen::Vector3f point; 00126 if(i & 0x4) 00127 point[1] = center[1] + leaf_size_/2; 00128 else 00129 point[1] = center[1] - leaf_size_/2; 00130 00131 if(i & 0x2) 00132 point[2] = center[2] + leaf_size_/2; 00133 else 00134 point[2] = center[2] - leaf_size_/2; 00135 00136 if((i & 0x1) ^ ((i >> 1) & 0x1)) 00137 point[0] = center[0] + leaf_size_/2; 00138 else 00139 point[0] = center[0] - leaf_size_/2; 00140 00141 p[i] = point; 00142 } 00143 00144 // Find the vertices where the surface intersects the cube 00145 if (edgeTable[cubeindex] & 1) 00146 interpolateEdge(iso_level,p[0],p[1],leaf_node.vertex[0],leaf_node.vertex[1], vertex_list[0]); 00147 if (edgeTable[cubeindex] & 2) 00148 interpolateEdge(iso_level,p[1],p[2],leaf_node.vertex[1],leaf_node.vertex[2], vertex_list[1]); 00149 if (edgeTable[cubeindex] & 4) 00150 interpolateEdge(iso_level,p[2],p[3],leaf_node.vertex[2],leaf_node.vertex[3], vertex_list[2]); 00151 if (edgeTable[cubeindex] & 8) 00152 interpolateEdge(iso_level,p[3],p[0],leaf_node.vertex[3],leaf_node.vertex[0], vertex_list[3]); 00153 if (edgeTable[cubeindex] & 16) 00154 interpolateEdge(iso_level,p[4],p[5],leaf_node.vertex[4],leaf_node.vertex[5], vertex_list[4]); 00155 if (edgeTable[cubeindex] & 32) 00156 interpolateEdge(iso_level,p[5],p[6],leaf_node.vertex[5],leaf_node.vertex[6], vertex_list[5]); 00157 if (edgeTable[cubeindex] & 64) 00158 interpolateEdge(iso_level,p[6],p[7],leaf_node.vertex[6],leaf_node.vertex[7], vertex_list[6]); 00159 if (edgeTable[cubeindex] & 128) 00160 interpolateEdge(iso_level,p[7],p[4],leaf_node.vertex[7],leaf_node.vertex[4], vertex_list[7]); 00161 if (edgeTable[cubeindex] & 256) 00162 interpolateEdge(iso_level,p[0],p[4],leaf_node.vertex[0],leaf_node.vertex[4], vertex_list[8]); 00163 if (edgeTable[cubeindex] & 512) 00164 interpolateEdge(iso_level,p[1],p[5],leaf_node.vertex[1],leaf_node.vertex[5], vertex_list[9]); 00165 if (edgeTable[cubeindex] & 1024) 00166 interpolateEdge(iso_level,p[2],p[6],leaf_node.vertex[2],leaf_node.vertex[6], vertex_list[10]); 00167 if (edgeTable[cubeindex] & 2048) 00168 interpolateEdge(iso_level,p[3],p[7],leaf_node.vertex[3],leaf_node.vertex[7], vertex_list[11]); 00169 00170 // Create the triangle 00171 for (int i=0;triTable[cubeindex][i]!=-1;i+=3) { 00172 pcl::PointXYZ p1,p2,p3; 00173 p1.x = vertex_list[triTable[cubeindex][i ]][0]; 00174 p1.y = vertex_list[triTable[cubeindex][i ]][1]; 00175 p1.z = vertex_list[triTable[cubeindex][i ]][2]; 00176 cloud.push_back(p1); 00177 p2.x = vertex_list[triTable[cubeindex][i+1]][0]; 00178 p2.y = vertex_list[triTable[cubeindex][i+1]][1]; 00179 p2.z = vertex_list[triTable[cubeindex][i+1]][2]; 00180 cloud.push_back(p2); 00181 p3.x = vertex_list[triTable[cubeindex][i+2]][0]; 00182 p3.y = vertex_list[triTable[cubeindex][i+2]][1]; 00183 p3.z = vertex_list[triTable[cubeindex][i+2]][2]; 00184 cloud.push_back(p3); 00185 } 00186 } 00187 00189 template <typename PointNT> void 00190 pcl::MarchingCubes<PointNT>::getNeighborList1D (Leaf leaf, Eigen::Vector3i &index3d, HashMap &neighbor_list) 00191 { 00192 for (int x = -1; x < 2; ++x) 00193 { 00194 for (int y = -1; y < 2; ++y) 00195 { 00196 for (int z = -1; z < 2; ++z) 00197 { 00198 if(x == 0 && y == 0 && z == 0) 00199 continue; 00200 00201 if(index3d[0] == 0 && x == -1) 00202 continue; 00203 if(index3d[1] == 0 && y == -1) 00204 continue; 00205 if(index3d[2] == 0 && z == -1) 00206 continue; 00207 00208 Eigen::Vector3i neighbor_index; 00209 neighbor_index[0] = index3d[0] + x; 00210 neighbor_index[1] = index3d[1] + y; 00211 neighbor_index[2] = index3d[2] + z; 00212 Leaf neighbor_leaf; 00213 00214 if( (x == 0 || x == 1) && 00215 (y == 0 || y == 1) && 00216 (z == 0 || z == 1) 00217 ) 00218 { 00219 neighbor_leaf.vertex[0] = leaf.vertex[0]; 00220 } 00221 if( (x == 0 || x == -1) && 00222 (y == 0 || y == 1) && 00223 (z == 0 || z == 1) 00224 ) 00225 { 00226 neighbor_leaf.vertex[1] = leaf.vertex[1]; 00227 } 00228 if( (x == 0 || x == -1) && 00229 (y == 0 || y == 1) && 00230 (z == 0 || z == -1) 00231 ) 00232 { 00233 neighbor_leaf.vertex[2] = leaf.vertex[2]; 00234 } 00235 if( (x == 0 || x == 1) && 00236 (y == 0 || y == 1) && 00237 (z == 0 || z == -1) 00238 ) 00239 { 00240 neighbor_leaf.vertex[3] = leaf.vertex[3]; 00241 } 00242 00243 if( (x == 0 || x == 1) && 00244 (y == 0 || y == -1) && 00245 (z == 0 || z == 1) 00246 ) 00247 { 00248 neighbor_leaf.vertex[4] = leaf.vertex[4]; 00249 } 00250 if( (x == 0 || x == -1) && 00251 (y == 0 || y == -1) && 00252 (z == 0 || z == 1) 00253 ) 00254 { 00255 neighbor_leaf.vertex[5] = leaf.vertex[5]; 00256 } 00257 if( (x == 0 || x == -1) && 00258 (y == 0 || y == -1) && 00259 (z == 0 || z == -1) 00260 ) 00261 { 00262 neighbor_leaf.vertex[6] = leaf.vertex[6]; 00263 } 00264 if( (x == 0 || x == 1) && 00265 (y == 0 || y == -1) && 00266 (z == 0 || z == -1) 00267 ) 00268 { 00269 neighbor_leaf.vertex[7] = leaf.vertex[7]; 00270 } 00271 00272 neighbor_list[getIndexIn1D(neighbor_index)] = neighbor_leaf; 00273 } 00274 } 00275 } 00276 } 00277 00279 template <typename PointNT> void 00280 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output) 00281 { 00282 if( !(iso_level_ > 0 && iso_level_ < 1) ) 00283 { 00284 PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_); 00285 output.cloud.width = output.cloud.height = 0; 00286 output.cloud.data.clear (); 00287 output.polygons.clear (); 00288 return; 00289 } 00290 if( leaf_size_ <=0 ) 00291 { 00292 PCL_ERROR ("[pcl::%s::performReconstruction] Invalid leaf size %f! Please use a number greater than 0.\n", getClassName ().c_str (), leaf_size_); 00293 output.cloud.width = output.cloud.height = 0; 00294 output.cloud.data.clear (); 00295 output.polygons.clear (); 00296 return; 00297 00298 } 00299 00300 getBoundingBox(); 00301 00302 // transform the point cloud into a voxel grid 00303 // this needs to be implemented in a child class 00304 voxelizeData(); 00305 00306 // run the actual marching cubes algorithm, store it into a point cloud, 00307 // and copy the point cloud + connectivity into output 00308 pcl::PointCloud<pcl::PointXYZ> cloud; 00309 BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_) 00310 { 00311 Eigen::Vector3i leaf_index; 00312 getIndexIn3D (entry.first, leaf_index); 00313 createSurface (entry.second, leaf_index, cloud, iso_level_); 00314 } 00315 pcl::toROSMsg (cloud, output.cloud); 00316 output.polygons.resize (cloud.size () / 3); 00317 00318 for (size_t i = 0; i < output.polygons.size (); ++i) 00319 { 00320 pcl::Vertices v; 00321 v.vertices.resize (3); 00322 for (int j = 0; j < 3; ++j) 00323 v.vertices[j] = i*3+j; 00324 output.polygons[i] = v; 00325 } 00326 } 00327 00328 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>; 00329 00330 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_ 00331