organized_fast_mesh.h
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_H_
00042 #define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
00043 
00044 #include <pcl/common/angles.h>
00045 #include <pcl/surface/reconstruction.h>
00046 
00047 namespace pcl
00048 {
00049 
00064   template <typename PointInT>
00065   class OrganizedFastMesh : public pcl::MeshConstruction<PointInT>
00066   {
00067     public:
00068       typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
00069       typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
00070 
00071       using pcl::MeshConstruction<PointInT>::input_;
00072       using pcl::MeshConstruction<PointInT>::check_tree_;
00073 
00074       typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
00075 
00076       typedef std::vector<pcl::Vertices> Polygons;
00077 
00078       enum TriangulationType
00079       {
00080         TRIANGLE_RIGHT_CUT,     // _always_ "cuts" a quad from top left to bottom right
00081         TRIANGLE_LEFT_CUT,      // _always_ "cuts" a quad from top right to bottom left
00082         TRIANGLE_ADAPTIVE_CUT,  // "cuts" where possible and prefers larger differences in 'z' direction
00083         QUAD_MESH               // create a simple quad mesh
00084       };
00085 
00087       OrganizedFastMesh ()
00088       : max_edge_length_a_ (0.0f)
00089       , max_edge_length_b_ (0.0f)
00090       , max_edge_length_c_ (0.0f)
00091       , max_edge_length_set_ (false)
00092       , max_edge_length_dist_dependent_ (false)
00093       , triangle_pixel_size_rows_ (1)
00094       , triangle_pixel_size_columns_ (1)
00095       , triangulation_type_ (QUAD_MESH)
00096       , viewpoint_ (Eigen::Vector3f::Zero ())
00097       , store_shadowed_faces_ (false)
00098       , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
00099       , distance_tolerance_ (-1.0f)
00100       , distance_dependent_ (false)
00101       , use_depth_as_distance_(false)
00102       {
00103         check_tree_ = false;
00104       };
00105 
00107       virtual ~OrganizedFastMesh () {};
00108 
00116       inline void
00117       setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
00118       {
00119         max_edge_length_a_ = a;
00120         max_edge_length_b_ = b;
00121         max_edge_length_c_ = c;
00122         if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
00123           max_edge_length_set_ = true;
00124         else
00125           max_edge_length_set_ = false;
00126       };
00127 
00128       inline void
00129       unsetMaxEdgeLength ()
00130       {
00131         max_edge_length_set_  = false;
00132       }
00133 
00138       inline void
00139       setTrianglePixelSize (int triangle_size)
00140       {
00141         setTrianglePixelSizeRows (triangle_size);
00142         setTrianglePixelSizeColumns (triangle_size);
00143       }
00144 
00149       inline void
00150       setTrianglePixelSizeRows (int triangle_size)
00151       {
00152         triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
00153       }
00154 
00159       inline void
00160       setTrianglePixelSizeColumns (int triangle_size)
00161       {
00162         triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
00163       }
00164 
00169       inline void
00170       setTriangulationType (TriangulationType type)
00171       {
00172         triangulation_type_ = type;
00173       }
00174 
00178       inline void setViewpoint (const Eigen::Vector3f& viewpoint)
00179       {
00180         viewpoint_ = viewpoint;
00181       }
00182 
00184       const inline Eigen::Vector3f& getViewpoint () const
00185       {
00186         return viewpoint_;
00187       }
00188 
00192       inline void
00193       storeShadowedFaces (bool enable)
00194       {
00195         store_shadowed_faces_ = enable;
00196       }
00197 
00203       inline void
00204       setAngleTolerance(float angle_tolerance)
00205       {
00206         if (angle_tolerance > 0)
00207           cos_angle_tolerance_ = fabsf (cosf (angle_tolerance));
00208         else
00209           cos_angle_tolerance_ = -1.0f;
00210       }
00211 
00212 
00213       inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
00214       {
00215         distance_tolerance_ = distance_tolerance;
00216         if (distance_tolerance_ < 0)
00217           return;
00218 
00219         distance_dependent_ = depth_dependent;
00220         if (!distance_dependent_)
00221           distance_tolerance_ *= distance_tolerance_;
00222       }
00223 
00226       inline void useDepthAsDistance(bool enable)
00227       {
00228         use_depth_as_distance_ = enable;
00229       }
00230 
00231     protected:
00233       float max_edge_length_a_;
00235       float max_edge_length_b_;
00237       float max_edge_length_c_;
00239       bool max_edge_length_set_;
00240 
00242       bool max_edge_length_dist_dependent_;
00243 
00245       int triangle_pixel_size_rows_;
00246 
00248       int triangle_pixel_size_columns_;
00249 
00251       TriangulationType triangulation_type_;
00252 
00254       Eigen::Vector3f viewpoint_;
00255 
00257       bool store_shadowed_faces_;
00258 
00260       float cos_angle_tolerance_;
00261 
00263       float distance_tolerance_;
00264 
00266       bool distance_dependent_;
00267 
00270       bool use_depth_as_distance_;
00271 
00272 
00276       void
00277       reconstructPolygons (std::vector<pcl::Vertices>& polygons);
00278 
00282       virtual void
00283       performReconstruction (std::vector<pcl::Vertices> &polygons);
00284 
00292       void
00293       performReconstruction (pcl::PolygonMesh &output);
00294 
00302       inline void
00303       addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
00304       {
00305         assert (idx < static_cast<int> (polygons.size ()));
00306         polygons[idx].vertices.resize (3);
00307         polygons[idx].vertices[0] = a;
00308         polygons[idx].vertices[1] = b;
00309         polygons[idx].vertices[2] = c;
00310       }
00311 
00320       inline void
00321       addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
00322       {
00323         assert (idx < static_cast<int> (polygons.size ()));
00324         polygons[idx].vertices.resize (4);
00325         polygons[idx].vertices[0] = a;
00326         polygons[idx].vertices[1] = b;
00327         polygons[idx].vertices[2] = c;
00328         polygons[idx].vertices[3] = d;
00329       }
00330 
00339       inline void
00340       resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
00341                       int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
00342       {
00343         float new_value = value;
00344         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
00345         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
00346         memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
00347       }
00348 
00353       inline bool
00354       isShadowed (const PointInT& point_a, const PointInT& point_b)
00355       {
00356         bool valid = true;
00357 
00358         Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
00359         Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
00360         float distance_to_points = dir_a.norm ();
00361         float distance_between_points = dir_b.norm ();
00362 
00363         if (cos_angle_tolerance_ > 0)
00364         {
00365           float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
00366           if (cos_angle != cos_angle)
00367             cos_angle = 1.0f;
00368           bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_;
00369 
00370           bool check_distance = true;
00371           if (check_angle && (distance_tolerance_ > 0))
00372           {
00373             float dist_thresh = distance_tolerance_;
00374             if (distance_dependent_)
00375             {
00376               float d = distance_to_points;
00377               if (use_depth_as_distance_)
00378                 d = std::max(point_a.z, point_b.z);
00379               dist_thresh *= d*d;
00380               dist_thresh *= dist_thresh;  // distance_tolerance_ is already squared if distance_dependent_ is false.
00381             }
00382             check_distance = (distance_between_points > dist_thresh);
00383           }
00384           valid = !(check_angle && check_distance);
00385         }
00386 
00387         // check if max. edge length is not exceeded
00388         if (max_edge_length_set_)
00389         {
00390           float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
00391           float dist_thresh = max_edge_length_a_;
00392           if (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
00393             dist_thresh += max_edge_length_b_ * dist;
00394           if (fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
00395             dist_thresh += max_edge_length_c_ * dist * dist;
00396           valid = (distance_between_points <= dist_thresh);
00397         }
00398 
00399         return !valid;
00400       }
00401 
00407       inline bool
00408       isValidTriangle (const int& a, const int& b, const int& c)
00409       {
00410         if (!pcl::isFinite (input_->points[a])) return (false);
00411         if (!pcl::isFinite (input_->points[b])) return (false);
00412         if (!pcl::isFinite (input_->points[c])) return (false);
00413         return (true);
00414       }
00415 
00421       inline bool
00422       isShadowedTriangle (const int& a, const int& b, const int& c)
00423       {
00424         if (isShadowed (input_->points[a], input_->points[b])) return (true);
00425         if (isShadowed (input_->points[b], input_->points[c])) return (true);
00426         if (isShadowed (input_->points[c], input_->points[a])) return (true);
00427         return (false);
00428       }
00429 
00436       inline bool
00437       isValidQuad (const int& a, const int& b, const int& c, const int& d)
00438       {
00439         if (!pcl::isFinite (input_->points[a])) return (false);
00440         if (!pcl::isFinite (input_->points[b])) return (false);
00441         if (!pcl::isFinite (input_->points[c])) return (false);
00442         if (!pcl::isFinite (input_->points[d])) return (false);
00443         return (true);
00444       }
00445 
00452       inline bool
00453       isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
00454       {
00455         if (isShadowed (input_->points[a], input_->points[b])) return (true);
00456         if (isShadowed (input_->points[b], input_->points[c])) return (true);
00457         if (isShadowed (input_->points[c], input_->points[d])) return (true);
00458         if (isShadowed (input_->points[d], input_->points[a])) return (true);
00459         return (false);
00460       }
00461 
00465       void
00466       makeQuadMesh (std::vector<pcl::Vertices>& polygons);
00467 
00471       void
00472       makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
00473 
00477       void
00478       makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
00479 
00483       void
00484       makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
00485   };
00486 }
00487 
00488 #include <pcl18/surface/impl/organized_fast_mesh.hpp>
00489 
00490 #endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_H_


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