organized_fast_mesh.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Dirk Holz (University of Bonn)
00006  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of Willow Garage, Inc. nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: organized_fast_mesh.hpp 4917 2012-03-05 17:36:10Z rusu $
00038  *
00039  */
00040 
00041 #ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
00042 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
00043 
00044 #include <pcl/surface/organized_fast_mesh.h>
00045 
00047 template <typename PointInT> void
00048 pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
00049 {
00050   reconstructPolygons (output.polygons);
00051 
00052   // Get the field names
00053   int x_idx = pcl::getFieldIndex (output.cloud, "x");
00054   int y_idx = pcl::getFieldIndex (output.cloud, "y");
00055   int z_idx = pcl::getFieldIndex (output.cloud, "z");
00056   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00057     return;
00058   // correct all measurements,
00059   // (running over complete image since some rows and columns are left out
00060   // depending on triangle_pixel_size)
00061   // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
00062   for (unsigned int i = 0; i < input_->points.size (); ++i)
00063     if (!isFinite (input_->points[i]))
00064       resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
00065 }
00066 
00068 template <typename PointInT> void
00069 pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
00070 {
00071   reconstructPolygons (polygons);
00072 }
00073 
00075 template <typename PointInT> void
00076 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
00077 {
00078   if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
00079     makeRightCutMesh (polygons);
00080   else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
00081     makeLeftCutMesh (polygons);
00082   else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
00083     makeAdaptiveCutMesh (polygons);
00084   else if (triangulation_type_ == QUAD_MESH)
00085     makeQuadMesh (polygons);
00086 }
00087 
00089 template <typename PointInT> void
00090 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
00091 {
00092   int last_column = input_->width - triangle_pixel_size_;
00093   int last_row = input_->height - triangle_pixel_size_;
00094 
00095   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00096   int y_big_incr = triangle_pixel_size_ * input_->width,
00097       x_big_incr = y_big_incr + triangle_pixel_size_;
00098   // Reserve enough space
00099   polygons.resize (input_->width * input_->height);
00100 
00101   // Go over the rows first
00102   for (int y = 0; y < last_row; y += triangle_pixel_size_)
00103   {
00104     // Initialize a new row
00105     i = y * input_->width;
00106     index_right = i + triangle_pixel_size_;
00107     index_down = i + y_big_incr;
00108     index_down_right = i + x_big_incr;
00109 
00110     // Go over the columns
00111     for (int x = 0; x < last_column; x += triangle_pixel_size_,
00112                                      i += triangle_pixel_size_,
00113                                      index_right += triangle_pixel_size_,
00114                                      index_down += triangle_pixel_size_,
00115                                      index_down_right += triangle_pixel_size_)
00116     {
00117       if (isValidQuad (i, index_down, index_right, index_down_right))
00118         if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
00119           addQuad (i, index_down, index_right, index_down_right, idx++, polygons);
00120     }
00121   }
00122   polygons.resize (idx);
00123 }
00124 
00126 template <typename PointInT> void
00127 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
00128 {
00129   int last_column = input_->width - triangle_pixel_size_;
00130   int last_row = input_->height - triangle_pixel_size_;
00131 
00132   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00133   int y_big_incr = triangle_pixel_size_ * input_->width,
00134       x_big_incr = y_big_incr + triangle_pixel_size_;
00135   // Reserve enough space
00136   polygons.resize (input_->width * input_->height * 2);
00137 
00138   // Go over the rows first
00139   for (int y = 0; y < last_row; y += triangle_pixel_size_)
00140   {
00141     // Initialize a new row
00142     i = y * input_->width;
00143     index_right = i + triangle_pixel_size_;
00144     index_down = i + y_big_incr;
00145     index_down_right = i + x_big_incr;
00146 
00147     // Go over the columns
00148     for (int x = 0; x < last_column; x += triangle_pixel_size_,
00149                                      i += triangle_pixel_size_,
00150                                      index_right += triangle_pixel_size_,
00151                                      index_down += triangle_pixel_size_,
00152                                      index_down_right += triangle_pixel_size_)
00153     {
00154       if (isValidTriangle (i, index_down_right, index_right))
00155         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00156           addTriangle (i, index_down_right, index_right, idx++, polygons);
00157 
00158       if (isValidTriangle (i, index_down, index_down_right))
00159         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00160           addTriangle (i, index_down, index_down_right, idx++, polygons);
00161     }
00162   }
00163   polygons.resize (idx);
00164 }
00165 
00167 template <typename PointInT> void
00168 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
00169 {
00170   int last_column = input_->width - triangle_pixel_size_;
00171   int last_row = input_->height - triangle_pixel_size_;
00172 
00173   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00174   int y_big_incr = triangle_pixel_size_ * input_->width,
00175       x_big_incr = y_big_incr + triangle_pixel_size_;
00176   // Reserve enough space
00177   polygons.resize (input_->width * input_->height * 2);
00178 
00179   // Go over the rows first
00180   for (int y = 0; y < last_row; y += triangle_pixel_size_)
00181   {
00182     // Initialize a new row
00183     i = y * input_->width;
00184     index_right = i + triangle_pixel_size_;
00185     index_down = i + y_big_incr;
00186     index_down_right = i + x_big_incr;
00187 
00188     // Go over the columns
00189     for (int x = 0; x < last_column; x += triangle_pixel_size_,
00190                                      i += triangle_pixel_size_,
00191                                      index_right += triangle_pixel_size_,
00192                                      index_down += triangle_pixel_size_,
00193                                      index_down_right += triangle_pixel_size_)
00194     {
00195       if (isValidTriangle (i, index_down, index_right))
00196         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00197           addTriangle (i, index_down, index_right, idx++, polygons);
00198 
00199       if (isValidTriangle (index_right, index_down, index_down_right))
00200         if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00201           addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00202     }
00203   }
00204   polygons.resize (idx);
00205 }
00206 
00208 template <typename PointInT> void
00209 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
00210 {
00211   int last_column = input_->width - triangle_pixel_size_;
00212   int last_row = input_->height - triangle_pixel_size_;
00213 
00214   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00215   int y_big_incr = triangle_pixel_size_ * input_->width,
00216       x_big_incr = y_big_incr + triangle_pixel_size_;
00217   // Reserve enough space
00218   polygons.resize (input_->width * input_->height * 4);
00219 
00220   // Go over the rows first
00221   for (int y = 0; y < last_row; y += triangle_pixel_size_)
00222   {
00223     // Initialize a new row
00224     i = y * input_->width;
00225     index_right = i + triangle_pixel_size_;
00226     index_down = i + y_big_incr;
00227     index_down_right = i + x_big_incr;
00228 
00229     // Go over the columns
00230     for (int x = 0; x < last_column; x += triangle_pixel_size_,
00231                                      i += triangle_pixel_size_,
00232                                      index_right += triangle_pixel_size_,
00233                                      index_down += triangle_pixel_size_,
00234                                      index_down_right += triangle_pixel_size_)
00235     {
00236       const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
00237       const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
00238       const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
00239       const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
00240 
00241       if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
00242       {
00243         float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
00244         float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
00245         if (dist_right_cut >= dist_left_cut)
00246         {
00247           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00248             addTriangle (i, index_down_right, index_right, idx++, polygons);
00249           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00250             addTriangle (i, index_down, index_down_right, idx++, polygons);
00251         }
00252         else
00253         {
00254           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00255             addTriangle (i, index_down, index_right, idx++, polygons);
00256           if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00257             addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00258         }
00259       }
00260       else
00261       {
00262         if (right_cut_upper)
00263           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00264             addTriangle (i, index_down_right, index_right, idx++, polygons);
00265         if (right_cut_lower)
00266           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00267             addTriangle (i, index_down, index_down_right, idx++, polygons);
00268         if (left_cut_upper)
00269           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00270             addTriangle (i, index_down, index_right, idx++, polygons);
00271         if (left_cut_lower)
00272           if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00273             addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00274       }
00275     }
00276   }
00277   polygons.resize (idx);
00278 }
00279 
00280 #define PCL_INSTANTIATE_OrganizedFastMesh(T)                \
00281   template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
00282 
00283 #endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:11