autoalign_4pcs.h

Go to the documentation of this file.

#ifndef _4PCS_
#define _4PCS_
/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *   
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/
#include <vcg/space/point3.h>
#include <vcg/space/point4.h>
#include <vcg/space/line3.h>
#include <vcg/space/plane3.h>
#include <vcg/math/base.h>
#include <vcg/math/point_matching.h>
#include <vcg/math/matrix44.h>

#include <vcg/space/index/grid_static_ptr.h>
#include <vcg/complex/trimesh/closest.h>
#include <vcg/complex/trimesh/update/bounding.h>

#include <vcg/simplex/vertex/base.h>
#include <vcg/simplex/face/base.h>
#include <vcg/complex/trimesh/base.h>
#include <vcg/complex/trimesh/stat.h>
#include <wrap/io_trimesh/export_ply.h>



// note: temporary (callback.h should be moved inside vcg)
typedef bool AACb( const int pos,const char * str );

namespace vcg{
        namespace tri{
template <class MeshType>
class FourPCS {
public:
        /* mesh only for using spatial indexing functions (to remove) */
  class PVertex;    // dummy prototype never used
  class PFace;

  class PUsedTypes: public vcg::UsedTypes < vcg::Use<PVertex>::template AsVertexType,
                                            vcg::Use<PFace  >::template AsFaceType >{};


  class PVertex : public vcg::Vertex< PUsedTypes,vcg::vertex::BitFlags,vcg::vertex::Coord3f ,vcg::vertex::Mark>{};
        /*same as for the vertes */ 
  class PFace   : public vcg::Face<   PUsedTypes> {};
        /*the mesh is a container of vertices and a container of faces */ 
        class PMesh   : public vcg::tri::TriMesh< std::vector<PVertex>, std::vector<PFace> > {};

        typedef typename MeshType::ScalarType ScalarType;
        typedef typename MeshType::CoordType CoordType;
        typedef typename MeshType::VertexIterator VertexIterator;
        typedef typename MeshType::VertexType VertexType;
        typedef vcg::Point4< vcg::Point3<ScalarType> > FourPoints;
        typedef vcg::GridStaticPtr<typename PMesh::VertexType, ScalarType > GridType; 

        /* class for Parameters */
        struct Parameters{
                ScalarType delta; 
                int feetsize;                   // how many points in the neighborhood of each of the 4 points
                ScalarType f;                   // overlapping estimation
                int scoreFeet,          // how many of the feetsize points must match (max feetsize*4) to try an early interrupt
                                scoreAln;                       // how good must be the alignement      to end the process successfully
        
                void Default(){
                        delta = 0.5;
                        feetsize = 25;
                        f = 0.5;
                        scoreFeet = 50;
                        scoreAln = 200;
                }
        };

        Parameters prs; 

        public:
        void Init(MeshType &_P,MeshType &_Q);
        bool Align( int   L, vcg::Matrix44f & result, AACb * cb = NULL );               // main function


private:        
        struct Couple: public std::pair<int,int>{
                Couple(const int & i, const int & j, float d):std::pair<int,int>(i,j),dist(d){}
                Couple(float d):std::pair<int,int>(0,0),dist(d){}
                float dist;
                const bool operator < (const   Couple & o) const {return dist < o.dist;}
                int & operator[](const int &i){return (i==0)? first : second;}
        };




        /* returns the closest point between to segments x1-x2 and x3-x4.  */
        void IntersectionLineLine(const CoordType & x1,const CoordType & x2,const CoordType & x3,const CoordType & x4, CoordType&x)
        {
                CoordType a = x2-x1, b = x4-x3, c = x3-x1;
                x = x1 + a * ((c^b).dot(a^b)) / (a^b).SquaredNorm();
        }

 


        struct CandiType{
                CandiType(){};
                CandiType(FourPoints _p,vcg::Matrix44<ScalarType>_T):p(_p),T(_T){}
                FourPoints  p; 
                vcg::Matrix44<ScalarType> T;
                ScalarType err;
                int score;
                int base; // debug: for which base
                inline bool operator <(const CandiType & o) const {return score > o.score;}
        };


        MeshType        *P,                                                                                     // mesh from which the coplanar base is selected
                                                *Q;                                                                                     // mesh where to find the correspondences
        std::vector<int> mapsub;                                        // subset of index to the vertices in Q


        PMesh     Invr;                                                                         // invariants
        
