manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Heavily refactored 4PCS. 

Some of the things done:
- better random generator
- small optimization (removed O(n) update bb
- exposed parameters 
- renamed for uniformity variables.
This commit is contained in:
Paolo Cignoni 2014-04-17 09:53:33 +00:00
parent 4be7b59a80
commit d5e181b445
1 changed files with 129 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

191
vcg/complex/algorithms/autoalign_4pcs.h
View File

@ -34,12 +34,7 @@ used in the paper pseudocode.
#include <vcg/complex/complex.h> #include <vcg/complex/complex.h>
#include <vcg/space/point_matching.h> #include <vcg/space/point_matching.h>
#include <vcg/complex/algorithms/closest.h> #include <vcg/complex/algorithms/closest.h>
#include <vcg/complex/algorithms/create/resampler.h> #include <vcg/math/random_generator.h>
#include <wrap/io_trimesh/export_ply.h>
// note: temporary (callback.h should be moved inside vcg)
typedef bool AACb( const int pos,const char * str );
namespace vcg{ namespace vcg{
namespace tri{ namespace tri{
@ -68,29 +63,23 @@ public:
struct Param struct Param
{ {
ScalarType delta; // Approximation Level ScalarType delta; // Approximation Level
int feetsize; // how many points in the neighborhood of each of the 4 points int feetSize; // how many points in the neighborhood of each of the 4 points
ScalarType f; // overlap estimation as a percentage 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 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 int scoreAln; // how good must be the alignement to end the process successfully
int n_samples_on_Q; // number of samples on P int n_samples_on_Q; // number of samples on P
int seed;
void Default(){ void Default(){
delta = 0.5; delta = 0.5;
feetsize = 25; feetSize = 25;
f = 0.5; overlap = 0.5;
scoreFeet = 50; scoreFeet = 50;
n_samples_on_Q=500; n_samples_on_Q=500;
scoreAln = n_samples_on_Q/8; scoreAln = n_samples_on_Q/8;
seed =0;
} }
}; };
Param par; /// parameters
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<int,int> struct Couple: public std::pair<int,int>
{ {
Couple(const int & i, const int & j, float d):std::pair<int,int>(i,j),dist(d){} Couple(const int & i, const int & j, float d):std::pair<int,int>(i,j),dist(d){}
@ -100,38 +89,27 @@ public:
int & operator[](const int &i){return (i==0)? first : second;} 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();
}
struct Candidate struct Candidate
{ {
Candidate(){} Candidate(){}
Candidate(FourPoints _p,vcg::Matrix44<ScalarType>_T):p(_p),T(_T){} Candidate(FourPoints _p,vcg::Matrix44<ScalarType>_T):p(_p),T(_T){}
FourPoints p; FourPoints p;
vcg::Matrix44<ScalarType> T; vcg::Matrix44<ScalarType> T;
ScalarType err;
int score; int score;
int base; // debug: for which base int base; // debug: for which base
inline bool operator <(const Candidate & o) const {return score > o.score;} inline bool operator <(const Candidate & o) const {return score > o.score;}
}; };
Param par; /// parameters
MeshType *P; // mesh from which the coplanar base is selected MeshType *P; // mesh from which the coplanar base is selected
MeshType *Q; // mesh where to find the correspondences MeshType *Q; // mesh where to find the correspondences
std::vector<int> mapsub; // subset of index to the vertices in Q std::vector<int> mapsub; // subset of index to the vertices in Q
PMesh Invr; // invariants PMesh Invr; // invariants
math::MarsenneTwisterRNG rnd;
std::vector< Candidate > U; std::vector< Candidate > U;
Candidate winner;
int iwinner; // winner == U[iwinner] int iwinner; // winner == U[iwinner]
FourPoints B; // coplanar base FourPoints B; // coplanar base
@ -156,6 +134,11 @@ public:
vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridQ; vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridQ;
vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridP; vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridP;
void Init(MeshType &Mov, MeshType &Fix);
bool Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb = NULL ); // main function
bool SelectCoplanarBase(); // on P bool SelectCoplanarBase(); // on P
bool FindCongruent(); // of base B, on Q, with approximation delta bool FindCongruent(); // of base B, on Q, with approximation delta
void ComputeR1(); void ComputeR1();
@ -165,6 +148,14 @@ public:
void EvaluateAlignment(Candidate & fp); void EvaluateAlignment(Candidate & fp);
void TestAlignment(Candidate & fp); void TestAlignment(Candidate & fp);
/* 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();
}
/* debug tools */ /* debug tools */
public: public:
std::vector<vcg::Matrix44f> allTr;// tutte le trasformazioni provate std::vector<vcg::Matrix44f> allTr;// tutte le trasformazioni provate
@ -181,48 +172,35 @@ public:
void FinishDebug(){ void FinishDebug(){
fclose(db); fclose(db);
} }
//void SaveALN(char * name,vcg::Matrix44f mat ){
// FILE * o = fopen(name,"w");
// fprintf(o,"2\n%s\n#\n",namemesh1);
// for(int i = 0 ; i < 4; ++i)
// fprintf(o,"%f %f %f %f\n",mat[i][0],mat[i][1],mat[i][2],mat[i][3]);
// fprintf(o,"%s\n#\n",namemesh2);
// fprintf(o,"1.0 0.0 0.0 0.0 \n");
// fprintf(o,"0.0 1.0 0.0 0.0 \n");
// fprintf(o,"0.0 0.0 1.0 0.0 \n");
// fprintf(o,"0.0 0.0 0.0 1.0 \n");
// fclose(o);
//}
}; };
template <class MeshType> template <class MeshType>
void FourPCS<MeshType>:: Init(MeshType &_P,MeshType &_Q) void FourPCS<MeshType>:: Init(MeshType &_movP,MeshType &_fixQ)
{ {
P = &_P;Q=&_Q; P = &_movP;
Q = &_fixQ;
tri::UpdateBounding<MeshType>::Box(*P);
if(par.seed==0) rnd.initialize(time(0));
else rnd.initialize(par.seed);
ugridQ.Set(Q->vert.begin(),Q->vert.end()); ugridQ.Set(Q->vert.begin(),Q->vert.end());
ugridP.Set(P->vert.begin(),P->vert.end()); ugridP.Set(P->vert.begin(),P->vert.end());
float ratio = std::min<int>(Q->vert.size(),par.n_samples_on_Q) / (float) Q->vert.size(); float ratio = std::min<int>(Q->vert.size(),par.n_samples_on_Q) / (float) Q->vert.size();
for(int vi = 0; vi < Q->vert.size(); ++vi) for(int vi = 0; vi < Q->vert.size(); ++vi)
if(rand()/(float) RAND_MAX < ratio) if(rnd.generate01() < ratio)
mapsub.push_back(vi); mapsub.push_back(vi);
for(int vi = 0; vi < P->vert.size(); ++vi) for(int vi = 0; vi < P->vert.size(); ++vi)
if(rand()/(float) RAND_MAX < ratio) if(rnd.generate01() < ratio)
subsetP.push_back(&P->vert[vi]); subsetP.push_back(&P->vert[vi]);
// estimate neigh distance // estimate neigh distance
float avD = 0.0; float avD = 0.0;
for(int i = 0 ; i < 100; ++i){ for(int i = 0 ; i < 100; ++i){
int ri = rand()/(float) RAND_MAX * Q->vert.size() -1; int ri = rnd.generate(Q->vert.size());
std::vector< CoordType > samples,d_samples; std::vector< CoordType > samples;
std::vector<ScalarType > dists; std::vector<ScalarType > dists;
std::vector<VertexType* > ress; std::vector<VertexType* > ress;
vcg::tri::GetKClosestVertex< vcg::tri::GetKClosestVertex<
@ -238,62 +216,49 @@ void FourPCS<MeshType>:: Init(MeshType &_P,MeshType &_Q)
avD /= sqrt(ratio); avD /= sqrt(ratio);
par.delta = avD * par.delta; par.delta = avD * par.delta;
side = P->bbox.Dim()[P->bbox.MaxDim()]*par.f; //rough implementation side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation
} }
template <class MeshType> template <class MeshType>
bool bool FourPCS<MeshType>::SelectCoplanarBase()
FourPCS<MeshType>::SelectCoplanarBase(){ {
vcg::tri::UpdateBounding<MeshType>::Box(*P);
// choose the inter point distance // choose the inter point distance
ScalarType dtol = side*0.1; //rough implementation ScalarType dtol = side*0.1; //rough implementation
//choose the first two points //choose the first two points
int i = 0,ch;
// first point random // first point random
ch = (rand()/(float)RAND_MAX)*(P->vert.size()-2); B[0] = P->vert[ rnd.generate(P->vert.size())].P();
B[0] = P->vert[ch].P();
// second a point at distance d+-dtol // second a point at distance d+-dtol
int i;
for(i = 0; i < P->vert.size(); ++i){ for(i = 0; i < P->vert.size(); ++i){
int id = rand()/(float)RAND_MAX * (P->vert.size()-1); int id = rnd.generate(P->vert.size()-1);
ScalarType dd = (P->vert[id].P() - B[0]).Norm(); ScalarType dd = (P->vert[id].P() - B[0]).Norm();
if( ( dd < side + dtol) && (dd > side - dtol)){ if( ( dd < side + dtol) && (dd > side - dtol)){
B[1] = P->vert[id].P(); B[1] = P->vert[id].P();
//printf("B[1] %d\n",i);
break; break;
} }
} }
if(i == P->vert.size()) if(i == P->vert.size()) return false;
return false;
// third point at distance side*0.8 from middle way between B[0] and B[1] // third point at distance side*0.8 from middle way between B[0] and B[1]
int best = -1; ScalarType bestv=std::numeric_limits<float>::max(); const vcg::Point3f middle = (B[0]+B[1])/2.0;
vcg::Point3f middle = (B[0]+B[1])/2.0;
for(i = 0; i < P->vert.size(); ++i){ for(i = 0; i < P->vert.size(); ++i){
int id = rand()/(float)RAND_MAX * (P->vert.size()-1); int id = rnd.generate(P->vert.size()-1);
ScalarType dd = (P->vert[id].P() - middle).Norm(); ScalarType dd = (P->vert[id].P() - middle).Norm();
if( ( dd < side*0.8) ){ if( ( dd < side*0.8) ){
best = id; B[2] = P->vert[id].P();
break; break;
} }
} }
if(best == -1) if(i == P->vert.size()) return false;
return false;
B[2] = P->vert[best].P() ;
//printf("B[2] %d\n",best);
//fourt point //fourth point
float cpr = (rand()/(float)RAND_MAX); float cpr = rnd.generate01();
vcg::Point3f crossP = B[0] *(1-cpr)+B[1]*cpr; vcg::Point3f crossP = B[0] *(1-cpr)+B[1]*cpr;
CoordType B4 = B[2]+(crossP-B[2]).Normalize()*side; CoordType B4 = B[2]+(crossP-B[2]).Normalize()*side;
CoordType n = ((B[0]-B[1]).normalized() ^ (B[2]-B[1]).normalized()).normalized(); CoordType n = ((B[0]-B[1]).normalized() ^ (B[2]-B[1]).normalized()).normalized();
VertexType * v =0;
ScalarType radius = dtol; ScalarType radius = dtol;
std::vector<typename MeshType::VertexType*> closests; std::vector<typename MeshType::VertexType*> closests;
@ -310,17 +275,17 @@ FourPCS<MeshType>::SelectCoplanarBase(){
if(closests.empty()) if(closests.empty())
return false; return false;
best = -1; bestv=std::numeric_limits<float>::max(); int bestInd = -1; ScalarType bestv=std::numeric_limits<float>::max();
for(i = 0; i <closests.size(); ++i){ for(i = 0; i <closests.size(); ++i){
ScalarType dist_from_plane = fabs((closests[i]->P() - B[1]).normalized().dot(n)); ScalarType dist_from_plane = fabs((closests[i]->P() - B[1]).normalized().dot(n));
if( dist_from_plane < bestv){ if( dist_from_plane < bestv){
bestv = dist_from_plane; bestv = dist_from_plane;
best = i; bestInd = i;
} }
} }
if(bestv >dtol) if(bestv >dtol)
return false; return false;
B[3] = closests[best]->P(); 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()));
@ -336,7 +301,7 @@ FourPCS<MeshType>::SelectCoplanarBase(){
return false; return false;
radius =side*0.5; radius =side*0.5;
std::vector< CoordType > samples,d_samples; std::vector< CoordType > samples;
std::vector<ScalarType > dists; std::vector<ScalarType > dists;
for(int i = 0 ; i< 4; ++i){ for(int i = 0 ; i< 4; ++i){
@ -345,7 +310,7 @@ FourPCS<MeshType>::SelectCoplanarBase(){
vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >, vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >,
std::vector<VertexType*>, std::vector<VertexType*>,
std::vector<ScalarType>, std::vector<ScalarType>,
std::vector< CoordType > >(*P,ugridP, par.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) //for(int i = 0 ; i< 4; ++i)
@ -385,11 +350,11 @@ bool FourPCS<MeshType>::IsTransfCongruent(FourPoints fp, vcg::Matrix44<ScalarTyp
template <class MeshType> template <class MeshType>
void void
FourPCS<MeshType>::ComputeR1(){ FourPCS<MeshType>::ComputeR1()
int vi,vj; {
R1.clear(); R1.clear();
int start = clock(); for(int vi = 0; vi < mapsub.size(); ++vi)
for(vi = 0; vi < mapsub.size(); ++vi) for(vj = vi; vj < mapsub.size(); ++vj){ for(int vj = vi; vj < mapsub.size(); ++vj){
ScalarType d = ((Q->vert[mapsub[vi]]).P()-(Q->vert[mapsub[vj]]).P()).Norm(); ScalarType d = ((Q->vert[mapsub[vi]]).P()-(Q->vert[mapsub[vj]]).P()).Norm();
if( (d < side+par.delta)) if( (d < side+par.delta))
{ {
@ -559,8 +524,11 @@ int FourPCS<MeshType>::EvaluateSample(Candidate & fp, CoordType & tp, CoordType
*/ */
if(v!=0) if(v!=0)
if( v->N().dot(np) - cosAngle >0) return 1; else return -1; {
if( v->N().dot(np) - cosAngle >0) return 1;
else return -1;
}
else return 0;
} }
@ -579,8 +547,7 @@ FourPCS<MeshType>::EvaluateAlignment(Candidate & fp){
} }
template <class MeshType> template <class MeshType>
void void FourPCS<MeshType>::TestAlignment(Candidate & fp){
FourPCS<MeshType>::TestAlignment(Candidate & fp){
radius = par.delta; radius = par.delta;
int n_delta_close = 0; int n_delta_close = 0;
for(uint j = 0; j < subsetP.size();++j){ for(uint j = 0; j < subsetP.size();++j){
@ -594,9 +561,8 @@ FourPCS<MeshType>::TestAlignment(Candidate & fp){
template <class MeshType> template <class MeshType>
bool bool FourPCS<MeshType>:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb )
FourPCS<MeshType>:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb ){ // main loop {
int bestv = 0; int bestv = 0;
bool found; bool found;
int n_tries = 0; int n_tries = 0;
@ -604,28 +570,31 @@ FourPCS<MeshType>:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos *
if(L==0) if(L==0)
{ {
L = (log(1.0-0.9) / log(1.0-pow((float)par.f,3.f)))+1; L = (log(1.0-0.9) / log(1.0-pow((float)par.overlap,3.f)))+1;
printf("using %d bases\n",L); printf("using %d bases\n",L);
} }
ComputeR1(); ComputeR1();
for(int t = 0; t < L; ++t ){ for(int t = 0; t < L; ++t )
do{ {
do
{
n_tries = 0; n_tries = 0;
do{ do
{
n_tries++; n_tries++;
found = SelectCoplanarBase(); found = SelectCoplanarBase();
} }
while(!found && (n_tries <50)); while(!found && (n_tries <50));
if(!found) { if(!found) {
par.f*=0.9; par.overlap*=0.9;
side = P->bbox.Dim()[P->bbox.MaxDim()]*par.f; //rough implementation side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation
ComputeR1(); ComputeR1();
} }
} while (!found && (par.f >0.1)); } while (!found && (par.overlap >0.1));
if(par.f <0.1) { if(par.overlap <0.1) {
printf("FAILED"); printf("FAILED");
return false; return false;
} }
@ -651,14 +620,8 @@ FourPCS<MeshType>:: Align( int L, vcg::Matrix44f & result, vcg::CallBackPos *
} }
} }
printf("Best score: %d \n", bestv); result = U[iwinner].T;
winner = U[iwinner];
result = winner.T;
// deallocations
Invr.Clear(); Invr.Clear();
return true; return true;
} }