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$
00038  *
00039  */
00040 
00041 #ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
00042 #define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
00043 
00044 #include <pcl18/surface/organized_fast_mesh.h>
00045 #include <rtabmap/utilite/ULogger.h>
00046 
00048 template <typename PointInT> void
00049 pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
00050 {
00051   reconstructPolygons (output.polygons);
00052 
00053   // Get the field names
00054   int x_idx = pcl::getFieldIndex (output.cloud, "x");
00055   int y_idx = pcl::getFieldIndex (output.cloud, "y");
00056   int z_idx = pcl::getFieldIndex (output.cloud, "z");
00057   if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00058     return;
00059   // correct all measurements,
00060   // (running over complete image since some rows and columns are left out
00061   // depending on triangle_pixel_size)
00062   // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
00063   for (unsigned int i = 0; i < input_->points.size (); ++i)
00064     if (!isFinite (input_->points[i]))
00065       resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
00066 }
00067 
00069 template <typename PointInT> void
00070 pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
00071 {
00072   reconstructPolygons (polygons);
00073 }
00074 
00076 template <typename PointInT> void
00077 pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
00078 {
00079   if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
00080     makeRightCutMesh (polygons);
00081   else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
00082     makeLeftCutMesh (polygons);
00083   else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
00084     makeAdaptiveCutMesh (polygons);
00085   else if (triangulation_type_ == QUAD_MESH)
00086     makeQuadMesh (polygons);
00087 }
00088 
00090 template <typename PointInT> void
00091 pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
00092 {
00093   int last_column = input_->width - triangle_pixel_size_columns_;
00094   int last_row = input_->height - triangle_pixel_size_rows_;
00095 
00096   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00097   int y_big_incr = triangle_pixel_size_rows_ * input_->width,
00098       x_big_incr = y_big_incr + triangle_pixel_size_columns_;
00099   // Reserve enough space
00100   polygons.resize (input_->width * input_->height);
00101 
00102   // Go over the rows first
00103   for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
00104   {
00105     // Initialize a new row
00106     i = y * input_->width;
00107     index_right = i + triangle_pixel_size_columns_;
00108     index_down = i + y_big_incr;
00109     index_down_right = i + x_big_incr;
00110 
00111     // Go over the columns
00112     for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
00113                                      i += triangle_pixel_size_columns_,
00114                                      index_right += triangle_pixel_size_columns_,
00115                                      index_down += triangle_pixel_size_columns_,
00116                                      index_down_right += triangle_pixel_size_columns_)
00117     {
00118       if (isValidQuad (i, index_right, index_down_right, index_down))
00119         if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
00120           addQuad (i, index_right, index_down_right, index_down, idx++, polygons);
00121     }
00122   }
00123   polygons.resize (idx);
00124 }
00125 
00127 template <typename PointInT> void
00128 pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
00129 {
00130   int last_column = input_->width - triangle_pixel_size_columns_;
00131   int last_row = input_->height - triangle_pixel_size_rows_;
00132 
00133   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00134   int y_big_incr = triangle_pixel_size_rows_ * input_->width,
00135       x_big_incr = y_big_incr + triangle_pixel_size_columns_;
00136   // Reserve enough space
00137   polygons.resize (input_->width * input_->height * 2);
00138 
00139   // Go over the rows first
00140   for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
00141   {
00142     // Initialize a new row
00143     i = y * input_->width;
00144     index_right = i + triangle_pixel_size_columns_;
00145     index_down = i + y_big_incr;
00146     index_down_right = i + x_big_incr;
00147 
00148     // Go over the columns
00149     for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
00150                                      i += triangle_pixel_size_columns_,
00151                                      index_right += triangle_pixel_size_columns_,
00152                                      index_down += triangle_pixel_size_columns_,
00153                                      index_down_right += triangle_pixel_size_columns_)
00154     {
00155       if (isValidTriangle (i, index_down_right, index_right))
00156         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00157           addTriangle (i, index_down_right, index_right, idx++, polygons);
00158 
00159       if (isValidTriangle (i, index_down, index_down_right))
00160         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00161           addTriangle (i, index_down, index_down_right, idx++, polygons);
00162     }
00163   }
00164   polygons.resize (idx);
00165 }
00166 
00168 template <typename PointInT> void
00169 pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
00170 {
00171   int last_column = input_->width - triangle_pixel_size_columns_;
00172   int last_row = input_->height - triangle_pixel_size_rows_;
00173 
00174   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00175   int y_big_incr = triangle_pixel_size_rows_ * input_->width,
00176       x_big_incr = y_big_incr + triangle_pixel_size_columns_;
00177   // Reserve enough space
00178   polygons.resize (input_->width * input_->height * 2);
00179 
00180   // Go over the rows first
00181   for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
00182   {
00183     // Initialize a new row
00184     i = y * input_->width;
00185     index_right = i + triangle_pixel_size_columns_;
00186     index_down = i + y_big_incr;
00187     index_down_right = i + x_big_incr;
00188 
00189     // Go over the columns
00190     for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
00191                                      i += triangle_pixel_size_columns_,
00192                                      index_right += triangle_pixel_size_columns_,
00193                                      index_down += triangle_pixel_size_columns_,
00194                                      index_down_right += triangle_pixel_size_columns_)
00195     {
00196       if (isValidTriangle (i, index_down, index_right))
00197         if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00198           addTriangle (i, index_down, index_right, idx++, polygons);
00199 
00200       if (isValidTriangle (index_right, index_down, index_down_right))
00201         if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00202           addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00203     }
00204   }
00205   polygons.resize (idx);
00206 }
00207 
00209 template <typename PointInT> void
00210 pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
00211 {
00212   int last_column = input_->width - triangle_pixel_size_columns_;
00213   int last_row = input_->height - triangle_pixel_size_rows_;
00214 
00215   int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
00216   int y_big_incr = triangle_pixel_size_rows_ * input_->width,
00217       x_big_incr = y_big_incr + triangle_pixel_size_columns_;
00218   // Reserve enough space
00219   polygons.resize (input_->width * input_->height * 2);
00220 
00221   // Go over the rows first
00222   for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
00223   {
00224     // Initialize a new row
00225     i = y * input_->width;
00226     index_right = i + triangle_pixel_size_columns_;
00227     index_down = i + y_big_incr;
00228     index_down_right = i + x_big_incr;
00229 
00230     // Go over the columns
00231     for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
00232                                      i += triangle_pixel_size_columns_,
00233                                      index_right += triangle_pixel_size_columns_,
00234                                      index_down += triangle_pixel_size_columns_,
00235                                      index_down_right += triangle_pixel_size_columns_)
00236     {
00237       const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
00238       const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
00239       const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
00240       const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
00241 
00242       if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
00243       {
00244         float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
00245         float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
00246         if (dist_right_cut >= dist_left_cut)
00247         {
00248           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00249             addTriangle (i, index_down_right, index_right, idx++, polygons);
00250           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00251             addTriangle (i, index_down, index_down_right, idx++, polygons);
00252         }
00253         else
00254         {
00255           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00256             addTriangle (i, index_down, index_right, idx++, polygons);
00257           if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00258             addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00259         }
00260       }
00261       else
00262       {
00263         if (right_cut_upper)
00264           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
00265             addTriangle (i, index_down_right, index_right, idx++, polygons);
00266         if (right_cut_lower)
00267           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
00268             addTriangle (i, index_down, index_down_right, idx++, polygons);
00269         if (left_cut_upper)
00270           if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
00271             addTriangle (i, index_down, index_right, idx++, polygons);
00272         if (left_cut_lower)
00273           if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
00274             addTriangle (index_right, index_down, index_down_right, idx++, polygons);
00275       }
00276     }
00277   }
00278   polygons.resize (idx);
00279 }
00280 
00281 #define PCL_INSTANTIATE_OrganizedFastMesh(T)                \
00282   template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
00283 
00284 #endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:21