        std::vector< CandiType > U;
        CandiType winner;       
        int iwinner;                                                                                    // winner == U[iwinner]

        FourPoints B;                                                                                   // coplanar base
        std::vector<FourPoints> bases;          // used bases
        ScalarType side;                                                                        // side
        std::vector<VertexType*> ExtB[4]; // selection of vertices "close" to the four point 
        std::vector<VertexType*> subsetP; // random selection on P
        ScalarType radius;

        ScalarType Bangle;
        std::vector<Couple > R1/*,R2*/;
        ScalarType r1,r2;

        // class for the point  'ei'
        struct EPoint{
                EPoint(vcg::Point3<ScalarType> _p, int _i):pos(_p),pi(_i){}
                vcg::Point3<ScalarType> pos;
                int pi;         //index to R[1|2]
                void GetBBox(vcg::Box3<ScalarType> & b){b.Add(pos);}
        };

        GridType *ugrid; // griglia
        vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridQ; 
        vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridP; 

 //FILE * f;

//private:
        bool SelectCoplanarBase();                                                                                              // on P
        bool FindCongruent() ;                                                                                                          // of base B, on Q, with approximation delta

//private:
        void ComputeR1R2(ScalarType d1,ScalarType d2);

        bool IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float &  trerr);
        int EvaluateSample(CandiType & fp, CoordType & tp, CoordType & np, const float &  angle);
        void EvaluateAlignment(CandiType & fp);
        void TestAlignment(CandiType & fp);

        /* debug tools */
public: 
        std::vector<vcg::Matrix44f> allTr;// tutte le trasformazioni provate
        FILE * db;
        char namemesh1[255],namemesh2[255];
        int n_base;
        void InitDebug(const char * name1, const char * name2){
                db = fopen("debugPCS.txt","w");
                sprintf(&namemesh1[0],"%s",name1);
                sprintf(&namemesh2[0],"%s",name2);
                n_base = 0;
        }

        void FinishDebug(){
                fclose(db);
        }
        //void SaveALN(char * name,vcg::Matrix44f mat ){
        //      FILE * o = fopen(name,"w");
        //      fprintf(o,"2\n%s\n#\n",namemesh1);
        //      for(int  i = 0 ; i < 4; ++i)
        //              fprintf(o,"%f %f %f %f\n",mat[i][0],mat[i][1],mat[i][2],mat[i][3]);
        //      fprintf(o,"%s\n#\n",namemesh2);
        //      fprintf(o,"1.0 0.0 0.0 0.0 \n");
        //      fprintf(o,"0.0 1.0 0.0 0.0 \n");
        //      fprintf(o,"0.0 0.0 1.0 0.0 \n");
        //      fprintf(o,"0.0 0.0 0.0 1.0 \n");

