From 60375025242764693f05c00aef9a0d9a350fca66 Mon Sep 17 00:00:00 2001 From: Gianpaolo Palma gianpaolopalma Date: Thu, 21 Jul 2016 09:19:14 +0000 Subject: [PATCH] Bug fixed due to a parenthesis mismatch --- .../edit_align/align/AlignPair.cpp | 768 +++++++++--------- 1 file changed, 387 insertions(+), 381 deletions(-) diff --git a/src/meshlabplugins/edit_align/align/AlignPair.cpp b/src/meshlabplugins/edit_align/align/AlignPair.cpp index 2f2b6a555..fd63c57e3 100644 --- a/src/meshlabplugins/edit_align/align/AlignPair.cpp +++ b/src/meshlabplugins/edit_align/align/AlignPair.cpp @@ -43,92 +43,92 @@ using namespace std; bool AlignPair::A2Mesh::Import(const char *filename, Matrix44d &Tr) { - int err = tri::io::Importer::Open(*this,filename); - if(err) { - printf("Error in reading %s: '%s'\n",filename,tri::io::Importer::ErrorMsg(err)); - exit(-1); - } - printf("read mesh `%s'\n", filename); - return Init(Tr); + int err = tri::io::Importer::Open(*this, filename); + if (err) { + printf("Error in reading %s: '%s'\n", filename, tri::io::Importer::ErrorMsg(err)); + exit(-1); + } + printf("read mesh `%s'\n", filename); + return Init(Tr); } bool AlignPair::A2Mesh::InitVert(const Matrix44d &Tr) { - Matrix44d Idn; Idn.SetIdentity(); - if(Tr!=Idn) tri::UpdatePosition::Matrix(*this,Tr); - tri::UpdateNormal::NormalizePerVertex(*this); - tri::UpdateBounding::Box(*this); - return true; + Matrix44d Idn; Idn.SetIdentity(); + if (Tr != Idn) tri::UpdatePosition::Matrix(*this, Tr); + tri::UpdateNormal::NormalizePerVertex(*this); + tri::UpdateBounding::Box(*this); + return true; } bool AlignPair::A2Mesh::Init(const Matrix44d &Tr) { - Matrix44d Idn; Idn.SetIdentity(); - tri::Clean::RemoveUnreferencedVertex(*this); - if(Tr!=Idn) tri::UpdatePosition::Matrix(*this,Tr); - tri::UpdateNormal::PerVertexNormalizedPerFaceNormalized(*this); - tri::UpdateFlags::FaceBorderFromNone(*this); - tri::UpdateBounding::Box(*this); + Matrix44d Idn; Idn.SetIdentity(); + tri::Clean::RemoveUnreferencedVertex(*this); + if (Tr != Idn) tri::UpdatePosition::Matrix(*this, Tr); + tri::UpdateNormal::PerVertexNormalizedPerFaceNormalized(*this); + tri::UpdateFlags::FaceBorderFromNone(*this); + tri::UpdateBounding::Box(*this); - return true; + return true; } void AlignPair::Stat::clear() { - I.clear(); - StartTime=0; - MovVertNum=0; - FixVertNum=0; - FixFaceNum=0; + I.clear(); + StartTime = 0; + MovVertNum = 0; + FixVertNum = 0; + FixFaceNum = 0; } // Restituisce true se nelle ultime iterazioni non e' diminuito // l'errore bool AlignPair::Stat::Stable(int lastiter) { - if(I.empty()) return false; - int parag = I.size()-lastiter; + if (I.empty()) return false; + int parag = I.size() - lastiter; - if(parag<0) parag=0; - if( I.back().pcl50 < I[parag].pcl50 ) return false; // se siamo diminuiti non e' stabile + if (parag < 0) parag = 0; + if (I.back().pcl50 < I[parag].pcl50) return false; // se siamo diminuiti non e' stabile - return true; + return true; } void AlignPair::Stat::Dump(FILE *fp) { - if(I.size()==0) { - fprintf(fp,"Empty AlignPair::Stat\n"); - return; - } - fprintf(fp,"Final Err %8.5f In %i iterations Total Time %ims\n",LastPcl50(),(int)I.size(),TotTime()); - fprintf(fp,"Mindist Med Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); - for(unsigned int qi=0;qi\n"); - fprintf(fp," Mindist 50ile Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); - for(unsigned int qi=0;qi %8.5f %9.6f %8.5f %5.3f %8.5f %9.6f %4ims %5i %5i %4i %4i %4i \n", - I[qi].MinDistAbs, - I[qi].pcl50, I[qi].pclhi, - I[qi].AVG, I[qi].RMS, I[qi].StdDev , - IterTime(qi), - I[qi].SampleTested,I[qi].SampleUsed,I[qi].DistanceDiscarded,I[qi].BorderDiscarded,I[qi].AngleDiscarded); - fprintf(fp,"\n"); + fprintf(fp, "Final Err %8.5f In %i iterations Total Time %ims\n", LastPcl50(), (int)I.size(), TotTime()); + fprintf(fp, "\n"); + fprintf(fp, "\n", + I[qi].MinDistAbs, + I[qi].pcl50, I[qi].pclhi, + I[qi].AVG, I[qi].RMS, I[qi].StdDev, + IterTime(qi), + I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded); + fprintf(fp, "
Mindist 50ile Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); + for (unsigned int qi = 0; qi < I.size(); ++qi) + fprintf(fp, "
%8.5f %9.6f %8.5f %5.3f %8.5f %9.6f %4ims %5i %5i %4i %4i %4i
\n"); } @@ -141,56 +141,56 @@ MaxPointNum an (unused) hard limit on the number of points that are choosen MinPointNum the minimum number of points that have to be chosen to be usable */ -bool AlignPair::ChoosePoints( vector &Ps, // vertici corrispondenti su src (rossi) - vector &Ns, // normali corrispondenti su src (rossi) - vector &Pt, // vertici scelti su trg (verdi) - vector &OPt, // vertici scelti su trg (verdi) - double PassHi, - Histogramf &H) +bool AlignPair::ChoosePoints(vector &Ps, // vertici corrispondenti su src (rossi) + vector &Ns, // normali corrispondenti su src (rossi) + vector &Pt, // vertici scelti su trg (verdi) + vector &OPt, // vertici scelti su trg (verdi) + double PassHi, + Histogramf &H) { - const int N = ap.MaxPointNum; - double newmaxd = H.Percentile(PassHi); - //printf("%5.1f of the pairs has a distance less than %g and greater than %g (0..%g) avg %g\n", Perc*100,newmind,newmaxd,H.maxv,H.Percentile(.5)); - int sz = Ps.size(); - int fnd=0; - int lastgood=sz-1; - math::SubtractiveRingRNG myrnd; - while(fnd %f\n",fnd,sz,newmaxd); + else + { + swap(Ps[index], Ps[lastgood]); + swap(Ns[index], Ns[lastgood]); + swap(Pt[index], Pt[lastgood]); + swap(OPt[index], OPt[lastgood]); + lastgood--; + } + } + Ps.resize(fnd); + Ns.resize(fnd); + Pt.resize(fnd); + OPt.resize(fnd); + printf("Scelte %i coppie tra le %i iniziali, scartate quelle con dist > %f\n", fnd, sz, newmaxd); - if( (int)Ps.size() Calcola anche il nuovo bounding box di tali vertici trasformati. */ bool AlignPair::InitMov( - vector< Point3d > &MovVert, - vector< Point3d > &MovNorm, - Box3d &trgbox, - const Matrix44d &in ) // trasformazione Iniziale (che porta i punti di trg su src) + vector< Point3d > &MovVert, + vector< Point3d > &MovNorm, + Box3d &trgbox, + const Matrix44d &in) // trasformazione Iniziale (che porta i punti di trg su src) { - Point3d pp,nn; + Point3d pp, nn; - MovVert.clear(); - MovNorm.clear(); - trgbox.SetNull(); + MovVert.clear(); + MovNorm.clear(); + trgbox.SetNull(); - A2Mesh::VertexIterator vi; - for(vi=mov->begin(); vi!=mov->end(); vi++) { - pp=in*(*vi).P(); - nn=in*Point3d((*vi).P()+(*vi).N())-pp; - nn.Normalize(); - MovVert.push_back(pp); - MovNorm.push_back(nn); - trgbox.Add(pp); - } - return true; + A2Mesh::VertexIterator vi; + for (vi = mov->begin(); vi != mov->end(); vi++) { + pp = in*(*vi).P(); + nn = in*Point3d((*vi).P() + (*vi).N()) - pp; + nn.Normalize(); + MovVert.push_back(pp); + MovNorm.push_back(nn); + trgbox.Add(pp); + } + return true; } bool AlignPair::InitFixVert(AlignPair::A2Mesh *fm, - AlignPair::Param &pp, - A2GridVert &u, - int PreferredGridSize) + AlignPair::Param &pp, + A2GridVert &u, + int PreferredGridSize) { - Box3d bb2=fm->bbox; - double MinDist= pp.MinDistAbs*1.1; - //the bbox of the grid should be enflated of the mindist used in the ICP search - bb2.Offset(Point3d(MinDist,MinDist,MinDist)); + Box3d bb2 = fm->bbox; + double MinDist = pp.MinDistAbs*1.1; + //the bbox of the grid should be enflated of the mindist used in the ICP search + bb2.Offset(Point3d(MinDist, MinDist, MinDist)); - u.SetBBox(bb2); + u.SetBBox(bb2); - //Inserisco la src nella griglia - if(PreferredGridSize==0) PreferredGridSize=fm->vert.size()*pp.UGExpansionFactor; - u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize); - printf("UG %i %i %i\n",u.siz[0],u.siz[1],u.siz[2]); - return true; + //Inserisco la src nella griglia + if (PreferredGridSize == 0) PreferredGridSize = fm->vert.size()*pp.UGExpansionFactor; + u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize); + printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]); + return true; } bool AlignPair::InitFix(AlignPair::A2Mesh *fm, - AlignPair::Param &pp, - A2Grid &u, - int PreferredGridSize) + AlignPair::Param &pp, + A2Grid &u, + int PreferredGridSize) { - tri::InitFaceIMark(*fm); + tri::InitFaceIMark(*fm); - Box3d bb2=fm->bbox; - // double MinDist= fm->bbox.Diag()*pp.MinDistPerc; - double MinDist= pp.MinDistAbs*1.1; - //gonfio della distanza utente il BBox della seconda mesh - bb2.Offset(Point3d(MinDist,MinDist,MinDist)); + Box3d bb2 = fm->bbox; + // double MinDist= fm->bbox.Diag()*pp.MinDistPerc; + double MinDist = pp.MinDistAbs*1.1; + //gonfio della distanza utente il BBox della seconda mesh + bb2.Offset(Point3d(MinDist, MinDist, MinDist)); - u.SetBBox(bb2); + u.SetBBox(bb2); - //Inserisco la src nella griglia - if(PreferredGridSize==0) PreferredGridSize=fm->face.size()*pp.UGExpansionFactor; - u.Set(fm->face.begin(), fm->face.end(), PreferredGridSize); - printf("UG %i %i %i\n",u.siz[0],u.siz[1],u.siz[2]); - return true; + //Inserisco la src nella griglia + if (PreferredGridSize == 0) PreferredGridSize = fm->face.size()*pp.UGExpansionFactor; + u.Set(fm->face.begin(), fm->face.end(), PreferredGridSize); + printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]); + return true; } /* The Main ICP alignment Function: @@ -280,204 +280,210 @@ la uniform grid sia gia' inizializzata con la mesh fix */ bool AlignPair::Align( - A2Grid &u, - A2GridVert &uv, - const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix) - Matrix44d &out, // trasformazione calcolata - vector &Pfix, // vertici corrispondenti su src (rossi) - vector &Nfix, // normali corrispondenti su src (rossi) - vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) - vector &ONmov, // normali scelti su trg (verdi) - Histogramf &H, - AlignPair::Stat &as) + A2Grid &u, + A2GridVert &uv, + const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix) + Matrix44d &out, // trasformazione calcolata + vector &Pfix, // vertici corrispondenti su src (rossi) + vector &Nfix, // normali corrispondenti su src (rossi) + vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) + vector &ONmov, // normali scelti su trg (verdi) + Histogramf &H, + AlignPair::Stat &as) { - vector beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare - // ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore; - // i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu'; - const int maxBeyondCnt=3; - vector< Point3d > movvert; - vector< Point3d > movnorm; - vector Pmov; // vertici scelti dopo la trasf iniziale - status=SUCCESS; - int tt0=clock(); + vector beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare + // ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore; + // i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu'; + const int maxBeyondCnt = 3; + vector< Point3d > movvert; + vector< Point3d > movnorm; + vector Pmov; // vertici scelti dopo la trasf iniziale + status = SUCCESS; + int tt0 = clock(); - out=in; + out = in; - int i; + int i; - double CosAngleThr=cos(ap.MaxAngleRad); - double StartMinDist=ap.MinDistAbs; - int tt1=clock(); - int ttsearch=0; - int ttleast=0; - int nc=0; - as.clear(); - as.StartTime=clock(); + double CosAngleThr = cos(ap.MaxAngleRad); + double StartMinDist = ap.MinDistAbs; + int tt1 = clock(); + int ttsearch = 0; + int ttleast = 0; + int nc = 0; + as.clear(); + as.StartTime = clock(); - beyondCntVec.resize(mov->size(),0); + beyondCntVec.resize(mov->size(), 0); - /**************** BEGIN ICP LOOP ****************/ - do + /**************** BEGIN ICP LOOP ****************/ + do + { + Stat::IterInfo ii; + Box3d movbox; + InitMov(movvert, movnorm, movbox, out); + H.SetRange(0, StartMinDist, 512, 2.5); + Pfix.clear(); + Nfix.clear(); + Pmov.clear(); + OPmov.clear(); + ONmov.clear(); + int tts0 = clock(); + ii.MinDistAbs = StartMinDist; + int LocSampleNum = min(ap.SampleNum, int(movvert.size())); + Box3d fixbox; + if (u.Empty()) fixbox = uv.bbox; + else fixbox = u.bbox; + for (i = 0; i < LocSampleNum; ++i) { - Stat::IterInfo ii; - Box3d movbox; - InitMov(movvert,movnorm,movbox,out); - H.SetRange(0,StartMinDist,512,2.5); - Pfix.clear(); - Nfix.clear(); - Pmov.clear(); - OPmov.clear(); - ONmov.clear(); - int tts0=clock(); - ii.MinDistAbs=StartMinDist; - int LocSampleNum=min(ap.SampleNum,int(movvert.size())); - Box3d fixbox; - if(u.Empty()) fixbox = uv.bbox; - else fixbox = u.bbox; - for(i=0;i= StartMinDist) { + ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; } - else - { - double error=StartMinDist; - Point3d closestPoint, closestNormal; - double maxd= StartMinDist; - ii.SampleTested++; - if(u.Empty()) // using the point cloud grid - { - A2Mesh::VertexPointer vp = tri::GetClosestVertex(*fix,uv,movvert[i], maxd, error); - if(error>=StartMinDist) { - ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; - } - if(movnorm[i].dot(vp->N()) < CosAngleThr) { - ii.AngleDiscarded++; continue; - } - closestPoint=vp->P(); - closestNormal=vp->N(); - } - else // using the standard faces and grid - { - A2Mesh::FacePointer f=vcg::tri::GetClosestFaceBase(*fix, u, movvert[i], maxd, error, closestPoint); - if(error>=StartMinDist) { - ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; - } - if(movnorm[i].dot(f->N()) < CosAngleThr) { - ii.AngleDiscarded++; continue; - } - Point3d ip; - InterpolationParameters(*f,f->N(),closestPoint, ip); - const double IP_EPS = 0.00001; - // If ip[i] == 0 it means that we are on the edge opposite to i - if( (fabs(ip[0])<=IP_EPS && f->IsB(1)) || (fabs(ip[1])<=IP_EPS && f->IsB(2)) || (fabs(ip[2])<=IP_EPS && f->IsB(0)) ){ - ii.BorderDiscarded++; continue; - } - closestNormal = f->N(); - } - // The sample was accepted. Store it. - Pmov.push_back(movvert[i]); - OPmov.push_back((*mov)[i].P()); - ONmov.push_back((*mov)[i].N()); - Nfix.push_back( closestNormal ); - Pfix.push_back( closestPoint ); - H.Add(float(error)); - ii.SampleUsed++; + if (movnorm[i].dot(vp->N()) < CosAngleThr) { + ii.AngleDiscarded++; continue; } - } // End for each pmov - int tts1=clock(); - //printf("Found %d pairs\n",(int)Pfix.size()); - if(!ChoosePoints(Pfix,Nfix,Pmov,OPmov,ap.PassHiFilter,H)) - if(int(Pfix.size())P(); + closestNormal = vp->N(); + } + else // using the standard faces and grid + { + A2Mesh::FacePointer f = vcg::tri::GetClosestFaceBase(*fix, u, movvert[i], maxd, error, closestPoint); + if (error >= StartMinDist) { + ii.DistanceDiscarded++; ++beyondCntVec[i]; continue; } - Matrix44d newout; - switch(ap.MatchMode) { - case AlignPair::Param::MMSimilarity : ComputeRotoTranslationScalingMatchMatrix(newout,Pfix,OPmov); break; - case AlignPair::Param::MMRigid : ComputeRigidMatchMatrix(Pfix,OPmov,newout); break; - default : - status = UNKNOWN_MODE; - ii.Time=clock(); - as.I.push_back(ii); - return false; + if (movnorm[i].dot(f->N()) < CosAngleThr) { + ii.AngleDiscarded++; continue; } + Point3d ip; + InterpolationParameters(*f, f->N(), closestPoint, ip); + const double IP_EPS = 0.00001; + // If ip[i] == 0 it means that we are on the edge opposite to i + if ((fabs(ip[0]) <= IP_EPS && f->IsB(1)) || (fabs(ip[1]) <= IP_EPS && f->IsB(2)) || (fabs(ip[2]) <= IP_EPS && f->IsB(0))){ + ii.BorderDiscarded++; continue; + } + closestNormal = f->N(); + } + // The sample was accepted. Store it. + Pmov.push_back(movvert[i]); + OPmov.push_back((*mov)[i].P()); + ONmov.push_back((*mov)[i].N()); + Nfix.push_back(closestNormal); + Pfix.push_back(closestPoint); + H.Add(float(error)); + ii.SampleUsed++; + } + else + beyondCntVec[i] = maxBeyondCnt + 1; + } - // double sum_before=0; - // double sum_after=0; - // for(unsigned int iii=0;iii %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ; - - // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata. - // Nei prossimi cicli si parte da questa matrice come iniziale. - out=newout; - - assert(Pfix.size()==Pmov.size()); - int tts2=clock(); - ttsearch+=tts1-tts0; - ttleast +=tts2-tts1; - ii.pcl50=H.Percentile(.5); - ii.pclhi=H.Percentile(ap.PassHiFilter); - ii.AVG=H.Avg(); - ii.RMS=H.RMS(); - ii.StdDev=H.StandardDeviation(); - ii.Time=clock(); - as.I.push_back(ii); - nc++; - // The distance of the next points to be considered is lowered according to the parameter. - // We use 5 times the percentile of the found points. - if(ap.ReduceFactorPerc<1) StartMinDist=max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist,5.0*H.Percentile(ap.ReduceFactorPerc))); - } - while ( - nc<=ap.MaxIterNum && - H.Percentile(.5) > ap.TrgDistAbs && - (ncap.MaxScale || math::Abs(1-scv[1])>ap.MaxScale || math::Abs(1-scv[2])>ap.MaxScale) ) { - status = TOO_MUCH_SCALE; + } + } // End for each pmov + int tts1 = clock(); + //printf("Found %d pairs\n",(int)Pfix.size()); + if (!ChoosePoints(Pfix, Nfix, Pmov, OPmov, ap.PassHiFilter, H)) + { + if (int(Pfix.size()) < ap.MinPointNum) + { + status = TOO_FEW_POINTS; + ii.Time = clock(); + as.I.push_back(ii); return false; + } } - if(shv[0]>ap.MaxShear || shv[1]>ap.MaxShear || shv[2]>ap.MaxShear ) { - status = TOO_MUCH_SHEAR; - return false; + Matrix44d newout; + switch (ap.MatchMode) + { + case AlignPair::Param::MMSimilarity: ComputeRotoTranslationScalingMatchMatrix(newout, Pfix, OPmov); break; + case AlignPair::Param::MMRigid: ComputeRigidMatchMatrix(Pfix, OPmov, newout); break; + default: + status = UNKNOWN_MODE; + ii.Time = clock(); + as.I.push_back(ii); + return false; } - printf("Grid %i %i %i - fn %i\n",u.siz[0],u.siz[1],u.siz[2],fix->fn); - printf("Init %8.3f Loop %8.3f Search %8.3f least sqrt %8.3f\n", - float(tt1-tt0)/CLOCKS_PER_SEC, float(tt2-tt1)/CLOCKS_PER_SEC, - float(ttsearch)/CLOCKS_PER_SEC,float(ttleast)/CLOCKS_PER_SEC ); - return true; + // double sum_before=0; + // double sum_after=0; + // for(unsigned int iii=0;iii %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ; + + // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata. + // Nei prossimi cicli si parte da questa matrice come iniziale. + out = newout; + + assert(Pfix.size() == Pmov.size()); + int tts2 = clock(); + ttsearch += tts1 - tts0; + ttleast += tts2 - tts1; + ii.pcl50 = H.Percentile(.5); + ii.pclhi = H.Percentile(ap.PassHiFilter); + ii.AVG = H.Avg(); + ii.RMS = H.RMS(); + ii.StdDev = H.StandardDeviation(); + ii.Time = clock(); + as.I.push_back(ii); + nc++; + // The distance of the next points to be considered is lowered according to the parameter. + // We use 5 times the percentile of the found points. + if (ap.ReduceFactorPerc<1) StartMinDist = max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist, 5.0*H.Percentile(ap.ReduceFactorPerc))); + } while ( + nc <= ap.MaxIterNum && + H.Percentile(.5) > ap.TrgDistAbs && + (ncap.MaxScale || math::Abs(1 - scv[1]) > ap.MaxScale || math::Abs(1 - scv[2]) > ap.MaxScale)) { + status = TOO_MUCH_SCALE; + return false; + } + if (shv[0] > ap.MaxShear || shv[1] > ap.MaxShear || shv[2] > ap.MaxShear) { + status = TOO_MUCH_SHEAR; + return false; + } + printf("Grid %i %i %i - fn %i\n", u.siz[0], u.siz[1], u.siz[2], fix->fn); + printf("Init %8.3f Loop %8.3f Search %8.3f least sqrt %8.3f\n", + float(tt1 - tt0) / CLOCKS_PER_SEC, float(tt2 - tt1) / CLOCKS_PER_SEC, + float(ttsearch) / CLOCKS_PER_SEC, float(ttleast) / CLOCKS_PER_SEC); + + return true; } -const char *AlignPair::ErrorMsg( ErrorCode code) +const char *AlignPair::ErrorMsg(ErrorCode code) { - switch(code){ - case SUCCESS: return "Success "; - case NO_COMMON_BBOX : return "No Common BBox "; - case TOO_FEW_POINTS : return "Too few points "; - case LSQ_DIVERGE : return "LSQ not converge"; - case TOO_MUCH_SHEAR : return "Too much shear "; - case TOO_MUCH_SCALE : return "Too much scale "; - case UNKNOWN_MODE : return "Unknown mode "; - default : assert(0); return "Catastrophic Error"; - } - return 0; + switch (code){ + case SUCCESS: return "Success "; + case NO_COMMON_BBOX: return "No Common BBox "; + case TOO_FEW_POINTS: return "Too few points "; + case LSQ_DIVERGE: return "LSQ not converge"; + case TOO_MUCH_SHEAR: return "Too much shear "; + case TOO_MUCH_SCALE: return "Too much scale "; + case UNKNOWN_MODE: return "Unknown mode "; + default: assert(0); return "Catastrophic Error"; + } + return 0; } /* @@ -625,38 +631,38 @@ return maxfnd; bool AlignPair::SampleMovVert(vector &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode) { - switch(SampleMode) - { - case AlignPair::Param::SMRandom : return SampleMovVertRandom(vert,SampleNum); - case AlignPair::Param::SMNormalEqualized : return SampleMovVertNormalEqualized(vert,SampleNum); - default: assert(0); - } - return false; + switch (SampleMode) + { + case AlignPair::Param::SMRandom: return SampleMovVertRandom(vert, SampleNum); + case AlignPair::Param::SMNormalEqualized: return SampleMovVertNormalEqualized(vert, SampleNum); + default: assert(0); + } + return false; } // Function to retrieve a static random number generator object. static math::SubtractiveRingRNG &LocRnd(){ - static math::SubtractiveRingRNG myrnd(time(NULL)); - return myrnd; + static math::SubtractiveRingRNG myrnd(time(NULL)); + return myrnd; } // Gets a random number in the interval [0..n]. static int LocRnd(int n){ - return LocRnd().generate(n); + return LocRnd().generate(n); } // Scelta a caso semplice bool AlignPair::SampleMovVertRandom(vector &vert, int SampleNum) { - if(int(vert.size())<=SampleNum) return true; - int i; - for(i=0;i=0 && pos < int(vert.size())); - swap(vert[i],vert[pos]); - } - vert.resize(SampleNum); - return true; + if (int(vert.size()) <= SampleNum) return true; + int i; + for (i = 0; i < SampleNum; ++i) + { + int pos = LocRnd(vert.size()); + assert(pos >= 0 && pos < int(vert.size())); + swap(vert[i], vert[pos]); + } + vert.resize(SampleNum); + return true; } /* @@ -673,51 +679,51 @@ e poi un punto all'interno del bucket bool AlignPair::SampleMovVertNormalEqualized(vector &vert, int SampleNum) { - // assert(0); + // assert(0); - // int t0=clock(); - static vector NV; - if(NV.size()==0) + // int t0=clock(); + static vector NV; + if (NV.size() == 0) + { + GenNormal::Fibonacci(30, NV); + printf("Generated %i normals\n", int(NV.size())); + } + // Bucket vector dove, per ogni normale metto gli indici + // dei vertici ad essa corrispondenti + vector > BKT(NV.size()); + for (size_t i = 0; i < vert.size(); ++i) + { + int ind = GenNormal::BestMatchingNormal(vert[i].N(), NV); + BKT[ind].push_back(i); + } + //int t1=clock(); + + // vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket + vector BKTpos(BKT.size(), 0); + + if (SampleNum >= int(vert.size())) SampleNum = vert.size() - 1; + + for (int i = 0; i < SampleNum;) + { + int ind = LocRnd(BKT.size()); // Scelgo un Bucket + int &CURpos = BKTpos[ind]; + vector &CUR = BKT[ind]; + + if (CURpos::Fibonacci(30,NV); - printf("Generated %i normals\n",int(NV.size())); + swap(CUR[CURpos], CUR[CURpos + LocRnd(BKT[ind].size() - CURpos)]); + swap(vert[i], vert[CUR[CURpos]]); + ++BKTpos[ind]; + ++i; } - // Bucket vector dove, per ogni normale metto gli indici - // dei vertici ad essa corrispondenti - vector > BKT(NV.size()); - for(size_t i=0;i::BestMatchingNormal(vert[i].N(),NV); - BKT[ind].push_back(i); - } - //int t1=clock(); + } + vert.resize(SampleNum); + // int t2=clock(); + // printf("Matching %6i\n",t1-t0); + // printf("Collecting %6i\n",t2-t1); + // printf("Total %6i\n",t2-t0); - // vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket - vector BKTpos(BKT.size(),0); - - if(SampleNum >= int(vert.size())) SampleNum= vert.size()-1; - - for(int i=0;i &CUR = BKT[ind]; - - if(CURpos