From cbcc40a8e2550cfcb65ed63c507bfeb065cf011b Mon Sep 17 00:00:00 2001 From: cignoni Date: Tue, 9 Feb 2016 09:35:43 +0000 Subject: [PATCH] Improved ransac. --- vcg/complex/algorithms/ransac_matching.h | 314 +++++++++++++++++------ 1 file changed, 237 insertions(+), 77 deletions(-) diff --git a/vcg/complex/algorithms/ransac_matching.h b/vcg/complex/algorithms/ransac_matching.h index 2e4fa966..69ca3658 100644 --- a/vcg/complex/algorithms/ransac_matching.h +++ b/vcg/complex/algorithms/ransac_matching.h @@ -24,13 +24,13 @@ #ifndef RANSAC_MATCHING_H #define RANSAC_MATCHING_H #include -#include +#include +#include #include #include namespace vcg { - template class BaseFeature { @@ -39,12 +39,27 @@ public: typename MeshType::VertexType *_v; typename MeshType::CoordType P() {return _v->cP();} }; + + template class BaseFeatureSet { public: typedef BaseFeature FeatureType; typedef typename MeshType::VertexType VertexType; + typedef typename MeshType::ScalarType ScalarType; + + class Param + { + public: + Param() + { + featureSampleRatio = 0.5; // the number of feature that we choose on the total number of samples. + } + + ScalarType featureSampleRatio; + }; + std::vector fixFeatureVec; std::vector movFeatureVec; @@ -52,12 +67,14 @@ public: FeatureType &ff(int i) { return fixFeatureVec[i]; } FeatureType &mf(int i) { return movFeatureVec[i]; } int ffNum() const { return fixFeatureVec.size(); } + int mfNum() const { return movFeatureVec.size(); } void Init(MeshType &fix, MeshType &mov, - std::vector &fixSampleVec, std::vector &movSampleVec) + std::vector &fixSampleVec, std::vector &movSampleVec, + Param &fpp) { - this->fixFeatureVec.resize(fixSampleVec.size()/20); - for(int i=0;ifixFeatureVec.resize(fixSampleVec.size()*fpp.featureSampleRatio); + for(int i=0;ifixFeatureVec[i]._v = fixSampleVec[i]; this->movFeatureVec.resize(movSampleVec.size()/20); @@ -69,55 +86,134 @@ public: } // Returns the indexes of all the fix features matching a given one (from mov usually) - void getMatchingFeatureVec(FeatureType &q, vector &mfiVec) + // remember that the idea is that + // we are aliging mov (that could be a single map) to fix (that could be a set of already aligned maps) + void getMatchingFixFeatureVec(FeatureType &q, vector &ffiVec, size_t maxMatchingFeature) { - mfiVec.resize(movFeatureVec.size()); + ffiVec.resize(std::min(fixFeatureVec.size(),maxMatchingFeature)); - for(int i=0;i class NDFeature { public: - typedef typename MeshType::ScalarType ScalarType; - + NDFeature():_v(0) {} typename MeshType::VertexType *_v; - typename MeshType::CoordType nd; // - + typename MeshType::CoordType nd; // typename MeshType::CoordType P() {return _v->cP();} - - static void EvalNormalVariation(MeshType &m, ScalarType dist) - { - tri::UpdateNormal::PerVertexNormalized(m); - - VertexConstDataWrapper ww(m); - KdTree tree(ww); - - for(int i=0;i ptIndVec; - std::vector sqDistVec; - tree.doQueryDist(m.vert[i].P(),dist, ptIndVec, sqDistVec); - ScalarType varSum=0; - for(int j=0;j::PerVertexQualityGray(m,0,0); - } - }; +template +class NDFeatureSet +{ +public: + typedef NDFeature FeatureType; + typedef typename MeshType::VertexType VertexType; + typedef typename MeshType::CoordType CoordType; + typedef typename MeshType::ScalarType ScalarType; + + class Param + { + public: + Param() + { + levAbs=CoordType(0,0,0); + levPerc[0] = 0.01; + levPerc[1] = levPerc[0]*2.0; + levPerc[2] = levPerc[1]*2.0; + } - - + CoordType levPerc; + CoordType levAbs; + }; + + std::vector fixFeatureVec; + std::vector movFeatureVec; + KdTree *fixFeatureTree; + + FeatureType &ff(int i) { return fixFeatureVec[i]; } + FeatureType &mf(int i) { return movFeatureVec[i]; } + int ffNum() const { return fixFeatureVec.size(); } + int mfNum() const { return movFeatureVec.size(); } + + void Init(MeshType &fix, MeshType &mov, + std::vector &fixSampleVec, std::vector &movSampleVec, Param &pp) + { + ScalarType dd = std::max(fix.bbox.Diag(),mov.bbox.Diag()); + if(pp.levAbs == CoordType(0,0,0)) + pp.levAbs= pp.levPerc * dd; + + BuildNDFeatureVector(fix,fixSampleVec,pp.levAbs,fixFeatureVec); + BuildNDFeatureVector(mov,movSampleVec,pp.levAbs,movFeatureVec); + + ConstDataWrapper cdw( &(fixFeatureVec[0].nd), fixFeatureVec.size(), sizeof(FeatureType)); + fixFeatureTree = new KdTree(cdw); + + printf("Generated %i ND Features on Fix\n",this->fixFeatureVec.size()); + printf("Generated %i ND Features on Mov\n",this->movFeatureVec.size()); + } + + + static void BuildNDFeatureVector(MeshType &m, std::vector &sampleVec, Point3f &distLev, std::vector &featureVec ) + { + tri::UpdateNormal::PerVertexNormalized(m); + tri::Smooth::VertexNormalLaplacian(m,10); + + VertexConstDataWrapper ww(m); + KdTree tree(ww); + featureVec.resize(sampleVec.size()); + const Point3f sqDistLev(distLev[0]*distLev[0], distLev[1]*distLev[1], distLev[2]*distLev[2]); + for(int i=0;i ptIndVec; + std::vector sqDistVec; + tree.doQueryDist(sampleVec[i]->P(), distLev[2], ptIndVec, sqDistVec); + Point3f varSum(0,0,0); + Point3i varCnt(0,0,0); + + for(int j=0;j &ffiVec, int maxNum) +{ + ffiVec.clear(); + typename KdTree::PriorityQueue pq; + this->fixFeatureTree->doQueryK(q.nd,maxNum,pq); + for(int i=0;i class RansacFramework { typedef typename FeatureSetType::FeatureType FeatureType; + typedef typename FeatureSetType::Param FeatureParam; + typedef typename MeshType::CoordType CoordType; typedef typename MeshType::BoxType BoxType; typedef typename MeshType::ScalarType ScalarType; @@ -153,6 +251,8 @@ public: inlierDistanceThrPerc = 1.5; // the distance between a transformed mov sample and the corresponding on fix should be 1.5 * sampling dist. congruenceThrPerc = 2.0; // the distance between two matching features must be within 2.0 * sampling distance minFeatureDistancePerc = 4.0; // the distance between two chosen features must be at least 4.0 * sampling distance + maxMatchingFeatureNum = 100; + areaThrPerc = 20.0; } ScalarType inlierRatioThr; @@ -161,8 +261,10 @@ public: ScalarType minFeatureDistancePerc; ScalarType samplingRadiusPerc; ScalarType samplingRadiusAbs; + ScalarType areaThrPerc; int iterMax; int evalSize; + int maxMatchingFeatureNum; ScalarType inlierSquareThr() const { return pow(samplingRadiusAbs* inlierDistanceThrPerc,2); } }; @@ -231,9 +333,45 @@ public: return false; } + + + // Scan the feature set of + void EvaluateFeature(int testSize, const char *filename, Param &pp) + { +// VertexConstDataWrapper ww(fixM); +// KdTree(ww) mTree; + MeshType tmpM; + int neededSizeSum=0; + int foundCnt=0; + printf("Testing Feature size %i\n",testSize); + for(int i=0;i closeFeatureVec; + FS.getMatchingFixFeatureVec(FS.mf(i), closeFeatureVec, j); + for(int k=0; k::AddVertex(tmpM, FS.mf(i).P()); + tmpM.vert.back().Q() = neededSize; + neededSizeSum+=neededSize; + if(neededSize::PerVertexQualityRamp(tmpM); + tri::io::ExporterPLY::Save(tmpM,filename, tri::io::Mask::IOM_VERTCOLOR + tri::io::Mask::IOM_VERTQUALITY); + printf("Found %i / %i Average Needed Size %5.2f on %i\n",foundCnt,FS.mfNum(), float(neededSizeSum)/FS.mfNum(),testSize); + + } + // The main loop. - // Choose three points on fix that make a scalene triangle - // and search on mov three other points with matchng distances + // Choose three points on mov that make a scalene triangle + // and search on fix three other points with matchng distances void Process_SearchEvaluateTriple (vector &cVec, Param &pp) { @@ -241,67 +379,74 @@ public: // ScalarType congruenceEps = pow(pp.samplingRadiusAbs * pp.congruenceThrPerc,2.0f); ScalarType congruenceEps = pp.samplingRadiusAbs * pp.congruenceThrPerc; ScalarType minFeatureDistEps = pp.samplingRadiusAbs * pp.minFeatureDistancePerc; + ScalarType minAreaThr = pp.samplingRadiusAbs * pp.samplingRadiusAbs *pp.areaThrPerc; printf("Starting search congruenceEps = samplingRadiusAbs * 3.0 = %6.2f \n",congruenceEps); int iterCnt=0; while ( (iterCnt < pp.iterMax) && (cVec.size()<100) ) { Candidate c; - // Choose a random pair of features from fix - c.fixInd[0] = rnd.generate(FS.ffNum()); - c.fixInd[1] = rnd.generate(FS.ffNum()); - ScalarType d01 = Distance(FS.ff(c.fixInd[0]).P(),FS.ff(c.fixInd[1]).P()); + // Choose a random pair of features from mov + c.movInd[0] = rnd.generate(FS.mfNum()); + c.movInd[1] = rnd.generate(FS.mfNum()); + ScalarType d01 = Distance(FS.mf(c.movInd[0]).P(),FS.mf(c.movInd[1]).P()); if( d01 > minFeatureDistEps ) { - c.fixInd[2] = rnd.generate(FS.ffNum()); - ScalarType d02=Distance(FS.ff(c.fixInd[0]).P(),FS.ff(c.fixInd[2]).P()); - ScalarType d12=Distance(FS.ff(c.fixInd[1]).P(),FS.ff(c.fixInd[2]).P()); - + c.movInd[2] = rnd.generate(FS.mfNum()); + ScalarType d02=Distance(FS.mf(c.movInd[0]).P(),FS.mf(c.movInd[2]).P()); + ScalarType d12=Distance(FS.mf(c.movInd[1]).P(),FS.mf(c.movInd[2]).P()); + ScalarType areaTri = DoubleArea(Triangle3(FS.mf(c.movInd[0]).P(), FS.mf(c.movInd[1]).P(), FS.mf(c.movInd[2]).P() )); if( ( d02 > minFeatureDistEps ) && // Sample are sufficiently distant ( d12 > minFeatureDistEps ) && + ( areaTri > minAreaThr) && ( fabs(d01-d02) > congruenceEps ) && // and they make a scalene triangle ( fabs(d01-d12) > congruenceEps ) && ( fabs(d12-d02) > congruenceEps ) ) { // Find a congruent triple on mov - printf("Starting search of a [%i] congruent triple for %4i %4i %4i - %6.2f %6.2f %6.2f\n",iterCnt,c.fixInd[0],c.fixInd[1],c.fixInd[2],d01,d02,d12); + printf("Starting search of a [%i] congruent triple for %4i %4i %4i - %6.2f %6.2f %6.2f\n", + iterCnt,c.movInd[0],c.movInd[1],c.movInd[2],d01,d02,d12); // As a first Step we ask for three vectors of matching features; - std::vector movFeatureVec0; - FS.getMatchingFeatureVec(FS.ff(c.fixInd[0]), movFeatureVec0); - std::vector movFeatureVec1; - FS.getMatchingFeatureVec(FS.ff(c.fixInd[1]), movFeatureVec1); - std::vector movFeatureVec2; - FS.getMatchingFeatureVec(FS.ff(c.fixInd[2]), movFeatureVec2); + std::vector fixFeatureVec0; + FS.getMatchingFixFeatureVec(FS.mf(c.movInd[0]), fixFeatureVec0,pp.maxMatchingFeatureNum); + std::vector fixFeatureVec1; + FS.getMatchingFixFeatureVec(FS.mf(c.movInd[1]), fixFeatureVec1,pp.maxMatchingFeatureNum); + std::vector fixFeatureVec2; + FS.getMatchingFixFeatureVec(FS.mf(c.movInd[2]), fixFeatureVec2,pp.maxMatchingFeatureNum); int congrNum=0; int congrGoodNum=0; - for(int i=0;i100) break; - c.movInd[0]=movFeatureVec0[i]; - for(int j=0;j100) break; - c.movInd[1]=movFeatureVec1[j]; - ScalarType m01 = Distance(FS.mf(c.movInd[0]).P(),FS.mf(c.movInd[1]).P()); + c.fixInd[1]=fixFeatureVec1[j]; + ScalarType m01 = Distance(FS.ff(c.fixInd[0]).P(),FS.ff(c.fixInd[1]).P()); if( (fabs(m01-d01)100) break; - c.movInd[2]=movFeatureVec2[k]; - ScalarType m02=Distance(FS.mf(c.movInd[0]).P(),FS.mf(c.movInd[2]).P()); - ScalarType m12=Distance(FS.mf(c.movInd[1]).P(),FS.mf(c.movInd[2]).P()); + c.fixInd[2]=fixFeatureVec2[k]; + ScalarType m02=Distance(FS.ff(c.fixInd[0]).P(),FS.ff(c.fixInd[2]).P()); + ScalarType m12=Distance(FS.ff(c.fixInd[1]).P(),FS.ff(c.fixInd[2]).P()); if( (fabs(m02-d02) pp.inlierRatioThr ){ - printf("- - Found %i th good congruent triple %i %i %i -- %f / %i \n", cVec.size(), c.movInd[0],c.movInd[1],c.movInd[2],c.err(),pp.evalSize); + printf("- - Found %lu th good congruent triple %i %i %i -- %f / %i \n", cVec.size(), c.movInd[0],c.movInd[1],c.movInd[2],c.err(),pp.evalSize); +// printf(" - %4.3f %4.3f %4.3f - %4.3f %4.3f %4.3f \n", +// FS.ff(c.fixInd[0]).nd[0], FS.ff(c.fixInd[0]).nd[1], FS.ff(c.fixInd[0]).nd[2], +// FS.mf(c.movInd[0]).nd[0], FS.mf(c.movInd[0]).nd[1],FS.mf(c.movInd[0]).nd[2]); + ++congrGoodNum; cVec.push_back(c); } @@ -316,7 +461,7 @@ public: ++iterCnt; } // end While - printf("Found %i candidates \n",cVec.size()); + printf("Found %lu candidates \n",cVec.size()); sort(cVec.begin(),cVec.end()); printf("best candidate %f \n",cVec[0].err()); @@ -333,25 +478,37 @@ public: } // end Process - int EvaluateMatrix(Candidate &c, Param &pp) + void EvaluateMatrix(Candidate &c, Param &pp) { c.inlierNum=0; c.evalSize=pp.evalSize; ScalarType sqThr = pp.inlierSquareThr(); Distribution H; - for(int i=0;i::iterator pi=movConsensusVec.begin(); + + for(int j=0;j<2;++j) { - Point3f qp = c.Tr*movConsensusVec[i]; - uint ind; - ScalarType squareDist; - consensusTree->doQueryClosest(qp,ind,squareDist); - if(squareDist < sqThr) - ++c.inlierNum; + for(int i=0;idoQueryClosest(qp,ind,squareDist); + if(squareDist < sqThr) + ++c.inlierNum; + ++pi; + } + if((j==0) && (c.inlierNum < mid/10)) + { + c.inlierNum *=2; + return; + } } } - int DumpInlier(MeshType &m, Candidate &c, Param &pp) + void DumpInlier(MeshType &m, Candidate &c, Param &pp) { ScalarType sqThr = pp.inlierSquareThr(); for(int i=0;i::PerVertexNormalizedPerFaceNormalized(fixM); + tri::UpdateNormal::PerVertexNormalizedPerFaceNormalized(movM); + // First a bit of Sampling typedef tri::TrivialPointerSampler BaseSampler; typename tri::SurfaceSampling::PoissonDiskParam pdp; @@ -427,7 +587,7 @@ void Init(MeshType &fixM, MeshType &movM, Param &pp) for(int i=0;imovConsensusVec.push_back(movSampleVec[i]->P()); - FS.Init(fixM, movM, fixSampleVec, movSampleVec); + FS.Init(fixM, movM, fixSampleVec, movSampleVec, fpp); std::random_shuffle(movConsensusVec.begin(),movConsensusVec.end());