        //      fclose(o);
        //}

};

template <class MeshType>
void
FourPCS<MeshType>:: Init(MeshType &_P,MeshType &_Q){ 

                P = &_P;Q=&_Q; 
                ugridQ.Set(Q->vert.begin(),Q->vert.end());
                ugridP.Set(P->vert.begin(),P->vert.end());
                int vi;
        //      float areaP = vcg::tri::Stat<MeshType>::ComputeMeshArea(*P);
        //      float areaQ = vcg::tri::Stat<MeshType>::ComputeMeshArea(*Q);

                float ratio = 800 / (float) Q->vert.size();
                for(vi = 0; vi < Q->vert.size(); ++vi)
                if(rand()/(float) RAND_MAX < ratio)
                        mapsub.push_back(vi);

                for(vi = 0; vi < P->vert.size(); ++vi)
                if(rand()/(float) RAND_MAX < ratio)
                        subsetP.push_back(&P->vert[vi]);

                // estimate neigh distance
                float avD = 0.0,dist; 
                for(int i = 0 ; i < 100; ++i){
                        int ri = rand()/(float) RAND_MAX * Q->vert.size() -1;
                        std::vector< CoordType > samples,d_samples;
                        std::vector<ScalarType > dists;
                        std::vector<VertexType* > ress;
                        vcg::tri::GetKClosestVertex<
                                        MeshType,
                                        vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType>,
                                        std::vector<VertexType*>,
                                        std::vector<ScalarType>,
                                        std::vector< CoordType > >(*Q,ugridQ,2,Q->vert[ri].cP(),Q->bbox.Diag(), ress,dists, samples);
                        assert(ress.size() == 2);
                        avD+=dists[1];
                }
                avD     /=100;                                          // average vertex-vertex distance
                avD /= sqrt(ratio);             // take into account the ratio

                prs.delta = avD * prs.delta; 
                side = P->bbox.Dim()[P->bbox.MaxDim()]*prs.f; //rough implementation

        }

template <class MeshType>
bool 
FourPCS<MeshType>::SelectCoplanarBase(){

        vcg::tri::UpdateBounding<MeshType>::Box(*P);

        // choose the inter point distance
        ScalarType dtol = side*0.1; //rough implementation

        //choose the first two points
        int i = 0,ch;
                
        // first point random
        ch = (rand()/(float)RAND_MAX)*(P->vert.size()-2);
        B[0] = P->vert[ch].P();
//printf("B[0] %d\n",ch);
        // second a point at distance d+-dtol
        for(i = 0; i < P->vert.size(); ++i){
                ScalarType dd = (P->vert[i].P() - B[0]).Norm();
                if(  ( dd < side + dtol) && (dd > side - dtol)){
                        B[1] = P->vert[i].P();
//printf("B[1] %d\n",i);
                        break;
                }
        }
        if(i ==  P->vert.size())
                return false;

        // third point at distance d from B[1] and forming a right angle
        int best = -1; ScalarType bestv=std::numeric_limits<float>::max();
        for(i = 0; i < P->vert.size(); ++i){
                int id = rand()/(float)RAND_MAX *  (P->vert.size()-1);
                ScalarType dd = (P->vert[id].P() - B[1]).Norm();
                if(  ( dd < side + dtol) && (dd > side - dtol)){
                        ScalarType angle =  fabs( ( P->vert[id].P()-B[1]).normalized().dot((B[1]-B[0]).normalized()));
                        if( angle < bestv){
                                bestv = angle;
                                best = id;
                        }                        
                }
        }
        if(best == -1)
                return false;
        B[2] = P->vert[best].P();
//printf("B[2] %d\n",best);

        CoordType n = ((B[0]-B[1]).normalized() ^ (B[2]-B[1]).normalized()).normalized();
        CoordType B4 = B[1] +  (B[0]-B[1]) + (B[2]-B[1]);
        VertexType * v =0; 
        ScalarType radius = dtol*4.0;

                std::vector<typename MeshType::VertexType*> closests;
                std::vector<ScalarType> distances;
                std::vector<CoordType> points;

                 vcg::tri::GetInSphereVertex<
                                        MeshType,
                                        vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >,
                                        std::vector<typename MeshType::VertexType*>,
                                        std::vector<ScalarType>,
                                        std::vector<CoordType>
                                >(*P,ugridP,B4,radius,closests,distances,points);

                if(closests.empty())
                        return false;
         best = -1;  bestv=std::numeric_limits<float>::max();
                for(i = 0; i <closests.size(); ++i){
                 ScalarType angle = fabs((closests[i]->P() - B[1]).normalized().dot(n));
                        if( angle < bestv){
                                bestv = angle;
                                best = i;
                        }                        
                }
                B[3] =  closests[best]->P();

//printf("B[3] %d\n", (typename MeshType::VertexType*)closests[best] - &(*P->vert.begin()));

                // compute r1 and r2
                CoordType x;
                std::swap(B[1],B[2]);
                IntersectionLineLine(B[0],B[1],B[2],B[3],x);

                r1 = (x - B[0]).dot(B[1]-B[0]) / (B[1]-B[0]).SquaredNorm();
                r2 = (x - B[2]).dot(B[3]-B[2]) / (B[3]-B[2]).SquaredNorm();

                if( ((B[0]+(B[1]-B[0])*r1)-(B[2]+(B[3]-B[2])*r2)).Norm() > prs.delta )
                        return false;

                radius  =side*0.5;
                std::vector< CoordType > samples,d_samples;
                std::vector<ScalarType > dists;

                for(int i  = 0 ; i< 4; ++i){
                        vcg::tri::GetKClosestVertex<
                                MeshType,
                                vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >,
                                std::vector<VertexType*>,
                                std::vector<ScalarType>,
                                std::vector< CoordType > >(*P,ugridP, prs.feetsize ,B[i],radius, ExtB[i],dists, samples);
                }

        //for(int i  = 0 ; i< 4; ++i) 
 //             printf("%d ",ExtB[i].size());
        //      printf("\n");
return true;

}


template <class MeshType>
bool
FourPCS<MeshType>::IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float &  trerr){
                 
                std::vector<vcg::Point3<ScalarType> > fix;
                std::vector<vcg::Point3<ScalarType> > mov;
                for(int i = 0 ; i < 4; ++i) mov.push_back(B[i]);
                for(int i = 0 ; i < 4; ++i) fix.push_back(fp[i]);

                vcg::Point3<ScalarType> n,p;
                n = (( B[1]-B[0]).normalized() ^ ( B[2]- B[0]).normalized())*( B[1]- B[0]).Norm();
                p =  B[0] + n;
                mov.push_back(p);
                n = (( fp[1]-fp[0]).normalized() ^ (fp[2]- fp[0]).normalized())*( fp[1]- fp[0]).Norm();
                p =  fp[0] + n;
                fix.push_back(p);

                vcg::PointMatching<ScalarType>::ComputeRigidMatchMatrix(mat,fix,mov);
                
                ScalarType err = 0.0;
                for(int i = 0; i < 4; ++i) err+= (mat * mov[i] - fix[i]).SquaredNorm();
                
                trerr = vcg::math::Sqrt(err);
                return  err  < prs.delta* prs.delta*4.0;
        }

