Point Cloud Library (PCL)
1.7.0
|
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 00045 ////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointNT> 00047 pcl::MarchingCubes<PointNT>::MarchingCubes () 00048 : min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ () 00049 { 00050 } 00051 00052 ////////////////////////////////////////////////////////////////////////////////////////////// 00053 template <typename PointNT> 00054 pcl::MarchingCubes<PointNT>::~MarchingCubes () 00055 { 00056 } 00057 00058 ////////////////////////////////////////////////////////////////////////////////////////////// 00059 template <typename PointNT> void 00060 pcl::MarchingCubes<PointNT>::getBoundingBox () 00061 { 00062 pcl::getMinMax3D (*input_, min_p_, max_p_); 00063 00064 min_p_ -= (max_p_ - min_p_) * percentage_extend_grid_/2; 00065 max_p_ += (max_p_ - min_p_) * percentage_extend_grid_/2; 00066 00067 Eigen::Vector4f bounding_box_size = max_p_ - min_p_; 00068 00069 bounding_box_size = max_p_ - min_p_; 00070 PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n", 00071 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ()); 00072 double max_size = 00073 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), 00074 bounding_box_size.z ()); 00075 (void)max_size; 00076 // ???? 00077 // data_size_ = static_cast<uint64_t> (max_size / leaf_size_); 00078 PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Lower left point is [%f, %f, %f]\n", 00079 min_p_.x (), min_p_.y (), min_p_.z ()); 00080 PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Upper left point is [%f, %f, %f]\n", 00081 max_p_.x (), max_p_.y (), max_p_.z ()); 00082 } 00083 00084 00085 ////////////////////////////////////////////////////////////////////////////////////////////// 00086 template <typename PointNT> void 00087 pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1, 00088 Eigen::Vector3f &p2, 00089 float val_p1, 00090 float val_p2, 00091 Eigen::Vector3f &output) 00092 { 00093 float mu = (iso_level_ - val_p1) / (val_p2-val_p1); 00094 output = p1 + mu * (p2 - p1); 00095 } 00096 00097 00098 ////////////////////////////////////////////////////////////////////////////////////////////// 00099 template <typename PointNT> void 00100 pcl::MarchingCubes<PointNT>::createSurface (std::vector<float> &leaf_node, 00101 Eigen::Vector3i &index_3d, 00102 pcl::PointCloud<PointNT> &cloud) 00103 { 00104 int cubeindex = 0; 00105 Eigen::Vector3f vertex_list[12]; 00106 if (leaf_node[0] < iso_level_) cubeindex |= 1; 00107 if (leaf_node[1] < iso_level_) cubeindex |= 2; 00108 if (leaf_node[2] < iso_level_) cubeindex |= 4; 00109 if (leaf_node[3] < iso_level_) cubeindex |= 8; 00110 if (leaf_node[4] < iso_level_) cubeindex |= 16; 00111 if (leaf_node[5] < iso_level_) cubeindex |= 32; 00112 if (leaf_node[6] < iso_level_) cubeindex |= 64; 00113 if (leaf_node[7] < iso_level_) cubeindex |= 128; 00114 00115 // Cube is entirely in/out of the surface 00116 if (edgeTable[cubeindex] == 0) 00117 return; 00118 00119 //Eigen::Vector4f index_3df (index_3d[0], index_3d[1], index_3d[2], 0.0f); 00120 Eigen::Vector3f center;// TODO coeff wise product = min_p_ + Eigen::Vector4f (1.0f/res_x_, 1.0f/res_y_, 1.0f/res_z_) * index_3df * (max_p_ - min_p_); 00121 center[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (index_3d[0]) / float (res_x_); 00122 center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (index_3d[1]) / float (res_y_); 00123 center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (index_3d[2]) / float (res_z_); 00124 00125 std::vector<Eigen::Vector3f> p; 00126 p.resize (8); 00127 for (int i = 0; i < 8; ++i) 00128 { 00129 Eigen::Vector3f point = center; 00130 if(i & 0x4) 00131 point[1] = static_cast<float> (center[1] + (max_p_[1] - min_p_[1]) / float (res_y_)); 00132 00133 if(i & 0x2) 00134 point[2] = static_cast<float> (center[2] + (max_p_[2] - min_p_[2]) / float (res_z_)); 00135 00136 if((i & 0x1) ^ ((i >> 1) & 0x1)) 00137 point[0] = static_cast<float> (center[0] + (max_p_[0] - min_p_[0]) / float (res_x_)); 00138 00139 p[i] = point; 00140 } 00141 00142 00143 // Find the vertices where the surface intersects the cube 00144 if (edgeTable[cubeindex] & 1) 00145 interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]); 00146 if (edgeTable[cubeindex] & 2) 00147 interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]); 00148 if (edgeTable[cubeindex] & 4) 00149 interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]); 00150 if (edgeTable[cubeindex] & 8) 00151 interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]); 00152 if (edgeTable[cubeindex] & 16) 00153 interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]); 00154 if (edgeTable[cubeindex] & 32) 00155 interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]); 00156 if (edgeTable[cubeindex] & 64) 00157 interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]); 00158 if (edgeTable[cubeindex] & 128) 00159 interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]); 00160 if (edgeTable[cubeindex] & 256) 00161 interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]); 00162 if (edgeTable[cubeindex] & 512) 00163 interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]); 00164 if (edgeTable[cubeindex] & 1024) 00165 interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]); 00166 if (edgeTable[cubeindex] & 2048) 00167 interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]); 00168 00169 // Create the triangle 00170 for (int i = 0; triTable[cubeindex][i] != -1; i+=3) 00171 { 00172 PointNT 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 00188 00189 ////////////////////////////////////////////////////////////////////////////////////////////// 00190 template <typename PointNT> void 00191 pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf, 00192 Eigen::Vector3i &index3d) 00193 { 00194 leaf = std::vector<float> (8, 0.0f); 00195 00196 leaf[0] = getGridValue (index3d); 00197 leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0)); 00198 leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1)); 00199 leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1)); 00200 leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0)); 00201 leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0)); 00202 leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1)); 00203 leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1)); 00204 } 00205 00206 00207 ////////////////////////////////////////////////////////////////////////////////////////////// 00208 template <typename PointNT> float 00209 pcl::MarchingCubes<PointNT>::getGridValue (Eigen::Vector3i pos) 00210 { 00211 /// TODO what to return? 00212 if (pos[0] < 0 || pos[0] >= res_x_) 00213 return -1.0f; 00214 if (pos[1] < 0 || pos[1] >= res_y_) 00215 return -1.0f; 00216 if (pos[2] < 0 || pos[2] >= res_z_) 00217 return -1.0f; 00218 00219 return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]]; 00220 } 00221 00222 00223 ////////////////////////////////////////////////////////////////////////////////////////////// 00224 template <typename PointNT> void 00225 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output) 00226 { 00227 if (!(iso_level_ >= 0 && iso_level_ < 1)) 00228 { 00229 PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_); 00230 output.cloud.width = output.cloud.height = 0; 00231 output.cloud.data.clear (); 00232 output.polygons.clear (); 00233 return; 00234 } 00235 00236 // Create grid 00237 grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f); 00238 00239 // Populate tree 00240 tree_->setInputCloud (input_); 00241 00242 getBoundingBox (); 00243 00244 // Transform the point cloud into a voxel grid 00245 // This needs to be implemented in a child class 00246 voxelizeData (); 00247 00248 00249 00250 // Run the actual marching cubes algorithm, store it into a point cloud, 00251 // and copy the point cloud + connectivity into output 00252 pcl::PointCloud<PointNT> cloud; 00253 00254 for (int x = 1; x < res_x_-1; ++x) 00255 for (int y = 1; y < res_y_-1; ++y) 00256 for (int z = 1; z < res_z_-1; ++z) 00257 { 00258 Eigen::Vector3i index_3d (x, y, z); 00259 std::vector<float> leaf_node; 00260 getNeighborList1D (leaf_node, index_3d); 00261 createSurface (leaf_node, index_3d, cloud); 00262 } 00263 pcl::toPCLPointCloud2 (cloud, output.cloud); 00264 00265 output.polygons.resize (cloud.size () / 3); 00266 for (size_t i = 0; i < output.polygons.size (); ++i) 00267 { 00268 pcl::Vertices v; 00269 v.vertices.resize (3); 00270 for (int j = 0; j < 3; ++j) 00271 v.vertices[j] = static_cast<int> (i) * 3 + j; 00272 output.polygons[i] = v; 00273 } 00274 } 00275 00276 00277 ////////////////////////////////////////////////////////////////////////////////////////////// 00278 template <typename PointNT> void 00279 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points, 00280 std::vector<pcl::Vertices> &polygons) 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 points.width = points.height = 0; 00286 points.points.clear (); 00287 polygons.clear (); 00288 return; 00289 } 00290 00291 // Create grid 00292 grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f); 00293 00294 // Populate tree 00295 tree_->setInputCloud (input_); 00296 00297 getBoundingBox (); 00298 00299 // Transform the point cloud into a voxel grid 00300 // This needs to be implemented in a child class 00301 voxelizeData (); 00302 00303 // Run the actual marching cubes algorithm, store it into a point cloud, 00304 // and copy the point cloud + connectivity into output 00305 points.clear (); 00306 for (int x = 1; x < res_x_-1; ++x) 00307 for (int y = 1; y < res_y_-1; ++y) 00308 for (int z = 1; z < res_z_-1; ++z) 00309 { 00310 Eigen::Vector3i index_3d (x, y, z); 00311 std::vector<float> leaf_node; 00312 getNeighborList1D (leaf_node, index_3d); 00313 createSurface (leaf_node, index_3d, points); 00314 } 00315 00316 polygons.resize (points.size () / 3); 00317 for (size_t i = 0; i < polygons.size (); ++i) 00318 { 00319 pcl::Vertices v; 00320 v.vertices.resize (3); 00321 for (int j = 0; j < 3; ++j) 00322 v.vertices[j] = static_cast<int> (i) * 3 + j; 00323 polygons[i] = v; 00324 } 00325 } 00326 00327 00328 00329 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>; 00330 00331 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_ 00332