mesh_processing.cpp
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) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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 #include <pcl/apps/in_hand_scanner/mesh_processing.h>
00042 
00043 #include <cmath>
00044 
00045 #include <pcl/apps/in_hand_scanner/utils.h>
00046 
00048 
00049 pcl::ihs::MeshProcessing::MeshProcessing ()
00050 {
00051 }
00052 
00054 
00055 void
00056 pcl::ihs::MeshProcessing::processBoundary (Mesh& mesh, const std::vector <HalfEdgeIndices>& boundary_collection, const bool cleanup) const
00057 {
00058   typedef std::vector <Mesh::HalfEdgeIndices> BoundaryCollection;
00059 
00060   Mesh::VertexIndex vi_a, vi_b, vi_c, vi_d;
00061   Eigen::Vector3f ab, bc, ac, n_adb, n_plane; // Edges and normals
00062   Mesh::FaceIndex opposite_face;
00063 
00064   for (BoundaryCollection::const_iterator it_bc=boundary_collection.begin (); it_bc!=boundary_collection.end (); ++it_bc)
00065   {
00066     const Mesh::HalfEdgeIndices& boundary = *it_bc;
00067     if (boundary.size () == 3)
00068     {
00069       opposite_face = mesh.getOppositeFaceIndex (boundary [0]);
00070 
00071       if (mesh.getOppositeFaceIndex (boundary [1]) == opposite_face  &&
00072           mesh.getOppositeFaceIndex (boundary [2]) == opposite_face)
00073       {
00074         // Isolated face.
00075         mesh.deleteFace (opposite_face);
00076       }
00077       else
00078       {
00079         // Close triangular hole.
00080         mesh.addFace (mesh.getTerminatingVertexIndex (boundary [0]),
00081                       mesh.getTerminatingVertexIndex (boundary [1]),
00082                       mesh.getTerminatingVertexIndex (boundary [2]));
00083       }
00084     }
00085     else // size != 3
00086     {
00087       // Add triangles where the angle between the edges is below a threshold. In the example this would leave only triangles 1-2-3 and triangles 4-5-6 (threshold = 60 degrees). Triangle 1-2-3 should not be added because vertex 2 is not convex (as vertex 5).
00088 
00089       // Example: The boundary is on the top. Vertex 7 is connected to vertex 0.
00090       //           2                   //
00091       //          / \                  //
00092       // ... 0 - 1   3 - 4   6 - 7 ... //
00093       //                  \ /          //
00094       //                   5           //
00095 
00096       for (int i=0; i<boundary.size (); ++i)
00097       {
00098         // The vertices on the boundary
00099         vi_a = mesh.getOriginatingVertexIndex (boundary [i]);
00100         vi_b = mesh.getTerminatingVertexIndex (boundary [i]);
00101         vi_c = mesh.getTerminatingVertexIndex (boundary [(i+1) % boundary.size ()]);
00102 
00103         const Eigen::Vector4f& v_a = mesh.getVertexDataCloud () [vi_a.get ()].getVector4fMap ();
00104         const Eigen::Vector4f& v_b = mesh.getVertexDataCloud () [vi_b.get ()].getVector4fMap ();
00105         const Eigen::Vector4f& v_c = mesh.getVertexDataCloud () [vi_c.get ()].getVector4fMap ();
00106 
00107         ab = (v_b - v_a).head <3> ();
00108         bc = (v_c - v_b).head <3> ();
00109         ac = (v_c - v_a).head <3> ();
00110 
00111         const float angle = std::acos (pcl::ihs::clamp (-ab.dot (bc) / ab.norm () / bc.norm (), -1.f, 1.f));
00112 
00113         if (angle < 1.047197551196598f) // 60 * pi / 180
00114         {
00115           // Third vertex belonging to the face of edge ab
00116           vi_d = mesh.getTerminatingVertexIndex (
00117                    mesh.getNextHalfEdgeIndex (
00118                      mesh.getOppositeHalfEdgeIndex (boundary [i])));
00119           const Eigen::Vector4f& v_d = mesh.getVertexDataCloud () [vi_d.get ()].getVector4fMap ();
00120 
00121           // n_adb is the normal of triangle a-d-b.
00122           // The plane goes through edge a-b and is perpendicular to the plane through a-d-b.
00123           n_adb   = (v_d - v_a).head <3> ().cross (ab)/*.normalized ()*/;
00124           n_plane = n_adb.cross (ab/*.nomalized ()*/);
00125 
00126           if (n_plane.dot (ac) > 0.f)
00127           {
00128             mesh.addFace (vi_a, vi_b, vi_c);
00129           }
00130         }
00131       }
00132     }
00133   }
00134 
00135   if (cleanup)
00136     mesh.cleanUp ();
00137 }
00138 


ros_in_hand_scanner
Author(s):
autogenerated on Thu Jun 6 2019 20:39:38