template <class MeshType>
void 
FourPCS<MeshType>::ComputeR1R2(ScalarType d1,ScalarType d2){
        int vi,vj;
        R1.clear();
        //R2.clear();
        int start = clock();
        for(vi = 0; vi  < mapsub.size(); ++vi) for(vj = vi; vj < mapsub.size(); ++vj){
                        ScalarType d = ((Q->vert[mapsub[vi]]).P()-(Q->vert[mapsub[vj]]).P()).Norm();
                        if( (d < d1+ side*0.5) && (d > d1-side*0.5))
                        {
                                R1.push_back(Couple(mapsub[vi],mapsub[vj],d ));
                                R1.push_back(Couple(mapsub[vj],mapsub[vi],d));
                        }
        }
        //for( vi  = 0;  vi   < mapsub.size(); ++ vi ) for( vj  =  vi ;  vj  < mapsub.size(); ++ vj ){
        //              ScalarType d = ((Q->vert[mapsub[vi]]).P()-(Q->vert[mapsub[vj]]).P()).Norm();
        //              if( (d < d2+side*0.5) && (d > d2-side*0.5)) 
        //              {
        //                      R2.push_back(Couple(mapsub[vi],mapsub[vj],d));
        //                      R2.push_back(Couple(mapsub[vj],mapsub[vi],d));
        //              }
        //}     

        std::sort(R1.begin(),R1.end());
//      std::sort(R2.begin(),R2.end());
}

