diff --git a/vcg/complex/algorithms/autoalign_4pcs.h b/vcg/complex/algorithms/autoalign_4pcs.h index 617a1f58..f09d81a3 100644 --- a/vcg/complex/algorithms/autoalign_4pcs.h +++ b/vcg/complex/algorithms/autoalign_4pcs.h @@ -64,25 +64,46 @@ public: /* class for Parameters */ struct Param { - ScalarType delta; // Approximation Level - int feetSize; // how many points in the neighborhood of each of the 4 points - ScalarType overlap; // overlap estimation as a percentage - int scoreFeet; // how many of the feetsize points must match (max feetsize*4) to try an early interrupt - int n_samples_on_Q; // number of samples on P - int seed; - ScalarType cosAngle; // max admittable angle that can be admitted between matching points in alignments (expressed as cos(ang) ) + ScalarType overlap; // overlap estimation as a percentage of overlapping points. + + int sampleNumP; // number of samples on moving mesh P (it determines the sampling radius to be used to sample Q too) + float samplingRadius; + + ScalarType deltaPerc; // Approximation Level (expressed as a percentage of the avg distance between samples) + ScalarType deltaAbs; // Approximation Level + int feetSize; // how many points in the neighborhood of each of the 4 points + int scoreFeet; // how many of the feetsize points must match (max feetsize*4) to try an early interrupt + ScalarType cosAngle; // max admittable angle that can be admitted between matching points in alignments (expressed as cos(ang) ) + int seed; // random seed used. Need for repeatability. void Default(){ - delta = 0.5; - feetSize = 25; overlap = 0.5; + sampleNumP=500; + samplingRadius=0; + + deltaPerc = 0.5; + deltaAbs = 0; + feetSize = 25; scoreFeet = 50; - n_samples_on_Q=500; seed =0; cosAngle = 0; // normals must differ more than 90 degree to be considered bad. } }; + struct Stat + { + Stat() : initTime(0),selectCoplanarBaseTime(0),findCongruentTime(0),testAlignmentTime(0) + {} + clock_t initTime; + clock_t selectCoplanarBaseTime; + clock_t findCongruentTime; + clock_t testAlignmentTime; + float init() {return 1000.0f*float(initTime)/float(CLOCKS_PER_SEC);} + float select() {return 1000.0f*float(selectCoplanarBaseTime)/float(CLOCKS_PER_SEC);} + float findCongruent() {return 1000.0f*float(findCongruentTime)/float(CLOCKS_PER_SEC);} + float testAlignment() {return 1000.0f*float(testAlignmentTime)/float(CLOCKS_PER_SEC);} + }; + class Couple { public: @@ -90,17 +111,16 @@ public: Couple(VertexPointer i, VertexPointer j, float d) : p0(i),p1(j),dist(d){} float dist; const bool operator < (const Couple & o) const {return dist < o.dist;} - VertexPointer operator[](const int &i){return (i==0)? this->p0 : this->p1;} + VertexPointer operator[](const int &i) const {return (i==0)? this->p0 : this->p1;} }; struct Candidate { - Candidate(){} - Candidate(FourPoints _p,vcg::Matrix44_T):p(_p),T(_T){} + Candidate():score(0){} + Candidate(FourPoints _p, vcg::Matrix44_T):p(_p),T(_T){} FourPoints p; vcg::Matrix44 T; int score; - int base; // debug: for which base inline bool operator <(const Candidate & o) const {return score > o.score;} }; @@ -115,9 +135,12 @@ public: Param par; /// parameters + Stat stat; - MeshType *P; // mesh from which the coplanar base is selected - MeshType *Q; // mesh where to find the correspondences + MeshType *P; // Moving Mesh (from which the coplanar base is selected) + MeshType *Q; // Fixed Mesh (mesh where to find the correspondences) + + math::MarsenneTwisterRNG rnd; std::vector subsetQ; // subset of the vertices in Q std::vector subsetP; // random selection on P @@ -125,35 +148,15 @@ public: ScalarType side; // side PMesh Invr; // invariants - math::MarsenneTwisterRNG rnd; std::vector< Candidate > U; // the int iwinner; // winner == U[iwinner] - FourPoints B; // coplanar base std::vector bases; // used bases std::vector ExtB[4]; // selection of vertices "close" to the four point - ScalarType radius; - std::vector R1; - ScalarType r1,r2; + vcg::GridStaticPtr ugridQ; + vcg::GridStaticPtr ugridP; - GridType *ugrid; // griglia - vcg::GridStaticPtr ugridQ; - vcg::GridStaticPtr ugridP; - - // the two main functions to be used - void Init(MeshType &Mov, MeshType &Fix); - bool Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb = NULL ); - -// ---- auxiliary functions - bool SelectCoplanarBase(); // on P - bool FindCongruent(); // of base B, on Q, with approximation delta - void ComputeR1(); - bool IsTransfCongruent(FourPoints fp,vcg::Matrix44 & mat, float & trerr); - int EvaluateSample(Candidate & fp, CoordType & tp, CoordType & np); - void EvaluateAlignment(Candidate & fp); - void TestAlignment(Candidate & fp); - - /* returns the closest point between to segments x1-x2 and x3-x4. */ + /* 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; @@ -161,78 +164,45 @@ public: } - /* debug tools */ -public: - std::vector 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); - } -}; - -template -void FourPCS:: Init(MeshType &_movP,MeshType &_fixQ) +void Init(MeshType &_movP,MeshType &_fixQ) { - P = &_movP; - Q = &_fixQ; - tri::UpdateBounding::Box(*P); - if(par.seed==0) rnd.initialize(time(0)); - else rnd.initialize(par.seed); - ugridQ.Set(Q->vert.begin(),Q->vert.end()); - ugridP.Set(P->vert.begin(),P->vert.end()); + clock_t t0= clock(); + P = &_movP; + Q = &_fixQ; + tri::UpdateBounding::Box(*P); + if(par.seed==0) rnd.initialize(time(0)); + else rnd.initialize(par.seed); - float radius=0; - tri::PoissonPruning(*Q,subsetQ,radius,par.n_samples_on_Q,par.seed); - tri::PoissonPruning(*P,subsetP,radius,par.n_samples_on_Q,par.seed); - float ratio = std::min(Q->vert.size(),par.n_samples_on_Q) / (float) Q->vert.size(); + ugridQ.Set(Q->vert.begin(),Q->vert.end()); + ugridP.Set(P->vert.begin(),P->vert.end()); - // estimate neigh distance - float avD = 0.0; - for(int i = 0 ; i < 100; ++i){ - int ri = rnd.generate(Q->vert.size()); - std::vector< CoordType > samples; - std::vector dists; - std::vector ress; - vcg::tri::GetKClosestVertex< - MeshType, - vcg::GridStaticPtr, - std::vector, - std::vector, - 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); + if(par.samplingRadius==0) + par.samplingRadius = tri::ComputePoissonDiskRadius(*P,par.sampleNumP); + tri::PoissonPruning(*P, subsetP, par.samplingRadius, par.seed); + tri::PoissonPruning(*Q, subsetQ, par.samplingRadius, par.seed); + par.deltaAbs = par.samplingRadius * par.deltaPerc; + side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation + stat.initTime+=clock()-t0; +} - par.delta = avD * par.delta; - side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation - } -template -bool FourPCS::SelectCoplanarBase() +// Try to select four coplanar points such that they are at least side distance and +// +bool SelectCoplanarBase(FourPoints &B, ScalarType &r1, ScalarType &r2) { + clock_t t0= clock(); + // choose the inter point distance ScalarType dtol = side*0.1; //rough implementation - //choose the first two points - // first point random + // **** first point: random B[0] = P->vert[ rnd.generate(P->vert.size())].P(); - // second a point at distance d+-dtol + // **** second point: a random point at distance side +-dtol int i; for(i = 0; i < P->vert.size(); ++i){ - int id = rnd.generate(P->vert.size()-1); + int id = rnd.generate(P->vert.size()); ScalarType dd = (P->vert[id].P() - B[0]).Norm(); if( ( dd < side + dtol) && (dd > side - dtol)){ B[1] = P->vert[id].P(); @@ -241,87 +211,82 @@ bool FourPCS::SelectCoplanarBase() } if(i == P->vert.size()) return false; - // third point at distance side*0.8 from middle way between B[0] and B[1] + // **** third point: at distance less than side*0.8 from middle way between B[0] and B[1] const vcg::Point3f middle = (B[0]+B[1])/2.0; for(i = 0; i < P->vert.size(); ++i){ - int id = rnd.generate(P->vert.size()-1); - ScalarType dd = (P->vert[id].P() - middle).Norm(); - if( ( dd < side*0.8) ){ + int id = rnd.generate(P->vert.size()); + if( Distance(P->vert[id].P(),middle) < side*0.8 ){ B[2] = P->vert[id].P(); break; } } if(i == P->vert.size()) return false; - //fourth point - float cpr = rnd.generate01(); - vcg::Point3f crossP = B[0] *(1-cpr)+B[1]*cpr; - CoordType B4 = B[2]+(crossP-B[2]).Normalize()*side; - CoordType n = ((B[0]-B[1]).normalized() ^ (B[2]-B[1]).normalized()).normalized(); - ScalarType radius = dtol; + // **** fourth point: + float cpr = rnd.generate01(); + vcg::Point3f crossP = B[0] *(1-cpr)+B[1]*cpr; + CoordType B4 = B[2]+(crossP-B[2]).Normalize()*side; + CoordType n = ((B[0]-B[1]).normalized() ^ (B[2]-B[1]).normalized()).normalized(); + ScalarType radius = dtol; - std::vector closests; - std::vector distances; - std::vector points; + std::vector closests; + std::vector distances; + std::vector points; - vcg::tri::GetInSphereVertex< - MeshType, - vcg::GridStaticPtr, - std::vector, - std::vector, - std::vector - >(*P,ugridP,B4,radius,closests,distances,points); + vcg::tri::GetInSphereVertex< + MeshType, + vcg::GridStaticPtr, + std::vector, + std::vector, + std::vector + >(*P,ugridP,B4,radius,closests,distances,points); - if(closests.empty()) - return false; - int bestInd = -1; ScalarType bestv=std::numeric_limits::max(); - for(i = 0; i P() - B[1]).normalized().dot(n)); - if( dist_from_plane < bestv){ - bestv = dist_from_plane; - bestInd = i; - } - } - if(bestv >dtol) - return false; - B[3] = closests[bestInd]->P(); + if(closests.empty()) + return false; + int bestInd = -1; ScalarType bestv=std::numeric_limits::max(); + for(i = 0; i P() - B[1]).normalized().dot(n)); + if( dist_from_plane < bestv){ + bestv = dist_from_plane; + bestInd = i; + } + } + if(bestv >dtol) + return false; + B[3] = closests[bestInd]->P(); -//printf("B[3] %d\n", (typename MeshType::VertexType*)closests[best] - &(*P->vert.begin())); + //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); + // 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(); + 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() > par.delta ) - return false; + if( ((B[0]+(B[1]-B[0])*r1)-(B[2]+(B[3]-B[2])*r2)).Norm() > par.deltaAbs ) + return false; - radius =side*0.5; - std::vector< CoordType > samples; - std::vector dists; + radius = side*0.5; + std::vector< CoordType > samples; + std::vector dists; - for(int i = 0 ; i< 4; ++i){ - vcg::tri::GetKClosestVertex< - MeshType, - vcg::GridStaticPtr, - std::vector, - std::vector, - std::vector< CoordType > >(*P,ugridP, par.feetSize ,B[i],radius, ExtB[i],dists, samples); - } - - //for(int i = 0 ; i< 4; ++i) - // printf("%d ",ExtB[i].size()); - // printf("\n"); -return true; + for(int i = 0 ; i< 4; ++i){ + vcg::tri::GetKClosestVertex< + MeshType, + vcg::GridStaticPtr, + std::vector, + std::vector, + std::vector< CoordType > >(*P,ugridP, par.feetSize ,B[i],radius, ExtB[i], dists, samples); + } + qDebug("ExtB %i",ExtB[0].size()+ExtB[1].size()+ExtB[2].size()+ExtB[3].size()); + stat.selectCoplanarBaseTime+=clock()-t0; + return true; } - -template -bool FourPCS::IsTransfCongruent(FourPoints fp, vcg::Matrix44 & mat, float & trerr) +bool IsTransfCongruent(const FourPoints &B, const FourPoints &fp, vcg::Matrix44 & mat) { std::vector > fix(4); std::vector > mov(4); @@ -330,232 +295,219 @@ bool FourPCS::IsTransfCongruent(FourPoints fp, vcg::Matrix44 par.delta) return false; - if(fabs( Distance(fix[0],fix[2]) - Distance(mov[0],mov[2]) ) > par.delta) return false; - if(fabs( Distance(fix[0],fix[3]) - Distance(mov[0],mov[3]) ) > par.delta) return false; - if(fabs( Distance(fix[1],fix[2]) - Distance(mov[1],mov[2]) ) > par.delta) return false; - if(fabs( Distance(fix[1],fix[3]) - Distance(mov[1],mov[3]) ) > par.delta) return false; - if(fabs( Distance(fix[2],fix[3]) - Distance(mov[2],mov[3]) ) > par.delta) return false; - - /* - vcg::Point3 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); -*/ + if(fabs( Distance(fix[0],fix[1]) - Distance(mov[0],mov[1]) ) > par.deltaAbs) return false; + if(fabs( Distance(fix[0],fix[2]) - Distance(mov[0],mov[2]) ) > par.deltaAbs) return false; + if(fabs( Distance(fix[0],fix[3]) - Distance(mov[0],mov[3]) ) > par.deltaAbs) return false; + if(fabs( Distance(fix[1],fix[2]) - Distance(mov[1],mov[2]) ) > par.deltaAbs) return false; + if(fabs( Distance(fix[1],fix[3]) - Distance(mov[1],mov[3]) ) > par.deltaAbs) return false; + if(fabs( Distance(fix[2],fix[3]) - Distance(mov[2],mov[3]) ) > par.deltaAbs) return false; vcg::ComputeRigidMatchMatrix(fix,mov,mat); - ScalarType err = 0.0; - for(int i = 0; i < 4; ++i) err+= (mat * mov[i] - fix[i]).SquaredNorm(); - - trerr = vcg::math::Sqrt(err); - return trerr < par.delta; + ScalarType maxSquaredDistance = 0.0; + for(int i = 0; i < 4; ++i) + maxSquaredDistance =std::max(maxSquaredDistance, SquaredDistance(mat * mov[i] ,fix[i])); + return sqrt(maxSquaredDistance) < par.deltaAbs; } -template -void -FourPCS::ComputeR1() +/// Compute the vector R1 of couple of points on FixQ at a given distance. +/// Used by FindCongruent +void ComputeR1(std::vector &R1) { - R1.clear(); - for(int vi = 0; vi < subsetQ.size(); ++vi) - for(int vj = vi; vj < subsetQ.size(); ++vj){ -// ScalarType d = ((Q->vert[subsetQ[vi]]).P()-(Q->vert[subsetQ[vj]]).P()).Norm(); - ScalarType d = (subsetQ[vi]->P()- subsetQ[vj]->P()).Norm(); - if( (d < side+par.delta)) - { - R1.push_back(Couple(subsetQ[vi],subsetQ[vj],d )); - R1.push_back(Couple(subsetQ[vj],subsetQ[vi],d)); - } + R1.clear(); + for(int vi = 0; vi < subsetQ.size(); ++vi) + for(int vj = vi; vj < subsetQ.size(); ++vj){ + ScalarType d = Distance(subsetQ[vi]->P(),subsetQ[vj]->P()); + if( (d < side+par.deltaAbs)) + { + R1.push_back(Couple(subsetQ[vi],subsetQ[vj], d)); + R1.push_back(Couple(subsetQ[vj],subsetQ[vi], d)); + } } - std::sort(R1.begin(),R1.end()); + std::sort(R1.begin(),R1.end()); } -template -bool FourPCS::FindCongruent() { // of base B, on Q, with approximation delta - bool done = false; - std::vector 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(); - - typename PMesh::VertexIterator vii; - typename std::vector::iterator bR1,eR1,bR2,eR2,ite; - bR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(0,0,d1-par.delta)); - eR1 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(0,0,d1+par.delta)); - bR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(0,0,d2-par.delta)); - eR2 = std::lower_bound::iterator,Couple>(R1.begin(),R1.end(),Couple(0,0,d2+par.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 - - // 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::AddVertices(Invr,1); -// (*vii).P() = Q->vert[R1[i][0]].P() + (Q->vert[R1[i][1]].P()-Q->vert[R1[i][0]].P()) * r1; - (*vii).P() = R1[i].p0->P() + ( R1[i].p1->P() - R1[i].p0->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 id = vcg::tri::Allocator::template AddPerVertexAttribute(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::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)); - R2inv.push_back( EPoint( R1[i].p0->P() + (R1[i].p1->P() - R1[i].p0->P()) * r2,i)); - ++i; - } - - n_closests = 0; n_congr = 0; ac =0 ; acf = 0; tr = 0; trf = 0; - printf("R2Inv.size = %d \n",R2inv.size()); - for(uint i = 0 ; i < R2inv.size() ; ++i){ - - std::vector closests; - - // 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(par.delta,par.delta, par.delta)); - bb.Add(R2inv[i].pos-vcg::Point3f(par.delta,par.delta, par.delta)); - - vcg::tri::GetInBoxVertex > - (Invr,*ugrid,bb,closests); - - if(closests.size() > 5) - closests.resize(5); - - n_closests+=closests.size(); - for(uint ip = 0; ip < closests.size(); ++ip){ - FourPoints p; - p[0] = R1[id[closests[ip]]][0]->P(); - p[1] = R1[id[closests[ip]]][1]->P(); - p[2] = R1[ R2inv[i].pi][0]->P(); - p[3] = 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++; - Candidate c(p,mat); - EvaluateAlignment(c); - - if( c.score > par.scoreFeet) - U.push_back(c); - -/* - EvaluateAlignment(U.back()); - U.back().base = bases.size()-1; - - if( U.back().score > par.scoreFeet){ - TestAlignment(U.back()); - if(U.back().score > par.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::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 -int FourPCS::EvaluateSample(Candidate & fp, CoordType & tp, CoordType & np) +// Find congruent elements of a base B, on Q, with approximation delta +// and put them in the U vector. +bool FindCongruent(const std::vector &R1, const FourPoints &B, const ScalarType r1, const ScalarType r2) { - VertexType* v=0; + clock_t t0=clock(); + int n_base=0; + bool done = false; + 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(); + + typename std::vector::const_iterator bR1,eR1,bR2,eR2,ite; + bR1 = std::lower_bound(R1.begin(),R1.end(),Couple(0,0,d1-par.deltaAbs)); + eR1 = std::lower_bound(R1.begin(),R1.end(),Couple(0,0,d1+par.deltaAbs)); + bR2 = std::lower_bound(R1.begin(),R1.end(),Couple(0,0,d2-par.deltaAbs)); + eR2 = std::lower_bound(R1.begin(),R1.end(),Couple(0,0,d2+par.deltaAbs)); + + // in [bR1,eR1) there are all the pairs at a distance d1 +- par.delta + // in [bR1,eR1) there are all the pairs at 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 + + // put [bR1,eR1) in a mesh to have the search operator for free (lazy me) + Invr.Clear(); + typename PMesh::VertexIterator vii; + int i = &(*bR1)-&(*R1.begin()); + for(ite = bR1; ite != eR1;++ite){ + vii = vcg::tri::Allocator::AddVertices(Invr,1); + // (*vii).P() = Q->vert[R1[i][0]].P() + (Q->vert[R1[i][1]].P()-Q->vert[R1[i][0]].P()) * r1; + (*vii).P() = ite->p0->P() + ( ite->p1->P() - ite->p0->P()) * r1; + ++i; + } + if(Invr.vert.empty() ) return false; + + // per vertex attribute 'index' remaps a vertex of Invr to its corresponding point in R1 + typename PMesh::template PerVertexAttributeHandle id = vcg::tri::Allocator::template AddPerVertexAttribute(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::Box(Invr); + + + std::vector R2inv; + 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)); + R2inv.push_back( EPoint( R1[i].p0->P() + (R1[i].p1->P() - R1[i].p0->P()) * r2,i)); + ++i; + } + + GridType ugrid; // griglia + ugrid.Set(Invr.vert.begin(),Invr.vert.end()); + n_closests = 0; n_congr = 0; ac =0 ; acf = 0; tr = 0; trf = 0; + printf("R2Inv.size = %d \n",R2inv.size()); + for(uint i = 0 ; i < R2inv.size() ; ++i) + { + std::vector closests; + + // 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(par.deltaAbs,par.deltaAbs, par.deltaAbs)); + bb.Add(R2inv[i].pos-vcg::Point3f(par.deltaAbs,par.deltaAbs, par.deltaAbs)); + + vcg::tri::GetInBoxVertex > + (Invr,ugrid,bb,closests); + + if(closests.size() > 5) + closests.resize(5); + + n_closests+=closests.size(); + for(uint ip = 0; ip < closests.size(); ++ip) + { + FourPoints p; + p[0] = R1[id[closests[ip]]][0]->cP(); + p[1] = R1[id[closests[ip]]][1]->cP(); + p[2] = R1[ R2inv[i].pi][0]->cP(); + p[3] = R1[ R2inv[i].pi][1]->cP(); + + n_base++; + if(!IsTransfCongruent(B,p,mat)) { + trf++; + } + else{ + tr++; + n_congr++; + Candidate c(p,mat); + EvaluateAlignment(c); + + if( c.score > par.scoreFeet) + U.push_back(c); + } + } + } + + vcg::tri::Allocator::DeletePerVertexAttribute(Invr,id); + printf("n_closests %5d = (An %5d ) + ( Tr %5d ) + (OK) %5d\n",n_closests,acf,trf,n_congr); + + stat.findCongruentTime += clock()-t0; + return done; +} + + +int EvaluateSample(Candidate & fp, const CoordType & tp, const CoordType & np) +{ + CoordType ttp = fp.T * tp; + vcg::Point4 np4 = fp.T * vcg::Point4(np[0],np[1],np[2],0.0); + CoordType tnp(np4[0],np4[1],np4[2]); + 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]; - - if(ugridQ.bbox.IsIn(tp)) - v = vcg::tri::GetClosestVertex(*Q, ugridQ, tp, radius, dist ); + VertexType* v = vcg::tri::GetClosestVertex(*Q, ugridQ, ttp, par.deltaAbs*2.0, dist ); if(v!=0) { - if( v->N().dot(np) > par.cosAngle ) return 1; + if( v->N().dot(tnp) > par.cosAngle ) return 1; else return -1; } else return 0; } - -template -void -FourPCS::EvaluateAlignment(Candidate & fp){ +// Check a candidate against the small subset of points ExtB +void EvaluateAlignment(Candidate & 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); + n_delta_close+=EvaluateSample(fp, ExtB[i][j]->P(), ExtB[i][j]->cN()); } } fp.score = n_delta_close; } -template -void 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(); - CoordType tp = subsetP[j]->P(); - n_delta_close+=EvaluateSample(fp,tp,np); - } - fp.score = n_delta_close; +void TestAlignment(Candidate & fp) +{ + clock_t t0 = clock(); + 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); + } + fp.score = n_delta_close; + stat.testAlignmentTime += clock()-t0; } -template -bool FourPCS:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb ) +bool Align(vcg::Matrix44f & result, vcg::CallBackPos * cb ) +{ + int maxAttempt =100; + int scoreThr = par.sampleNumP*0.8; + + Candidate bestC; + + std::vector R1; + ComputeR1(R1); + for(int i = 0; i < maxAttempt && bestC.score bestC.score) + bestC = U[i]; + } + } + } + result = bestC.T; + return bestC.score >0; +} + +bool Align(int L, vcg::Matrix44f & result, vcg::CallBackPos * cb ) { int bestv = 0; bool found; @@ -564,37 +516,40 @@ bool FourPCS:: Align( int L, vcg::Matrix44f & result, vcg::CallBa if(L==0) { + // overlap is expressed as the probability that a point in P(mov) can be found in Q (fix) L = (log(1.0-0.9) / log(1.0-pow((float)par.overlap,3.f)))+1; printf("using %d bases\n",L); } - - ComputeR1(); + std::vector R1; + ComputeR1(R1); for(int t = 0; t < L; ++t ) { + FourPoints B; + ScalarType r1,r2; do { n_tries = 0; do { n_tries++; - found = SelectCoplanarBase(); + found = SelectCoplanarBase(B,r1,r2); } - while(!found && (n_tries <50)); + while(!found && (n_tries < 50)); if(!found) { par.overlap*=0.9; side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation - ComputeR1(); + ComputeR1(R1); } } while (!found && (par.overlap >0.1)); - if(par.overlap <0.1) { + if(par.overlap < 0.1) { printf("FAILED"); return false; } bases.push_back(B); if(cb) cb(t*100/L,"Trying bases"); - if(FindCongruent()) + if(FindCongruent(R1,B,r1,r2)) break; } @@ -619,6 +574,8 @@ bool FourPCS:: Align( int L, vcg::Matrix44f & result, vcg::CallBa return true; } +}; // end class + } // namespace tri } // namespace vcg #endif