shape_reconstruction: extended_marching

Go to the documentation of this 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 
00025 
00026 #ifndef __VCG_EXTENDED_MARCHING_CUBES
00027 #define __VCG_EXTENDED_MARCHING_CUBES
00028 
00029 #include <float.h>
00030 #include <vcg/simplex/face/topology.h>
00031 #include <vcg/complex/algorithms/update/normal.h>
00032 #include <vcg/complex/algorithms/update/topology.h>
00033 #include <vcg/complex/algorithms/clean.h>
00034 #include "emc_lookup_table.h"
00035 #include <eigenlib/Eigen/SVD>
00036 
00037 namespace vcg
00038 {
00039     namespace tri
00040     {
00041         // Doxygen documentation
00044 
00045             /*
00046         * Cube description:
00047         *         3 ________ 2           _____2__
00048         *         /|       /|         / |       /|
00049         *       /  |     /  |      11/  3   10/  |
00050         *   7 /_______ /    |      /__6_|__ /    |1
00051         *    |     |  |6    |     |     |     |
00052         *    |    0|__|_____|1    |     |__|_0|__|
00053         *    |    /   |    /      7   8/   5    /
00054         *    |  /     |  /        |  /     |  /9
00055         *    |/_______|/          |/___4___|/
00056         *   4          5
00057         */
00058 
00060 
00067         template<class TRIMESH_TYPE, class WALKER_TYPE>
00068         class ExtendedMarchingCubes
00069         {
00070         public:
00071 #if defined(__GNUC__)
00072             typedef unsigned int                                size_t;
00073 #else
00074 #ifdef                  _WIN64
00075             typedef unsigned __int64    size_t;
00076 #else
00077             typedef _W64 unsigned int   size_t;
00078 #endif
00079 #endif
00080             typedef typename vcg::tri::Allocator< TRIMESH_TYPE > AllocatorType;
00081             typedef typename TRIMESH_TYPE::ScalarType                   ScalarType;
00082             typedef typename TRIMESH_TYPE::VertexType                   VertexType;
00083             typedef typename TRIMESH_TYPE::VertexPointer        VertexPointer;
00084             typedef typename TRIMESH_TYPE::VertexIterator       VertexIterator;
00085             typedef typename TRIMESH_TYPE::FaceType                             FaceType;
00086             typedef typename TRIMESH_TYPE::FacePointer          FacePointer;
00087             typedef typename TRIMESH_TYPE::FaceIterator         FaceIterator;
00088             typedef typename TRIMESH_TYPE::CoordType                    CoordType;
00089             typedef typename TRIMESH_TYPE::CoordType*                   CoordPointer;
00090 
00091       struct LightEdge
00092             {
00093         LightEdge(size_t _face, size_t _edge):face(_face), edge(_edge) { }
00094                 size_t face, edge;
00095       };
00096 
00104             ExtendedMarchingCubes(TRIMESH_TYPE &mesh, WALKER_TYPE &walker, ScalarType angle=30)
00105             {
00106                 _mesh           = &mesh;
00107                 _walker = &walker;
00108                 _featureAngle = vcg::math::ToRad(angle);
00109                 _initialized = _finalized = false;
00110             };
00111 
00116             void Initialize()
00117             {
00118                 assert(!_initialized && !_finalized);
00119                 _featureFlag    = VertexType::NewBitFlag();
00120                 _initialized    = true;
00121             };
00122 
00127             void Finalize()
00128             {
00129                 assert(_initialized && !_finalized);
00130         FlipEdges();
00131 
00132                 VertexIterator v_iter = _mesh->vert.begin();
00133                 VertexIterator v_end    = _mesh->vert.end();
00134                 for ( ; v_iter!=v_end; v_iter++)
00135                     v_iter->ClearUserBit( _featureFlag );
00136                 VertexType::DeleteBitFlag( _featureFlag );
00137                 _featureFlag = 0;
00138                 _mesh = NULL;
00139                 _walker = NULL;
00140                 _finalized = true;
00141             };
00142 
00149         void ProcessCell(const vcg::Point3i &min, const vcg::Point3i &max)
00150             {
00151                 assert(_initialized && !_finalized);
00152                 assert(min[0]<max[0] && min[1]<max[1] && min[2]<max[2]);
00153                 _corners[0].X()=min.X();                _corners[0].Y()=min.Y();                _corners[0].Z()=min.Z();
00154                 _corners[1].X()=max.X();                _corners[1].Y()=min.Y();                _corners[1].Z()=min.Z();
00155                 _corners[2].X()=max.X();                _corners[2].Y()=max.Y();                _corners[2].Z()=min.Z();
00156                 _corners[3].X()=min.X();                _corners[3].Y()=max.Y();                _corners[3].Z()=min.Z();
00157                 _corners[4].X()=min.X();                _corners[4].Y()=min.Y();                _corners[4].Z()=max.Z();
00158                 _corners[5].X()=max.X();                _corners[5].Y()=min.Y();                _corners[5].Z()=max.Z();
00159                 _corners[6].X()=max.X();                _corners[6].Y()=max.Y();                _corners[6].Z()=max.Z();
00160                 _corners[7].X()=min.X();                _corners[7].Y()=max.Y();                _corners[7].Z()=max.Z();
00161 
00162                 unsigned char cubetype = 0;
00163                 if ((_field[0] = _walker->V(_corners[0].X(), _corners[0].Y(), _corners[0].Z())) >= 0) cubetype+=  1;
00164                 if ((_field[1] = _walker->V(_corners[1].X(), _corners[1].Y(), _corners[1].Z())) >= 0) cubetype+=  2;
00165                 if ((_field[2] = _walker->V(_corners[2].X(), _corners[2].Y(), _corners[2].Z())) >= 0) cubetype+=  4;
00166                 if ((_field[3] = _walker->V(_corners[3].X(), _corners[3].Y(), _corners[3].Z())) >= 0) cubetype+=  8;
00167                 if ((_field[4] = _walker->V(_corners[4].X(), _corners[4].Y(), _corners[4].Z())) >= 0) cubetype+= 16;
00168                 if ((_field[5] = _walker->V(_corners[5].X(), _corners[5].Y(), _corners[5].Z())) >= 0) cubetype+= 32;
00169                 if ((_field[6] = _walker->V(_corners[6].X(), _corners[6].Y(), _corners[6].Z())) >= 0) cubetype+= 64;
00170                 if ((_field[7] = _walker->V(_corners[7].X(), _corners[7].Y(), _corners[7].Z())) >= 0) cubetype+=128;
00171 
00172                 if (cubetype==0 || cubetype==255)
00173                     return;
00174 
00175                 size_t vertices_idx[12];
00176                 memset(vertices_idx, -1, 12*sizeof(size_t));
00177                 int code = EMCLookUpTable::EdgeTable(cubetype);
00178                 VertexPointer vp = NULL;
00179                 if (   1&code ) { _walker->GetXIntercept(_corners[0], _corners[1], vp); vertices_idx[ 0] = vp - &_mesh->vert[0]; }
00180                 if (   2&code ) { _walker->GetYIntercept(_corners[1], _corners[2], vp); vertices_idx[ 1] = vp - &_mesh->vert[0]; }
00181                 if (   4&code ) { _walker->GetXIntercept(_corners[3], _corners[2], vp); vertices_idx[ 2] = vp - &_mesh->vert[0]; }
00182                 if (   8&code ) { _walker->GetYIntercept(_corners[0], _corners[3], vp); vertices_idx[ 3] = vp - &_mesh->vert[0]; }
00183                 if (  16&code ) { _walker->GetXIntercept(_corners[4], _corners[5], vp); vertices_idx[ 4] = vp - &_mesh->vert[0]; }
00184                 if (  32&code ) { _walker->GetYIntercept(_corners[5], _corners[6], vp); vertices_idx[ 5] = vp - &_mesh->vert[0]; }
00185                 if (  64&code ) { _walker->GetXIntercept(_corners[7], _corners[6], vp); vertices_idx[ 6] = vp - &_mesh->vert[0]; }
00186                 if ( 128&code ) { _walker->GetYIntercept(_corners[4], _corners[7], vp); vertices_idx[ 7] = vp - &_mesh->vert[0]; }
00187                 if ( 256&code ) { _walker->GetZIntercept(_corners[0], _corners[4], vp); vertices_idx[ 8] = vp - &_mesh->vert[0]; }
00188                 if ( 512&code ) { _walker->GetZIntercept(_corners[1], _corners[5], vp); vertices_idx[ 9] = vp - &_mesh->vert[0]; }
00189                 if (1024&code ) { _walker->GetZIntercept(_corners[2], _corners[6], vp); vertices_idx[10] = vp - &_mesh->vert[0]; }
00190                 if (2048&code ) { _walker->GetZIntercept(_corners[3], _corners[7], vp); vertices_idx[11] = vp - &_mesh->vert[0]; }
00191 
00192                 int m, n, vertices_num;
00193                 int components = EMCLookUpTable::TriTable(cubetype, 1)[0]; //unsigned int components =  triTable[cubetype][1][0];
00194                 int     *indices   = &EMCLookUpTable::TriTable(cubetype, 1)[components+1]; //int                                         *indices   = &EMCLookUpTable::TriTable(cubetype, 1, components+1);
00195 
00196                 std::vector< size_t > vertices_list;
00197                 for (m=1; m<=components; m++)
00198                 {
00199                     // current sheet contains vertices_num vertices
00200                     vertices_num = EMCLookUpTable::TriTable(cubetype, 1)[m]; //vertices_num = triTable[cubetype][1][m];
00201 
00202                     // collect vertices
00203                     vertices_list.clear();
00204                     for (n=0; n<vertices_num; ++n)
00205                         vertices_list.push_back( vertices_idx[ indices[n] ] );
00206 
00207           VertexPointer feature = FindFeature( vertices_list );
00208                     if (feature != NULL) // i.e. is a valid vertex
00209                     {
00210                         // feature -> create triangle fan around feature vertex
00211                         size_t feature_idx = feature - &_mesh->vert[0];
00212                         size_t face_idx                 = _mesh->face.size();
00213                         vertices_list.push_back( vertices_list[0] );
00214                         AllocatorType::AddFaces(*_mesh, (int) vertices_num);
00215                         for (int j=0; j<vertices_num; ++j, face_idx++)
00216                         {
00217                             _mesh->face[face_idx].V(0) = &_mesh->vert[ vertices_list[j  ] ];
00218                             _mesh->face[face_idx].V(1) = &_mesh->vert[ vertices_list[j+1] ];
00219                             _mesh->face[face_idx].V(2) = &_mesh->vert[ feature_idx                       ];
00220                         }
00221                     }
00222                     else
00223                     {
00224                         // no feature -> old marching cubes triangle table
00225                         for (int j=0; EMCLookUpTable::PolyTable(vertices_num, j) != -1; j+=3) //for (int j=0; polyTable[vertices_num][j] != -1; j+=3)
00226                         {
00227                             size_t face_idx                     = _mesh->face.size();
00228                             AllocatorType::AddFaces(*_mesh, 1);
00229                             //_mesh->face[ face_idx].V(0) = &_mesh->vert[ vertices_idx[ indices[ polyTable[vertices_num][j  ] ] ] ];
00230                             //_mesh->face[ face_idx].V(1) = &_mesh->vert[ vertices_idx[ indices[ polyTable[vertices_num][j+1] ] ] ];
00231                             //_mesh->face[ face_idx].V(2) = &_mesh->vert[ vertices_idx[ indices[ polyTable[vertices_num][j+2] ] ] ];
00232                             _mesh->face[ face_idx].V(0) = &_mesh->vert[ vertices_idx[ indices[ EMCLookUpTable::PolyTable(vertices_num, j  ) ] ] ];
00233                             _mesh->face[ face_idx].V(1) = &_mesh->vert[ vertices_idx[ indices[ EMCLookUpTable::PolyTable(vertices_num, j+1) ] ] ];
00234                             _mesh->face[ face_idx].V(2) = &_mesh->vert[ vertices_idx[ indices[ EMCLookUpTable::PolyTable(vertices_num, j+2) ] ] ];
00235                         }
00236                     }
00237                     indices += vertices_num;
00238 
00239                 }
00240             }; // end of ApplyEMC
00241 
00242         private:
00245             WALKER_TYPE         *_walker;
00248             TRIMESH_TYPE        *_mesh;
00251             bool        _initialized;;
00254             bool        _finalized;
00258             ScalarType _featureAngle;
00262             int                         _featureFlag;
00266             vcg::Point3i _corners[8];
00270             ScalarType   _field[8];
00271 
00272 
00278             VertexPointer FindFeature(const std::vector<size_t> &vertices_idx)
00279             {
00280                 unsigned int i, j, rank;
00281                 size_t vertices_num = (size_t) vertices_idx.size();
00282 
00283                 CoordType *points               = new CoordType[ vertices_num ];
00284                 CoordType *normals      = new CoordType[ vertices_num ];
00285         Box3<ScalarType> bb;
00286                 for (i=0; i<vertices_num; i++)
00287                 {
00288                     points[i]           = _mesh->vert[ vertices_idx[i] ].P();
00289                     normals[i].Import(_mesh->vert[ vertices_idx[i] ].N());
00290          bb.Add(points[i]);
00291                 }
00292 
00293                 // move barycenter of points into (0, 0, 0)
00294                 CoordType center((ScalarType) 0.0, (ScalarType) 0.0, (ScalarType) 0.0);
00295                 for (i=0; i<vertices_num; ++i)
00296                     center += points[i];
00297                 center /= (ScalarType) vertices_num;
00298                 for (i=0; i<vertices_num; ++i)
00299                     points[i] -= center;
00300 
00301                 // normal angle criterion
00302                 double c, minC, maxC;
00303                 CoordType axis;
00304                 for (minC=1.0, i=0; i<vertices_num-1; ++i)
00305                 {
00306                     for (j=i+1; j<vertices_num; ++j)
00307                     {
00308                         c = normals[i]*normals[j];
00309                         if (c < minC)
00310                         {
00311                             minC = c;
00312                             axis = normals[i] ^ normals[j];
00313                         }
00314                     }
00315                 } //end for (minC=1.0, i=0; i<vertNumber; ++i)
00316 
00317                 if (minC > cos(_featureAngle))
00318                     return NULL;  // invalid vertex
00319 
00320                 // ok, we have a feature: is it edge or corner, i.e. rank 2 or 3 ?
00321                 axis.Normalize();
00322                 for (minC=1.0, maxC=-1.0, i=0; i<vertices_num; ++i)
00323                 {
00324                     c = axis * normals[i];
00325                     if (c < minC)  minC = c;
00326                     if (c > maxC)  maxC = c;
00327                 }
00328                 c = std::max< double >(fabs(minC), fabs(maxC));
00329                 c = sqrt(1.0-c*c);
00330                 rank = (c > cos(_featureAngle) ? 2 : 3);
00331 
00332                 // setup linear system (find intersection of tangent planes)
00333                 //--vcg::ndim::Matrix<double>  A((unsigned int) vertices_num, 3);
00334                 Eigen::MatrixXd A(vertices_num,3);
00335 
00336                 //--double *b = new double[ vertices_num ];
00337                 Eigen::MatrixXd b(vertices_num,1);
00338 
00339                 for (i=0; i<vertices_num; ++i)
00340                 {
00341                   //--A[i][0] =  normals[i][0];
00342                   //--A[i][1] =  normals[i][1];
00343                   //--A[i][2] =  normals[i][2];
00344                   //--b[i]              =  (points[i] * normals[i]);
00345                   A(i,0) =  normals[i][0];
00346                   A(i,1) =  normals[i][1];
00347                   A(i,2) =  normals[i][2];
00348                   b(i)          =  (points[i] * normals[i]);
00349                 }
00350 
00351                 // SVD of matrix A
00352                 Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
00353                 Eigen::MatrixXd sol(3,1);
00354                 sol=svd.solve(b);
00355 
00356                 // vcg::ndim::Matrix<double>  V(3, 3);
00357                 // double *w = new double[vertices_num];
00358                 // vcg::SingularValueDecomposition< typename vcg::ndim::Matrix<double> > (A, w, V, LeaveUnsorted, 100);
00359 
00360                 // rank == 2 -> suppress smallest singular value
00361 //                              if (rank == 2)
00362 //                              {
00363 //                                      double        smin   = DBL_MAX; // the max value, as defined in <float.h>
00364 //                                      unsigned int  sminid = 0;
00365 //                unsigned int  srank  = std::min< unsigned int >(vertices_num, 3u);
00366 
00367 //                                      for (i=0; i<srank; ++i)
00368 //                                      {
00369 //                                              if (w[i] < smin)
00370 //                                              {
00371 //                                                      smin   = w[i];
00372 //                                                      sminid = i;
00373 //                                              }
00374 //                                      }
00375 //                                      w[sminid] = 0.0;
00376 //                              }
00377 //
00378 //                              // SVD backsubstitution -> least squares, least norm solution x
00379 //                              double *x = new double[3];
00380 //                              vcg::SingularValueBacksubstitution< vcg::ndim::Matrix<double> >(A, w, V, x, b);
00381 
00382                 // transform x to world coords
00383                 //--CoordType point((ScalarType) x[0], (ScalarType) x[1], (ScalarType) x[2]);
00384                 CoordType point((ScalarType) sol(0), (ScalarType) sol(1), (ScalarType) sol(2));
00385                 point += center;
00386 
00387         // Safety check if the feature point found by svd is
00388         // out of the bbox of the vertices perhaps it is better to put it back in the center...
00389         if(!bb.IsIn(point)) point = center;
00390 
00391                 // insert the feature-point
00392                 VertexPointer mean_point = &*AllocatorType::AddVertices( *_mesh, 1);
00393                 mean_point->SetUserBit(_featureFlag);
00394                 mean_point->P() = point;
00395                 mean_point->N().SetZero();
00396 //                              delete []x;
00397                 delete []points;
00398                 delete []normals;
00399                 return mean_point;
00400             } // end of FindFeature
00401 
00407             void FlipEdges()
00408             {
00409               std::vector< LightEdge > edges;
00410               for (FaceIterator fi = _mesh->face.begin(); fi!=_mesh->face.end(); fi++)
00411               {
00412                 size_t i = tri::Index(*_mesh,*fi);
00413                 if (fi->V(1) > fi->V(0)) edges.push_back( LightEdge(i,0) );
00414                 if (fi->V(2) > fi->V(1)) edges.push_back( LightEdge(i,1) );
00415                 if (fi->V(0) > fi->V(2)) edges.push_back( LightEdge(i,2) );
00416               }
00417               vcg::tri::UpdateTopology< TRIMESH_TYPE >::FaceFace( *_mesh );
00418 
00419               // Select all the triangles that has a vertex shared with a non manifold edge.
00420               int nonManifEdge = tri::Clean< TRIMESH_TYPE >::CountNonManifoldEdgeFF(*_mesh,true);
00421               if(nonManifEdge >0)
00422                 tri::UpdateSelection< TRIMESH_TYPE >::FaceFromVertexLoose(*_mesh);
00423               //qDebug("Got %i non manif edges",nonManifEdge);
00424 
00425               typename std::vector< LightEdge >::iterator e_it  = edges.begin();
00426               typename std::vector< LightEdge >::iterator e_end = edges.end();
00427 
00428               FacePointer g, f;
00429               int                       w, z;
00430               for( ; e_it!=e_end; e_it++)
00431               {
00432                 f = &_mesh->face[e_it->face];
00433                 z = (int) e_it->edge;
00434 
00435                 //          v2------v1    swap the diagonal only if v2 and v3 are feature and v0 and v1 are not.
00436                 //          |     /  |
00437                 //          |  /     |
00438                 //          v0------v3
00439                 if (!(f->IsS()) && vcg::face::CheckFlipEdge< FaceType >(*f, z))
00440                 {
00441                   VertexPointer v0, v1, v2, v3;
00442                   v0 = f->V(z);
00443                   v1 = f->V1(z);
00444                   v2 = f->V2(z);
00445                   g = f->FFp(z);
00446                   w = f->FFi(z);
00447                   v3 = g->V2(w);
00448                   bool b0, b1, b2, b3;
00449                   b0 = !v0->IsUserBit(_featureFlag) ;
00450                   b1 = !v1->IsUserBit(_featureFlag) ;
00451                   b2 =  v2->IsUserBit(_featureFlag) ;
00452                   b3 =  v3->IsUserBit(_featureFlag) ;
00453                   if( b0 && b1 && b2 && b3)
00454                     vcg::face::FlipEdge< FaceType >(*f, z);
00455 
00456                 } // end if (vcg::face::CheckFlipEdge< _Face >(*f, z))
00457               } // end for( ; e_it!=e_end; e_it++)
00458 
00459             } //end of FlipEdges
00460 
00461 
00462         }; // end of class ExtendedMarchingCubes
00463         //      /*! @} */
00464         // end of Doxygen documentation
00465 
00466     } // end of namespace tri
00467 }; // end of namespace vcg
00468 
00469 #endif // __VCG_EXTENDED_MARCHING_CUBES