template <class MeshType>
bool 
FourPCS<MeshType>::FindCongruent() { // of base B, on Q, with approximation delta
        bool done = false;
        std::vector<EPoint> R2inv;
        int n_closests = 0, n_congr = 0;
        int ac =0 ,acf = 0,tr = 0,trf =0;
        ScalarType d1,d2;
        d1 = (B[1]-B[0]).Norm();
        d2 = (B[3]-B[2]).Norm();

        int start = clock();
        //int vi,vj;

        typename PMesh::VertexIterator vii;
        typename std::vector<Couple>::iterator bR1,eR1,bR2,eR2,ite,cite;
        bR1 = std::lower_bound<typename std::vector<Couple>::iterator,Couple>(R1.begin(),R1.end(),Couple(d1-prs.delta*2.0));
        eR1 = std::lower_bound<typename std::vector<Couple>::iterator,Couple>(R1.begin(),R1.end(),Couple(d1+prs.delta*2.0));
        bR2 = std::lower_bound<typename std::vector<Couple>::iterator,Couple>(R1.begin(),R1.end(),Couple(d2-prs.delta*2.0));
        eR2 = std::lower_bound<typename std::vector<Couple>::iterator,Couple>(R1.begin(),R1.end(),Couple(d2+prs.delta*2.0));
        
        // in  [bR1,eR1) there are all the pairs ad a distance d1 +- prs.delta
        // in  [bR1,eR1) there are all the pairs ad a distance d2 +- prs.delta

        if(bR1 == R1.end()) return false;// if there are no such pairs return
        if(bR2 == R1.end()) return false; // if there are no such pairs return

        // put [bR1,eR1) in a mesh to have the search operator for free (lazy me)
        Invr.Clear();
        int i = &(*bR1)-&(*R1.begin());
        for(ite = bR1; ite != eR1;++ite){
                vii = vcg::tri::Allocator<PMesh>::AddVertices(Invr,1);
                (*vii).P() = Q->vert[R1[i][0]].P() + (Q->vert[R1[i][1]].P()-Q->vert[R1[i][0]].P()) * r1;
                ++i;
        }
        if(Invr.vert.empty() ) return false;

        // index remaps a vertex of Invr to its corresponding point in R1
        typename PMesh::template PerVertexAttributeHandle<int> id = vcg::tri::Allocator<PMesh>::template AddPerVertexAttribute<int>(Invr,std::string("index"));
        i = &(*bR1)-&(*R1.begin());
        for(vii = Invr.vert.begin(); vii != Invr.vert.end();++vii,++i)  id[vii] = i;

        vcg::tri::UpdateBounding<PMesh>::Box(Invr);
        //      printf("Invr size %d\n",Invr.vn);

        ugrid = new GridType();
        ugrid->Set(Invr.vert.begin(),Invr.vert.end());

        i = &(*bR2)-&(*R1.begin());
        // R2inv contains all the points generated by the couples in R2 (with the reference to remap into R2)
        for(ite = bR2; ite != eR2;++ite){
                R2inv.push_back( EPoint( Q->vert[R1[i][0]].P() + (Q->vert[R1[i][1]].P()-Q->vert[R1[i][0]].P()) * r2,i));
                ++i;
        }

        n_closests = 0; n_congr = 0; ac =0 ; acf = 0; tr = 0; trf = 0;
        //      fprintf(db,"R2Inv.size  = %d \n",R2inv.size());
   for(uint i = 0 ; i < R2inv.size() ; ++i){
                
                std::vector<typename PMesh::VertexType*> closests;
                std::vector<ScalarType> distances;
                std::vector<CoordType> points;

                // for each point in R2inv get all the points in R1 closer than prs.delta
                vcg::Matrix44<ScalarType> mat;
                vcg::Box3f bb;
                bb.Add(R2inv[i].pos+vcg::Point3f(prs.delta * 0.1,prs.delta * 0.1 , prs.delta * 0.1 ));
                bb.Add(R2inv[i].pos-vcg::Point3f(prs.delta * 0.1,prs.delta* 0.1  , prs.delta* 0.1));

                vcg::tri::GetInBoxVertex<PMesh,GridType,std::vector<typename PMesh::VertexType*> >
                         (Invr,*ugrid,bb,closests);
 
                 n_closests+=closests.size();
                 for(uint ip = 0; ip < closests.size(); ++ip){
                                FourPoints p;
                                p[0] = Q->vert[R1[id[closests[ip]]][0]].P();
                                p[1] = Q->vert[R1[id[closests[ip]]][1]].P();
                                p[2] = Q->vert[R1[ R2inv[i].pi][0]].P();
                                p[3] = Q->vert[R1[ R2inv[i].pi][1]].P();

                                float trerr;
                          n_base++;
                                        if(!IsTransfCongruent(p,mat,trerr)) {
                                                trf++;
                                                //char name[255];
                                                //sprintf(name,"faileTR_%d_%f.aln",n_base,trerr);
                                                //fprintf(db,"TransCongruent %s\n", name);
                                                //SaveALN(name, mat); 
                                        }
                                        else{
                                                tr++;
                                        n_congr++;
                                                U.push_back(CandiType(p,mat));
                                                EvaluateAlignment(U.back());
                                                U.back().base = bases.size()-1;

                                                if( U.back().score > prs.scoreFeet){
                                                        TestAlignment(U.back());
                                                        if(U.back().score > prs.scoreAln)
                                                                {
                                                                        done = true; break;
                                                                }
                                                        }
                                                //char name[255];
                                                //sprintf(name,"passed_score_%5d_%d.aln",U.back().score,n_base);
                                                //fprintf(db,"OK TransCongruent %s, score: %d \n", name,U.back().score);
                                                //SaveALN(name, mat); 
                                        }
                                }                                                        
         }

         delete ugrid;
         vcg::tri::Allocator<PMesh>::DeletePerVertexAttribute(Invr,id);
         printf("n_closests %5d = (An %5d ) + ( Tr %5d ) + (OK) %5d\n",n_closests,acf,trf,n_congr); 
         
         return done;
//       printf("done n_closests %d congr %d in %f s\n ",n_closests,n_congr,(clock()-start)/(float)CLOCKS_PER_SEC);
//       printf("angle:%d %d, trasf %d %d\n",ac,acf,tr,trf);
}



