From 074a89c5888561c36e022efe9cd7333ffba4ca9b Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Fri, 29 May 2020 14:48:29 +0200 Subject: [PATCH] more implementations imported from meshlab --- vcg/complex/algorithms/align_pair.h | 357 ++++++++++++++++-- vcg/complex/algorithms/point_matching_scale.h | 4 + 2 files changed, 325 insertions(+), 36 deletions(-) diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h index f7803a98..bc5c7a6f 100644 --- a/vcg/complex/algorithms/align_pair.h +++ b/vcg/complex/algorithms/align_pair.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace vcg { @@ -57,6 +58,12 @@ Classe per gestire un allineamento tra DUE sole mesh. class AlignPair { public: + AlignPair() + { + clear(); + myrnd.initialize(time(NULL)); + } + enum ErrorCode { SUCCESS, NO_COMMON_BBOX, @@ -334,7 +341,7 @@ public: Stat as; Param ap; ErrorCode status; - bool IsValid() + bool isValid() { return status==SUCCESS; } @@ -392,12 +399,6 @@ public: status=SUCCESS; } - AlignPair() - { - clear(); - myrnd.initialize(time(NULL)); - } - /******* Data Members *********/ std::vector *mov; @@ -443,22 +444,126 @@ public: } } - bool SampleMovVert(std::vector &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode); - bool SampleMovVertRandom(std::vector &vert, int SampleNum); - bool SampleMovVertNormalEqualized(std::vector &vert, int SampleNum); -/* -*Una volta trovati coppie di punti corrispondenti se ne sceglie -al piu' per calcolare la trasformazione che li fa coincidere. -La scelta viene fatta in base ai due parametri PassLo e PassHi; -*/ - bool ChoosePoints( - std::vector &Ps, // vertici corrispondenti su fix (rossi) - std::vector &Ns, // normali corrispondenti su fix (rossi) - std::vector &Pt, // vertici scelti su mov (verdi) - std::vector &OPt, // vertici scelti su mov (verdi) + inline bool sampleMovVert( + std::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; + } + } + + inline bool SampleMovVertRandom(std::vector &vert, int sampleNum) + { + if (int(vert.size()) <= sampleNum) + return true; + for (int i = 0; i < sampleNum; ++i) { + int pos = myrnd.generate(vert.size()); + assert(pos >= 0 && pos < int(vert.size())); + std::swap(vert[i], vert[pos]); + } + vert.resize(sampleNum); + return true; + } + + bool SampleMovVertNormalEqualized(std::vector &vert, int sampleNum) + { + std::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 + std::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(int(i)); + } + + // vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket + std::vector BKTpos(BKT.size(), 0); + + if (sampleNum >= int(vert.size())) + sampleNum = vert.size() - 1; + + for (int i = 0; i < sampleNum;) { + int ind = myrnd.generate(BKT.size()); // Scelgo un Bucket + int &CURpos = BKTpos[ind]; + std::vector &CUR = BKT[ind]; + + if (CURpos coppie di punti corrispondenti se ne sceglie + * al piu' per calcolare la trasformazione che li fa coincidere. + * La scelta viene fatta in base ai due parametri PassLo e PassHi; + */ + inline bool choosePoints( + std::vector &ps, // vertici corrispondenti su fix (rossi) + std::vector &ns, // normali corrispondenti su fix (rossi) + std::vector &pt, // vertici scelti su mov (verdi) + std::vector &opt, // vertici scelti su mov (verdi) //vector &Nt, // normali scelti su mov (verdi) - double PassHi, - Histogramf &H); + double passHi, + Histogramf &h) + { + const int N = ap.MaxPointNum; + double newmaxd = h.Percentile(float(passHi)); + int sz = int(ps.size()); + int fnd = 0; + int lastgood = sz - 1; + math::SubtractiveRingRNG myrnd; + while (fnd < N && fnd < lastgood) { + int index = fnd + myrnd.generate(lastgood - fnd); + double dd = Distance(ps[index], pt[index]); + if (dd <= newmaxd){ + std::swap(ps[index], ps[fnd]); + std::swap(ns[index], ns[fnd]); + std::swap(pt[index], pt[fnd]); + std::swap(opt[index], opt[fnd]); + ++fnd; + } + else { + std::swap(ps[index], ps[lastgood]); + std::swap(ns[index], ns[lastgood]); + std::swap(pt[index], pt[lastgood]); + std::swap(opt[index], opt[lastgood]); + lastgood--; + } + } + ps.resize(fnd); + ns.resize(fnd); + pt.resize(fnd); + opt.resize(fnd); + + if ((int)ps.size() < ap.MinPointNum){ + printf("Troppi pochi punti!\n"); + ps.clear(); + ns.clear(); + pt.clear(); + opt.clear(); + return false; + } + return true; + } /* Minimo esempio di codice per l'uso della funzione di allineamento. @@ -491,19 +596,26 @@ aa.Align(In,UG,res); // si spera :) res.as.Dump(stdout); */ - bool Align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res) + bool align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res) { res.ap=ap; - bool ret=Align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); + bool ret=align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as); res.err=res.as.lastPcl50(); res.status=status; return ret; } - double Abs2Perc(double val, Box3d bb) const {return val/bb.Diag();} - double Perc2Abs(double val, Box3d bb) const {return val*bb.Diag();} + double abs2Perc(double val, Box3d bb) const + { + return val/bb.Diag(); + } + + double perc2Abs(double val, Box3d bb) const + { + return val*bb.Diag(); + } /************************************************************************************ Versione Vera della Align a basso livello. @@ -513,17 +625,190 @@ in ************************************************************************************/ - bool Align( + inline bool align( A2Grid &u, - A2GridVert &uv, - const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix - Matrix44d &out, // trasformazione calcolata - std::vector &Pfix, // vertici corrispondenti su src (rossi) - std::vector &Nfix, // normali corrispondenti su src (rossi) - std::vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) - std::vector &ONmov, // normali scelti su trg (verdi) - Histogramf &H, - Stat &as); + A2GridVert &uv, + const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix + Matrix44d &out, // trasformazione calcolata + std::vector &pfix, // vertici corrispondenti su src (rossi) + std::vector &nfix, // normali corrispondenti su src (rossi) + std::vector &opmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector &onmov, // normali scelti su trg (verdi) + Histogramf &h, + Stat &as) + { + std::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; + std::vector< Point3d > movvert; + std::vector< Point3d > movnorm; + std::vector Pmov; // vertici scelti dopo la trasf iniziale + status = SUCCESS; + int tt0 = clock(); + + out = in; + + 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(); + + beyondCntVec.resize(mov->size(), 0); + + /**************** BEGIN ICP LOOP ****************/ + do { + Stat::IterInfo ii; + Box3d movbox; + InitMov(movvert, movnorm, movbox, out); + h.SetRange(0.0f, float(startMinDist), 512, 2.5f); + pfix.clear(); + nfix.clear(); + Pmov.clear(); + opmov.clear(); + onmov.clear(); + int tts0 = clock(); + ii.MinDistAbs = startMinDist; + int LocSampleNum = std::min(ap.SampleNum, int(movvert.size())); + Box3d fixbox; + if (u.Empty()) + fixbox = uv.bbox; + else + fixbox = u.bbox; + for (i = 0; i < LocSampleNum; ++i) { + if (beyondCntVec[i] < maxBeyondCnt) { + if (fixbox.IsIn(movvert[i])) { + 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++; + } + else { + beyondCntVec[i] = maxBeyondCnt + 1; + } + } + } // 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; + } + } + Matrix44d newout; + switch (ap.MatchMode){ + case AlignPair::Param::MMSimilarity: + vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(newout, pfix, Pmov); + break; + case AlignPair::Param::MMRigid: + ComputeRigidMatchMatrix(pfix, Pmov, newout); + break; + default: + status = UNKNOWN_MODE; + ii.Time = clock(); + as.I.push_back(ii); + return false; + } + + //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 * out; + + assert(Pfix.size() == Pmov.size()); + int tts2 = clock(); + ttsearch += tts1 - tts0; + ttleast += tts2 - tts1; + ii.pcl50 = h.Percentile(.5); + ii.pclhi = h.Percentile(float(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 = std::max(ap.MinDistAbs*ap.MinMinDistPerc, std::min(startMinDist, 5.0*h.Percentile(float(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; + } bool InitMov( diff --git a/vcg/complex/algorithms/point_matching_scale.h b/vcg/complex/algorithms/point_matching_scale.h index 7921110f..2acfb2a1 100644 --- a/vcg/complex/algorithms/point_matching_scale.h +++ b/vcg/complex/algorithms/point_matching_scale.h @@ -21,6 +21,9 @@ * * ****************************************************************************/ +#ifndef VCG_POINT_MATCHING_SCALE +#define VCG_POINT_MATCHING_SCALE + #include #include #include @@ -132,3 +135,4 @@ public: } +#endif