vcglib: normal_extrapolation.h Source File

00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2004                                                \/)\/    *
00006 * Visual Computing Lab                                            /\/|      *
00007 * ISTI - Italian National Research Council                           |      *
00008 *                                                                    \      *
00009 * All rights reserved.                                                      *
00010 *                                                                           *
00011 * This program is free software; you can redistribute it and/or modify      *
00012 * it under the terms of the GNU General Public License as published by      *
00013 * the Free Software Foundation; either version 2 of the License, or         *
00014 * (at your option) any later version.                                       *
00015 *                                                                           *
00016 * This program is distributed in the hope that it will be useful,           *
00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020 * for more details.                                                         *
00021 *                                                                           *
00022 ****************************************************************************/
00023 
00024 #ifndef VCG_SPACE_NORMAL_EXTRAPOLATION_H
00025 #define VCG_SPACE_NORMAL_EXTRAPOLATION_H
00026 
00027 #include <vcg/math/matrix33.h>
00028 #include <vcg/math/linear.h>
00029 #include <vcg/math/lin_algebra.h>
00030 #include <vcg/math/disjoint_set.h>
00031 #include <vcg/space/box3.h>
00032 #include <vcg/space/point3.h>
00033 #include <vcg/space/index/octree.h>
00034 #include <wrap/callback.h>
00035 
00036 #include <vector>
00037 #include <queue>
00038 #include <algorithm>
00039 #include <limits>
00040 #include <stdlib.h>
00041 
00042 namespace vcg
00043 {
00046         template < class VERTEX_CONTAINER >
00047         class NormalExtrapolation
00048         {
00049         public:
00050                 typedef typename VERTEX_CONTAINER::value_type    VertexType;
00051                 typedef                                  VertexType                                                                      * VertexPointer;
00052                 typedef typename VERTEX_CONTAINER::iterator              VertexIterator;
00053                 typedef typename VertexType::CoordType                           CoordType;
00054                 typedef typename VertexType::NormalType                          NormalType;
00055                 typedef typename VertexType::ScalarType                          ScalarType;
00056                 typedef typename vcg::Box3< ScalarType >                         BoundingBoxType;
00057                 typedef typename vcg::Matrix33<ScalarType>       MatrixType;
00058 
00059                 enum NormalOrientation {IsCorrect=0, MustBeFlipped=1};
00060 
00061         private:
00062                                 /*************************************************
00063                         *               Inner class definitions
00064                         **************************************************/
00065                         // Dummy class: no object marker is needed
00066                         class DummyObjectMarker {};
00067 
00068                         // Object functor: compute the distance between a vertex and a point
00069                         struct VertPointDistanceFunctor
00070                         {
00071                                 inline bool operator()(const VertexType &v, const CoordType &p, ScalarType &d, CoordType &q) const
00072                                 {
00073                                         ScalarType distance = vcg::Distance(p, v.P());
00074                                         if (distance>d)
00075                                                 return false;
00076 
00077                                         d = distance;
00078                                         q = v.P();
00079                                         return true;
00080                                 }
00081                         };
00082                         // Plane structure: identify a plain as a <center, normal> pair
00083                         struct Plane
00084                         {
00085                                 Plane() { center.SetZero(); normal.SetZero();};
00086 
00087                                 // Object functor: return the bounding-box enclosing a given plane
00088                                 inline void GetBBox(BoundingBoxType     &bb) { bb.Set(center); };
00089 
00090                                 CoordType               center;
00091                                 NormalType      normal;
00092                                 int                                     index;
00093                         };
00094 
00095                         // Object functor: compute the distance between a point and the plane
00096                         struct PlanePointDistanceFunctor
00097                         {
00098                                 inline bool operator()(const Plane &plane, const CoordType &p, ScalarType &d, CoordType &q) const
00099                                 {
00100                                         ScalarType distance = vcg::Distance(p, plane.center);
00101                                         if (distance>d)
00102                                                 return false;
00103 
00104                                         d = distance;
00105                                         q = plane.center;
00106                                         return true;
00107                                 }
00108                         };
00109 
00110                         // Represent an edge in the Riemannian graph
00111                         struct RiemannianEdge
00112                         {
00113                                 RiemannianEdge(Plane *p=NULL, ScalarType w=std::numeric_limits<ScalarType>::max())  {plane=p; weight=w; }
00114 
00115                                 Plane                   *plane;
00116                                 ScalarType weight;
00117                         };
00118                         // Represent an edge in the MST tree
00119                         struct MSTEdge
00120                         {
00121                                 MSTEdge(Plane *p0=NULL, Plane *p1=NULL, ScalarType w=std::numeric_limits<ScalarType>::max()) {u=p0; v=p1; weight=w;};
00122                                 inline bool operator<(const MSTEdge &e) const {return weight<e.weight;}
00123 
00124                                 Plane                   *u;
00125                                 Plane                   *v;
00126                                 ScalarType weight;
00127                         };
00128                         // Represent a node in the MST tree
00129                         struct MSTNode
00130                         {
00131                                 MSTNode(MSTNode* p=NULL)  {parent=p;}
00132 
00133                                 MSTNode                                                                                 *parent;
00134                                 VertexPointer                                                            vertex;
00135                                 std::vector< MSTNode* >                  sons;
00136                         };
00137 
00138                         typedef                                         std::vector< Plane >                    PlaneContainer;
00139                         typedef typename        PlaneContainer::iterator        PlaneIterator;
00140 
00141         public:
00144                 static void ExtrapolateNormals(const VertexIterator &begin, const VertexIterator &end, const unsigned int k, const int root_index=-1, NormalOrientation orientation=IsCorrect, CallBackPos *callback=NULL)
00145                 {
00146                         BoundingBoxType dataset_bb;
00147                         for (VertexIterator iter=begin; iter!=end; iter++)
00148                                 dataset_bb.Add(iter->P());
00149                         ScalarType max_distance = dataset_bb.Diag();
00150 
00151                         // Step 1: identify the tangent planes used to locally approximate the surface
00152                         int vertex_count = int( std::distance(begin, end) );
00153                         int step                                 = int(vertex_count/100)-1;
00154                         int progress             = 0;
00155                         int percentage;
00156                         char message[128];
00157                         sprintf(message, "Locating tangent planes...");
00158                         std::vector< Plane > tangent_planes(vertex_count);
00159                         vcg::Octree< VertexType, ScalarType > octree_for_planes;
00160                         octree_for_planes.Set( begin, end );
00161 
00162                         std::vector< VertexPointer > nearest_vertices;
00163                         std::vector< CoordType           > nearest_points;
00164                         std::vector< ScalarType          > distances;
00165                         for (VertexIterator iter=begin; iter!=end; iter++)
00166                         {
00167                                 if (callback!=NULL && (++progress%step)==0 && (percentage=int((progress*100)/vertex_count))<100) (callback)(percentage, message);
00168             VertPointDistanceFunctor vpdf;
00169             DummyObjectMarker dom;
00170                                 octree_for_planes.GetKClosest(vpdf, dom, k, iter->P(), max_distance, nearest_vertices, distances, nearest_points);
00171 
00172                                 // for each vertex *iter, compute the centroid as avarege of the k-nearest vertices of *iter
00173                                 Plane *plane = &tangent_planes[ std::distance(begin, iter) ];
00174                                 for (unsigned int n=0; n<k; n++)
00175                                         plane->center += nearest_points[n];
00176                                 plane->center /= ScalarType(k);
00177 
00178                                 // then, identity the normal associated to the centroid
00179                                 MatrixType      covariance_matrix;
00180                                 CoordType diff;
00181                                 covariance_matrix.SetZero();
00182                                 for (unsigned int n=0; n<k; n++)
00183                                 {
00184                                         diff = nearest_points[n] - plane->center;
00185                                         for (int i=0; i<3; i++)
00186                                                 for (int j=0; j<3; j++)
00187                                                         covariance_matrix[i][j]+=diff[i]*diff[j];
00188                                 }
00189 
00190                                 CoordType               eigenvalues;
00191                                 MatrixType      eigenvectors;
00192                                 int required_rotations;
00193                                 vcg::Jacobi< MatrixType, CoordType >(covariance_matrix, eigenvalues, eigenvectors, required_rotations);
00194                                 vcg::SortEigenvaluesAndEigenvectors< MatrixType, CoordType >(eigenvalues, eigenvectors);
00195                                 for (int d=0; d<3; d++)
00196                                         plane->normal[d] = eigenvectors[d][2];
00197                                 plane->normal.Normalize();
00198                                 iter->N() = plane->normal;
00199                                 plane->index = int( std::distance(begin, iter) );
00200                         }
00201 
00202                         // Step 2: build the Riemannian graph, i.e. the graph where each point is connected to the k-nearest neigbours.
00203                         dataset_bb.SetNull();
00204                         PlaneIterator ePlane = tangent_planes.end();
00205                         for (PlaneIterator iPlane=tangent_planes.begin(); iPlane!=ePlane; iPlane++)
00206                                 dataset_bb.Add(iPlane->center);
00207                         max_distance = dataset_bb.Diag();
00208 
00209                         vcg::Octree< Plane, ScalarType > octree_for_plane;
00210                         octree_for_plane.Set( tangent_planes.begin(), tangent_planes.end());
00211                         std::vector< Plane* >   nearest_planes(distances.size());
00212                         std::vector< std::vector< RiemannianEdge > > riemannian_graph(vertex_count); //it's probably that we are wasting the last position...
00213                         progress = 0;
00214                         sprintf(message, "Building Riemannian graph...");
00215                         for (PlaneIterator iPlane=tangent_planes.begin(); iPlane!=ePlane; iPlane++)
00216                         {
00217                                 if (callback!=NULL && (++progress%step)==0 && (percentage=int((progress*100)/vertex_count))<100) (callback)(percentage, message);
00218 
00219                         unsigned int kk = k;
00220             PlanePointDistanceFunctor ppdf;
00221             DummyObjectMarker dom;
00222                                 octree_for_plane.GetKClosest
00223                                 (ppdf, dom, kk, iPlane->center, max_distance, nearest_planes, distances, nearest_points, true, false);
00224 
00225                                 for (unsigned int n=0; n<k; n++)
00226                                         if (iPlane->index<nearest_planes[n]->index)
00227                                                 riemannian_graph[iPlane->index].push_back(RiemannianEdge(nearest_planes[n], 1.0f - fabs(iPlane->normal .dot(nearest_planes[n]->normal))));
00228                         }
00229 
00230                         // Step 3: compute the minimum spanning tree (MST) over the Riemannian graph (we use the Kruskal algorithm)
00231                         std::vector< MSTEdge > E;
00232                         typename std::vector< std::vector< RiemannianEdge > >::iterator iRiemannian = riemannian_graph.begin();
00233                         typename std::vector< RiemannianEdge >::iterator                                                                iRiemannianEdge, eRiemannianEdge;
00234                         for (int i=0; i<vertex_count; i++, iRiemannian++)
00235                                 for (iRiemannianEdge=iRiemannian->begin(), eRiemannianEdge=iRiemannian->end(); iRiemannianEdge!=eRiemannianEdge; iRiemannianEdge++)
00236                                         E.push_back(MSTEdge(&tangent_planes[i], iRiemannianEdge->plane, iRiemannianEdge->weight));
00237 
00238                         std::sort( E.begin(), E.end() );
00239                         vcg::DisjointSet<Plane> planeset;
00240 
00241                         for (typename std::vector< Plane >::iterator iPlane=tangent_planes.begin(); iPlane!=ePlane; iPlane++)
00242                                 planeset.MakeSet( &*iPlane );
00243 
00244                         typename std::vector< MSTEdge >::iterator iMSTEdge = E.begin();
00245                         typename std::vector< MSTEdge >::iterator eMSTEdge = E.end();
00246                         std::vector< MSTEdge > unoriented_tree;
00247                         Plane *u, *v;
00248                         for ( ; iMSTEdge!=eMSTEdge; iMSTEdge++)
00249                                 if ((u=planeset.FindSet(iMSTEdge->u))!=(v=planeset.FindSet(iMSTEdge->v)))
00250                                         unoriented_tree.push_back( *iMSTEdge ), planeset.Union(u, v);
00251                         E.clear();
00252 
00253                         // compute for each plane the list of sorting edges
00254                         std::vector< std::vector< int > > incident_edges(vertex_count);
00255                         iMSTEdge = unoriented_tree.begin();
00256                         eMSTEdge = unoriented_tree.end();
00257 
00258                         progress                        = 0;
00259                         int mst_size  = int(unoriented_tree.size());
00260                         sprintf(message, "Building orieted graph...");
00261                         for ( ; iMSTEdge!=eMSTEdge; iMSTEdge++)
00262                         {
00263                                 if (callback!=NULL && (++progress%step)==0 && (percentage=int((progress*100)/mst_size))<100) (callback)(percentage, message);
00264 
00265                                 int u_index = int(iMSTEdge->u->index);
00266                                 int v_index = int(iMSTEdge->v->index);
00267                                 incident_edges[ u_index ].push_back( v_index ),
00268                                 incident_edges[ v_index ].push_back( u_index );
00269                         }
00270 
00271                         // Traverse the incident_edges vector and build the MST
00272                         VertexIterator iCurrentVertex, iSonVertex;
00273                         std::vector< MSTNode > MST(vertex_count);
00274 
00275                         typename std::vector< Plane >::iterator iFirstPlane = tangent_planes.begin();
00276                         typename std::vector< Plane >::iterator iCurrentPlane, iSonPlane;
00277 
00278                         MSTNode *mst_root;
00279                         int r_index = (root_index!=-1)? root_index : rand()*vertex_count/RAND_MAX;
00280                         mst_root = &MST[ r_index ];
00281                         mst_root->parent = mst_root; //the parent of the root is the root itself
00282 
00283                         if (orientation==MustBeFlipped)
00284                         {
00285                                 iCurrentVertex = begin;
00286                                 std::advance(iCurrentVertex, r_index);
00287                                 iCurrentVertex->N() = iCurrentVertex->N()*ScalarType(-1.0f);
00288                         }
00289 
00290                         { // just to limit the scope of the variable border
00291                                 std::queue< int > border;
00292                                 border.push(r_index);
00293                                 int maxSize             = 0;
00294                                 int     queueSize = 0;
00295                                 progress                        = 0;
00296                                 sprintf(message, "Extracting the tree...");
00297                                 while ((queueSize=int(border.size()))>0)
00298                                 {
00299                                         if (callback!=NULL && ((++progress%step)==0) && (percentage=int((maxSize-queueSize)*100/maxSize))<100) (callback)(percentage, message);
00300 
00301                                         int current_node_index = border.front();  border.pop();
00302 
00303                                         MSTNode *current_node = &MST[current_node_index];                                       //retrieve the pointer to the current MST node
00304                                         std::advance((iCurrentVertex=begin), current_node_index);       //retrieve the pointer to the correspective vertex
00305                                         current_node->vertex = &*iCurrentVertex;                                                                        //and associate it to the MST node
00306 
00307                                         std::vector< int >::iterator iSon = incident_edges[ current_node_index ].begin();
00308                                         std::vector< int >::iterator eSon = incident_edges[ current_node_index ].end();
00309                                         for ( ; iSon!=eSon; iSon++)
00310                                         {
00311                                                 MSTNode *son = &MST[ *iSon ];
00312                                                 if (son->parent==NULL) // the node hasn't been visited
00313                                                 {
00314                                                         son->parent = current_node;                                                             // Update the MST nodes
00315                                                         current_node->sons.push_back(son);
00316                                                         //std::advance((iSonVertex=begin), *iSon);//retrieve the pointer to the Vertex associated to son
00317                                                         border.push( *iSon );
00318                                                 }
00319                                                 maxSize = vcg::math::Max<int>(maxSize, queueSize);
00320                                         }
00321                                 }
00322                         }
00323 
00324                         // and finally visit the MST tree in order to propagate the normals
00325                         {
00326                                 std::queue< MSTNode* > border;
00327                                 border.push(mst_root);
00328                                 sprintf(message, "Orienting normals...");
00329                                 progress                        = 0;
00330                                 int maxSize             = 0;
00331                                 int queueSize           = 0;
00332                                 while ((queueSize=int(border.size()))>0)
00333                                 {
00334                                         MSTNode *current_node = border.front(); border.pop();
00335                                         //std::vector< MSTNode* >::iterator iMSTSon = current_node->sons.begin();
00336                                         //std::vector< MSTNode* >::iterator eMSTSon = current_node->sons.end();
00337                                         for (int s=0; s<int(current_node->sons.size()); s++)
00338                                         {
00339                                                 if (callback!=NULL && ((++progress%step)==0) && (percentage=int((maxSize-queueSize)*100/maxSize))<100) (callback)(percentage, message);
00340 
00341                                                 if (current_node->vertex->N().dot(current_node->sons[s]->vertex->N())<ScalarType(0.0f))
00342                                                         current_node->sons[s]->vertex->N() *= ScalarType(-1.0f);
00343                                                 border.push( current_node->sons[s] );
00344                                                 maxSize = vcg::math::Max<int>(maxSize, queueSize);
00345                                         }
00346                                 }
00347                         }
00348                         if (callback!=NULL) (callback)(100, message);
00349                 };
00350         };
00351 
00352 };//end of namespace vcg
00353 
00354 #endif //end of VCG_SPACE_NORMAL_EXTRAPOLATION_H