template <class MeshType>
int FourPCS<MeshType>::EvaluateSample(CandiType & fp, CoordType & tp, CoordType & np, const float &  angle){
                        VertexType*   v;
                ScalarType   dist ;
                radius = prs.delta;
                tp = fp.T * tp;

                                vcg::Point4<ScalarType> np4;
                                np4 = fp.T * vcg::Point4<ScalarType>(np[0],np[1],np[2],0.0);
                                np[0] = np4[0]; np[1] = np4[1];         np[2] = np4[2];
 
      v = 0;
                        //v = vcg::tri::GetClosestVertex<
                        //      MeshType,
                        //      vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >
                        //  >(*Q,ugridQ,tp,radius,  dist  );
                        typename MeshType::VertexType vq;
                        vq.P() = tp;
                        vq.N() = np;
                        v = vcg::tri::GetClosestVertexNormal<
                                MeshType,
                                vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >
                          >(*Q,ugridQ,vq,radius,  dist  );
                 
                        if(v!=0) 
                                if( v->N().dot(np) -angle >0)  return 1; else return -1;
        
}


template <class MeshType>
void
FourPCS<MeshType>::EvaluateAlignment(CandiType  & fp){
                int n_delta_close = 0;
                for(int i  = 0 ; i< 4; ++i) {
                        for(uint j = 0; j < ExtB[i].size();++j){
                                CoordType np = ExtB[i][j]->cN();;
                                CoordType tp  = ExtB[i][j]->P();
                                n_delta_close+=EvaluateSample(fp,tp,np,0.9);
                        }               
                }
                fp.score = n_delta_close;
}

template <class MeshType>
void
FourPCS<MeshType>::TestAlignment(CandiType  & fp){
                radius = prs.delta;
                int n_delta_close = 0;
                for(uint j = 0; j < subsetP.size();++j){
                                CoordType np = subsetP[j]->N();
                                CoordType tp  = subsetP[j]->P();
                                n_delta_close+=EvaluateSample(fp,tp,np,0.6);
                         }
                fp.score =  n_delta_close;
}


template <class MeshType>
bool 
FourPCS<MeshType>::	 Align(  int L, vcg::Matrix44f & result, AACb * cb ){ // main loop
        
        int bestv = 0;
        bool found;
        int n_tries = 0;
        U.clear();

        if(L==0)
        {
                L = (log(1.0-0.9999) / log(1.0-pow((float)prs.f,3.f)))+1;
                printf("using %d bases\n",L);
        }

        ComputeR1R2(side*1.4,side*1.4);

        for(int t  = 0; t  < L; ++t ){
                do{
                        n_tries = 0;
                        do{
                                n_tries++;
                                found = SelectCoplanarBase();
                                }
                                while(!found && (n_tries <50));
                                if(!found) {
                                        prs.f*=0.98;
                                        side = P->bbox.Dim()[P->bbox.MaxDim()]*prs.f; //rough implementation
                                        ComputeR1R2(side*1.4,side*1.4);
                                }
                } while (!found && (prs.f >0.1));

                if(prs.f <0.1) {
                        printf("FAILED");
                        return false;
                        }
                bases.push_back(B);
                if(cb) cb(t*100/L,"trying bases");
                if(FindCongruent()) 
                        break;
        }

        if(U.empty()) return false;

        std::sort(U.begin(),U.end());

        bestv  = -std::numeric_limits<float>::max();
        iwinner = 0;

        for(int i = 0 ; i <  U.size() ;++i)
         {
                TestAlignment(U[i]);
                if(U[i].score > bestv){
                        bestv = U[i].score;
                        iwinner = i;
                        }
        }
        
        printf("Best score: %d \n", bestv);

        winner =  U[iwinner];
        result = winner.T;

        // deallocations
        Invr.Clear();
        
        return true;
}

        } // namespace tri
} // namespace vcg
#endif

---

vcglib
Author(s): Christian Bersch
autogenerated on Fri Jan 11 09:16:32 2013