diff --git a/vcg/complex/algorithms/autoalign_4pcs.h b/vcg/complex/algorithms/autoalign_4pcs.h index 21708412..a720983b 100644 --- a/vcg/complex/algorithms/autoalign_4pcs.h +++ b/vcg/complex/algorithms/autoalign_4pcs.h @@ -64,13 +64,13 @@ public: typedef vcg::GridStaticPtr GridType; /* class for Parameters */ - struct Parameters + struct Param { - ScalarType delta; - int feetsize; // how many points in the neighborhood of each of the 4 points - ScalarType f; // overlap 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 + ScalarType delta; // Approximation Level + int feetsize; // how many points in the neighborhood of each of the 4 points + ScalarType f; // overlap estimation as a percentage + int scoreFeet; // how many of the feetsize points must match (max feetsize*4) to try an early interrupt + int scoreAln; // how good must be the alignement to end the process successfully void Default(){ delta = 0.5; @@ -81,75 +81,74 @@ public: } }; - Parameters prs; /// parameters + Param par; /// parameters - public: - void Init(MeshType &_P,MeshType &_Q); - bool Align( int L, vcg::Matrix44f & result, AACb * cb = NULL ); // main function +public: + void Init(MeshType &_P,MeshType &_Q); + bool Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb = NULL ); // main function private: - struct Couple: public std::pair - { - Couple(const int & i, const int & j, float d):std::pair(i,j),dist(d){} - Couple(float d):std::pair(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;} - }; + struct Couple: public std::pair + { + Couple(const int & i, const int & j, float d):std::pair(i,j),dist(d){} + Couple(float d):std::pair(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(); - } + /* 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_T):p(_p),T(_T){} - FourPoints p; - vcg::Matrix44 T; - ScalarType err; - int score; - int base; // debug: for which base - inline bool operator <(const CandiType & o) const {return score > o.score;} - }; + struct Candidate + { + Candidate(){} + Candidate(FourPoints _p,vcg::Matrix44_T):p(_p),T(_T){} + FourPoints p; + vcg::Matrix44 T; + ScalarType err; + int score; + int base; // debug: for which base + inline bool operator <(const Candidate & 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 mapsub; // subset of index to the vertices in Q + MeshType *P; // mesh from which the coplanar base is selected + MeshType *Q; // mesh where to find the correspondences + std::vector mapsub; // subset of index to the vertices in Q - PMesh Invr; // invariants + PMesh Invr; // invariants - std::vector< CandiType > U; - CandiType winner; - int iwinner; // winner == U[iwinner] + std::vector< Candidate > U; + Candidate winner; + int iwinner; // winner == U[iwinner] - FourPoints B; // coplanar base - std::vector bases; // used bases - ScalarType side; // side - std::vector ExtB[4]; // selection of vertices "close" to the four point - std::vector subsetP; // random selection on P - ScalarType radius; + FourPoints B; // coplanar base + std::vector bases; // used bases + ScalarType side; // side + std::vector ExtB[4]; // selection of vertices "close" to the four point + std::vector subsetP; // random selection on P + ScalarType radius; - ScalarType Bangle; - std::vector R1/*,R2*/; - ScalarType r1,r2; + ScalarType Bangle; + std::vector R1/*,R2*/; + ScalarType r1,r2; - // class for the point 'ei' - struct EPoint{ - EPoint(vcg::Point3 _p, int _i):pos(_p),pi(_i){} - vcg::Point3 pos; - int pi; //index to R[1|2] - void GetBBox(vcg::Box3 & b){b.Add(pos);} - }; + // class for the point 'ei' + struct EPoint{ + EPoint(vcg::Point3 _p, int _i):pos(_p),pi(_i){} + vcg::Point3 pos; + int pi; //index to R[1|2] + void GetBBox(vcg::Box3 & b){b.Add(pos);} + }; GridType *ugrid; // griglia vcg::GridStaticPtr ugridQ; @@ -160,9 +159,9 @@ private: void ComputeR1R2(ScalarType d1,ScalarType d2); bool IsTransfCongruent(FourPoints fp,vcg::Matrix44 & mat, float & trerr); - int EvaluateSample(CandiType & fp, CoordType & tp, CoordType & np, const float & angle); - void EvaluateAlignment(CandiType & fp); - void TestAlignment(CandiType & fp); + int EvaluateSample(Candidate & fp, CoordType & tp, CoordType & np, const float & angle); + void EvaluateAlignment(Candidate & fp); + void TestAlignment(Candidate & fp); /* debug tools */ public: @@ -197,9 +196,8 @@ public: }; template -void -FourPCS:: Init(MeshType &_P,MeshType &_Q){ - +void FourPCS:: 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()); @@ -232,8 +230,8 @@ FourPCS:: Init(MeshType &_P,MeshType &_Q){ 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 + par.delta = avD * par.delta; + side = P->bbox.Dim()[P->bbox.MaxDim()]*par.f; //rough implementation } @@ -322,7 +320,7 @@ FourPCS::SelectCoplanarBase(){ 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 ) + if( ((B[0]+(B[1]-B[0])*r1)-(B[2]+(B[3]-B[2])*r2)).Norm() > par.delta ) return false; radius =side*0.5; @@ -335,7 +333,7 @@ FourPCS::SelectCoplanarBase(){ vcg::GridStaticPtr, std::vector, std::vector, - std::vector< CoordType > >(*P,ugridP, prs.feetsize ,B[i],radius, ExtB[i],dists, samples); + std::vector< CoordType > >(*P,ugridP, par.feetsize ,B[i],radius, ExtB[i],dists, samples); } //for(int i = 0 ; i< 4; ++i) @@ -368,7 +366,7 @@ bool FourPCS::IsTransfCongruent(FourPoints fp, vcg::Matrix44 @@ -400,8 +398,7 @@ FourPCS::ComputeR1R2(ScalarType d1,ScalarType d2){ } template -bool -FourPCS::FindCongruent() { // of base B, on Q, with approximation delta +bool FourPCS::FindCongruent() { // of base B, on Q, with approximation delta bool done = false; std::vector R2inv; int n_closests = 0, n_congr = 0; @@ -415,13 +412,13 @@ FourPCS::FindCongruent() { // of base B, on Q, with approximation delt typename PMesh::VertexIterator vii; typename std::vector::iterator bR1,eR1,bR2,eR2,ite,cite; - bR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d1-prs.delta*2.0)); - eR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d1+prs.delta*2.0)); - bR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d2-prs.delta*2.0)); - eR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d2+prs.delta*2.0)); + bR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d1-par.delta*2.0)); + eR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d1+par.delta*2.0)); + bR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d2-par.delta*2.0)); + eR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(d2+par.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 + // in [bR1,eR1) there are all the pairs ad a distance d1 +- par.delta + // in [bR1,eR1) there are all the pairs ad a distance d2 +- par.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 @@ -455,18 +452,16 @@ FourPCS::FindCongruent() { // of base B, on Q, with approximation delt } 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){ + printf("R2Inv.size = %d \n",R2inv.size()); + for(uint i = 0 ; i < R2inv.size() ; ++i){ std::vector closests; - std::vector distances; - std::vector points; - // for each point in R2inv get all the points in R1 closer than prs.delta + // for each point in R2inv get all the points in R1 closer than par.delta vcg::Matrix44 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)); + bb.Add(R2inv[i].pos+vcg::Point3f(par.delta * 0.1,par.delta * 0.1 , par.delta * 0.1 )); + bb.Add(R2inv[i].pos-vcg::Point3f(par.delta * 0.1,par.delta* 0.1 , par.delta* 0.1)); vcg::tri::GetInBoxVertex > (Invr,*ugrid,bb,closests); @@ -491,13 +486,13 @@ FourPCS::FindCongruent() { // of base B, on Q, with approximation delt else{ tr++; n_congr++; - U.push_back(CandiType(p,mat)); + U.push_back(Candidate(p,mat)); EvaluateAlignment(U.back()); U.back().base = bases.size()-1; - if( U.back().score > prs.scoreFeet){ + if( U.back().score > par.scoreFeet){ TestAlignment(U.back()); - if(U.back().score > prs.scoreAln) + if(U.back().score > par.scoreAln) { done = true; break; } @@ -522,38 +517,39 @@ FourPCS::FindCongruent() { // of base B, on Q, with approximation delt template -int FourPCS::EvaluateSample(CandiType & fp, CoordType & tp, CoordType & np, const float & angle){ - VertexType* v; - ScalarType dist ; - radius = prs.delta; - tp = fp.T * tp; +int FourPCS::EvaluateSample(Candidate & fp, CoordType & tp, CoordType & np, const float & cosAngle) +{ + VertexType* v; + ScalarType dist ; + radius = par.delta; + tp = fp.T * tp; - vcg::Point4 np4; - np4 = fp.T * vcg::Point4(np[0],np[1],np[2],0.0); - np[0] = np4[0]; np[1] = np4[1]; np[2] = np4[2]; + vcg::Point4 np4; + np4 = fp.T * vcg::Point4(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 - // >(*Q,ugridQ,tp,radius, dist ); - typename MeshType::VertexType vq; - vq.P() = tp; - vq.N() = np; - v = vcg::tri::GetClosestVertexNormal< - MeshType, - vcg::GridStaticPtr - >(*Q,ugridQ,vq,radius, dist ); + v = 0; + //v = vcg::tri::GetClosestVertex< + // MeshType, + // vcg::GridStaticPtr + // >(*Q,ugridQ,tp,radius, dist ); + typename MeshType::VertexType vq; + vq.P() = tp; + vq.N() = np; + v = vcg::tri::GetClosestVertexNormal< + MeshType, + vcg::GridStaticPtr + >(*Q,ugridQ,vq,radius, dist ); - if(v!=0) - if( v->N().dot(np) -angle >0) return 1; else return -1; + if(v!=0) + if( v->N().dot(np) - cosAngle >0) return 1; else return -1; } template void -FourPCS::EvaluateAlignment(CandiType & fp){ +FourPCS::EvaluateAlignment(Candidate & fp){ int n_delta_close = 0; for(int i = 0 ; i< 4; ++i) { for(uint j = 0; j < ExtB[i].size();++j){ @@ -567,8 +563,8 @@ FourPCS::EvaluateAlignment(CandiType & fp){ template void -FourPCS::TestAlignment(CandiType & fp){ - radius = prs.delta; +FourPCS::TestAlignment(Candidate & fp){ + radius = par.delta; int n_delta_close = 0; for(uint j = 0; j < subsetP.size();++j){ CoordType np = subsetP[j]->N(); @@ -581,7 +577,7 @@ FourPCS::TestAlignment(CandiType & fp){ template bool -FourPCS:: Align( int L, vcg::Matrix44f & result, AACb * cb ){ // main loop +FourPCS:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb ){ // main loop int bestv = 0; bool found; @@ -590,7 +586,7 @@ FourPCS:: Align( int L, vcg::Matrix44f & result, AACb * cb ){ // mai if(L==0) { - L = (log(1.0-0.9999) / log(1.0-pow((float)prs.f,3.f)))+1; + L = (log(1.0-0.9999) / log(1.0-pow((float)par.f,3.f)))+1; printf("using %d bases\n",L); } @@ -605,13 +601,13 @@ FourPCS:: Align( int L, vcg::Matrix44f & result, AACb * cb ){ // mai } while(!found && (n_tries <50)); if(!found) { - prs.f*=0.98; - side = P->bbox.Dim()[P->bbox.MaxDim()]*prs.f; //rough implementation + par.f*=0.98; + side = P->bbox.Dim()[P->bbox.MaxDim()]*par.f; //rough implementation ComputeR1R2(side*1.4,side*1.4); } - } while (!found && (prs.f >0.1)); + } while (!found && (par.f >0.1)); - if(prs.f <0.1) { + if(par.f <0.1) { printf("FAILED"); return false; }