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 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_SURFACE_IMPL_POISSON_H_ 00041 #define PCL_SURFACE_IMPL_POISSON_H_ 00042 00043 #include <pcl/surface/poisson.h> 00044 #include <pcl/common/common.h> 00045 #include <pcl/common/vector_average.h> 00046 #include <pcl/Vertices.h> 00047 00048 #include <pcl/surface/3rdparty/poisson4/octree_poisson.h> 00049 #include <pcl/surface/3rdparty/poisson4/sparse_matrix.h> 00050 #include <pcl/surface/3rdparty/poisson4/function_data.h> 00051 #include <pcl/surface/3rdparty/poisson4/ppolynomial.h> 00052 #include <pcl/surface/3rdparty/poisson4/multi_grid_octree_data.h> 00053 #include <pcl/surface/3rdparty/poisson4/geometry.h> 00054 00055 #define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12 00056 00057 #include <stdarg.h> 00058 #include <string> 00059 00060 using namespace pcl; 00061 00062 ////////////////////////////////////////////////////////////////////////////////////////////// 00063 template <typename PointNT> 00064 pcl::Poisson<PointNT>::Poisson () 00065 : depth_ (8) 00066 , min_depth_ (5) 00067 , point_weight_ (4) 00068 , scale_ (1.1f) 00069 , solver_divide_ (8) 00070 , iso_divide_ (8) 00071 , samples_per_node_ (1.0) 00072 , confidence_ (false) 00073 , output_polygons_ (false) 00074 , no_reset_samples_ (false) 00075 , no_clip_tree_ (false) 00076 , manifold_ (true) 00077 , refine_ (3) 00078 , kernel_depth_ (8) 00079 , degree_ (2) 00080 , non_adaptive_weights_ (false) 00081 , show_residual_ (false) 00082 , min_iterations_ (8) 00083 , solver_accuracy_ (1e-3f) 00084 { 00085 } 00086 00087 ////////////////////////////////////////////////////////////////////////////////////////////// 00088 template <typename PointNT> 00089 pcl::Poisson<PointNT>::~Poisson () 00090 { 00091 } 00092 00093 ////////////////////////////////////////////////////////////////////////////////////////////// 00094 template <typename PointNT> template <int Degree> void 00095 pcl::Poisson<PointNT>::execute (poisson::CoredVectorMeshData &mesh, 00096 poisson::Point3D<float> ¢er, 00097 float &scale) 00098 { 00099 pcl::poisson::Real iso_value = 0; 00100 poisson::TreeNodeData::UseIndex = 1; 00101 poisson::Octree<Degree> tree; 00102 00103 /// TODO OPENMP stuff 00104 // tree.threads = Threads.value; 00105 center.coords[0] = center.coords[1] = center.coords[2] = 0; 00106 00107 00108 if (solver_divide_ < min_depth_) 00109 { 00110 PCL_WARN ("[pcl::Poisson] solver_divide_ must be at least as large as min_depth_: %d >= %d\n", solver_divide_, min_depth_); 00111 solver_divide_ = min_depth_; 00112 } 00113 if (iso_divide_< min_depth_) 00114 { 00115 PCL_WARN ("[pcl::Poisson] iso_divide_ must be at least as large as min_depth_: %d >= %d\n", iso_divide_, min_depth_); 00116 iso_divide_ = min_depth_; 00117 } 00118 00119 pcl::poisson::TreeOctNode::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE); 00120 00121 kernel_depth_ = depth_ - 2; 00122 00123 tree.setBSplineData (depth_, pcl::poisson::Real (1.0 / (1 << depth_)), true); 00124 00125 tree.maxMemoryUsage = 0; 00126 00127 00128 int point_count = tree.setTree (input_, depth_, min_depth_, kernel_depth_, samples_per_node_, 00129 scale_, center, scale, confidence_, point_weight_, !non_adaptive_weights_); 00130 00131 tree.ClipTree (); 00132 tree.finalize (); 00133 tree.RefineBoundary (iso_divide_); 00134 00135 PCL_DEBUG ("Input Points: %d\n" , point_count ); 00136 PCL_DEBUG ("Leaves/Nodes: %d/%d\n" , tree.tree.leaves() , tree.tree.nodes() ); 00137 00138 tree.maxMemoryUsage = 0; 00139 tree.SetLaplacianConstraints (); 00140 00141 tree.maxMemoryUsage = 0; 00142 tree.LaplacianMatrixIteration (solver_divide_, show_residual_, min_iterations_, solver_accuracy_); 00143 00144 iso_value = tree.GetIsoValue (); 00145 00146 tree.GetMCIsoTriangles (iso_value, iso_divide_, &mesh, 0, 1, manifold_, output_polygons_); 00147 } 00148 00149 00150 ////////////////////////////////////////////////////////////////////////////////////////////// 00151 template <typename PointNT> void 00152 pcl::Poisson<PointNT>::performReconstruction (PolygonMesh &output) 00153 { 00154 poisson::CoredVectorMeshData mesh; 00155 poisson::Point3D<float> center; 00156 float scale = 1.0f; 00157 00158 switch (degree_) 00159 { 00160 case 1: 00161 { 00162 execute<1> (mesh, center, scale); 00163 break; 00164 } 00165 case 2: 00166 { 00167 execute<2> (mesh, center, scale); 00168 break; 00169 } 00170 case 3: 00171 { 00172 execute<3> (mesh, center, scale); 00173 break; 00174 } 00175 case 4: 00176 { 00177 execute<4> (mesh, center, scale); 00178 break; 00179 } 00180 case 5: 00181 { 00182 execute<5> (mesh, center, scale); 00183 break; 00184 } 00185 default: 00186 { 00187 PCL_ERROR (stderr, "Degree %d not supported\n", degree_); 00188 } 00189 } 00190 00191 // Write output PolygonMesh 00192 pcl::PointCloud<pcl::PointXYZ> cloud; 00193 cloud.points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ())); 00194 poisson::Point3D<float> p; 00195 for (int i = 0; i < int (mesh.inCorePoints.size ()); i++) 00196 { 00197 p = mesh.inCorePoints[i]; 00198 cloud.points[i].x = p.coords[0]*scale+center.coords[0]; 00199 cloud.points[i].y = p.coords[1]*scale+center.coords[1]; 00200 cloud.points[i].z = p.coords[2]*scale+center.coords[2]; 00201 } 00202 for (int i = int (mesh.inCorePoints.size ()); i < int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()); i++) 00203 { 00204 mesh.nextOutOfCorePoint (p); 00205 cloud.points[i].x = p.coords[0]*scale+center.coords[0]; 00206 cloud.points[i].y = p.coords[1]*scale+center.coords[1]; 00207 cloud.points[i].z = p.coords[2]*scale+center.coords[2]; 00208 } 00209 pcl::toPCLPointCloud2 (cloud, output.cloud); 00210 output.polygons.resize (mesh.polygonCount ()); 00211 00212 // Write faces 00213 std::vector<poisson::CoredVertexIndex> polygon; 00214 for (int p_i = 0; p_i < mesh.polygonCount (); p_i++) 00215 { 00216 pcl::Vertices v; 00217 mesh.nextPolygon (polygon); 00218 v.vertices.resize (polygon.size ()); 00219 00220 for (int i = 0; i < static_cast<int> (polygon.size ()); ++i) 00221 if (polygon[i].inCore ) 00222 v.vertices[i] = polygon[i].idx; 00223 else 00224 v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ()); 00225 00226 output.polygons[p_i] = v; 00227 } 00228 } 00229 00230 ////////////////////////////////////////////////////////////////////////////////////////////// 00231 template <typename PointNT> void 00232 pcl::Poisson<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points, 00233 std::vector<pcl::Vertices> &polygons) 00234 { 00235 poisson::CoredVectorMeshData mesh; 00236 poisson::Point3D<float> center; 00237 float scale = 1.0f; 00238 00239 switch (degree_) 00240 { 00241 case 1: 00242 { 00243 execute<1> (mesh, center, scale); 00244 break; 00245 } 00246 case 2: 00247 { 00248 execute<2> (mesh, center, scale); 00249 break; 00250 } 00251 case 3: 00252 { 00253 execute<3> (mesh, center, scale); 00254 break; 00255 } 00256 case 4: 00257 { 00258 execute<4> (mesh, center, scale); 00259 break; 00260 } 00261 case 5: 00262 { 00263 execute<5> (mesh, center, scale); 00264 break; 00265 } 00266 default: 00267 { 00268 PCL_ERROR (stderr, "Degree %d not supported\n", degree_); 00269 } 00270 } 00271 00272 // Write output PolygonMesh 00273 // Write vertices 00274 points.points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ())); 00275 poisson::Point3D<float> p; 00276 for (int i = 0; i < int(mesh.inCorePoints.size ()); i++) 00277 { 00278 p = mesh.inCorePoints[i]; 00279 points.points[i].x = p.coords[0]*scale+center.coords[0]; 00280 points.points[i].y = p.coords[1]*scale+center.coords[1]; 00281 points.points[i].z = p.coords[2]*scale+center.coords[2]; 00282 } 00283 for (int i = int(mesh.inCorePoints.size()); i < int (mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++) 00284 { 00285 mesh.nextOutOfCorePoint (p); 00286 points.points[i].x = p.coords[0]*scale+center.coords[0]; 00287 points.points[i].y = p.coords[1]*scale+center.coords[1]; 00288 points.points[i].z = p.coords[2]*scale+center.coords[2]; 00289 } 00290 00291 polygons.resize (mesh.polygonCount ()); 00292 00293 // Write faces 00294 std::vector<poisson::CoredVertexIndex> polygon; 00295 for (int p_i = 0; p_i < mesh.polygonCount (); p_i++) 00296 { 00297 pcl::Vertices v; 00298 mesh.nextPolygon (polygon); 00299 v.vertices.resize (polygon.size ()); 00300 00301 for (int i = 0; i < static_cast<int> (polygon.size ()); ++i) 00302 if (polygon[i].inCore ) 00303 v.vertices[i] = polygon[i].idx; 00304 else 00305 v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ()); 00306 00307 polygons[p_i] = v; 00308 } 00309 } 00310 00311 00312 #define PCL_INSTANTIATE_Poisson(T) template class PCL_EXPORTS pcl::Poisson<T>; 00313 00314 #endif // PCL_SURFACE_IMPL_POISSON_H_ 00315