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

295
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,43 +89,32 @@ 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;}
}; };
MeshType *P; // mesh from which the coplanar base is selected Param par; /// parameters
MeshType *Q; // mesh where to find the correspondences
std::vector<int> mapsub; // subset of index to the vertices in Q
MeshType *P; // mesh from which the coplanar base is selected
PMesh Invr; // invariants MeshType *Q; // mesh where to find the correspondences
std::vector<int> mapsub; // subset of index to the vertices in Q
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
std::vector<FourPoints> bases; // used bases std::vector<FourPoints> bases; // used bases
ScalarType side; // side ScalarType side; // side
std::vector<VertexType*> ExtB[4]; // selection of vertices "close" to the four point std::vector<VertexType*> ExtB[4]; // selection of vertices "close" to the four point
std::vector<VertexType*> subsetP; // random selection on P std::vector<VertexType*> subsetP; // random selection on P
ScalarType radius; ScalarType radius;
@ -148,7 +126,7 @@ public:
struct EPoint{ struct EPoint{
EPoint(vcg::Point3<ScalarType> _p, int _i):pos(_p),pi(_i){} EPoint(vcg::Point3<ScalarType> _p, int _i):pos(_p),pi(_i){}
vcg::Point3<ScalarType> pos; vcg::Point3<ScalarType> pos;
int pi; //index to R[1|2] int pi; //index to R[1|2]
void GetBBox(vcg::Box3<ScalarType> & b){b.Add(pos);} void GetBBox(vcg::Box3<ScalarType> & b){b.Add(pos);}
}; };
@ -156,8 +134,13 @@ 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;
bool SelectCoplanarBase(); // on P
bool FindCongruent() ; // of base B, on Q, with approximation delta void Init(MeshType &Mov, MeshType &Fix);
bool Align( int L, vcg::Matrix44f & result, vcg::CallBackPos * cb = NULL ); // main function
bool SelectCoplanarBase(); // on P
bool FindCongruent(); // of base B, on Q, with approximation delta
void ComputeR1(); void ComputeR1();
bool IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float & trerr); bool IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float & trerr);
@ -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<
@ -234,66 +212,53 @@ void FourPCS<MeshType>:: Init(MeshType &_P,MeshType &_Q)
assert(ress.size() == 2); assert(ress.size() == 2);
avD+=dists[1]; avD+=dists[1];
} }
avD /=100; // average vertex-vertex distance avD /=100; // average vertex-vertex distance
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(){ {
// choose the inter point distance
ScalarType dtol = side*0.1; //rough implementation
vcg::tri::UpdateBounding<MeshType>::Box(*P); //choose the first two points
// choose the inter point distance // first point random
ScalarType dtol = side*0.1; //rough implementation B[0] = P->vert[ rnd.generate(P->vert.size())].P();
//choose the first two points // second a point at distance d+-dtol
int i = 0,ch; int i;
for(i = 0; i < P->vert.size(); ++i){
// first point random int id = rnd.generate(P->vert.size()-1);
ch = (rand()/(float)RAND_MAX)*(P->vert.size()-2); ScalarType dd = (P->vert[id].P() - B[0]).Norm();
B[0] = P->vert[ch].P(); if( ( dd < side + dtol) && (dd > side - dtol)){
B[1] = P->vert[id].P();
// second a point at distance d+-dtol break;
for(i = 0; i < P->vert.size(); ++i){
int id = rand()/(float)RAND_MAX * (P->vert.size()-1);
ScalarType dd = (P->vert[id].P() - B[0]).Norm();
if( ( dd < side + dtol) && (dd > side - dtol)){
B[1] = P->vert[id].P();
//printf("B[1] %d\n",i);
break;
}
} }
if(i == P->vert.size()) }
return false; if(i == P->vert.size()) return false;
// third point at distance 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) ){
B[2] = P->vert[id].P();
break;
}
}
if(i == P->vert.size()) return false;
// third point at distance side*0.8 from middle way between B[0] and B[1] //fourth point
int best = -1; ScalarType bestv=std::numeric_limits<float>::max(); float cpr = rnd.generate01();
vcg::Point3f middle = (B[0]+B[1])/2.0;
for(i = 0; i < P->vert.size(); ++i){
int id = rand()/(float)RAND_MAX * (P->vert.size()-1);
ScalarType dd = (P->vert[id].P() - middle).Norm();
if( ( dd < side*0.8) ){
best = id;
break;
}
}
if(best == -1)
return false;
B[2] = P->vert[best].P() ;
//printf("B[2] %d\n",best);
//fourt point
float cpr = (rand()/(float)RAND_MAX);
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,12 +310,12 @@ 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)
// printf("%d ",ExtB[i].size()); // printf("%d ",ExtB[i].size());
// printf("\n"); // printf("\n");
return true; return true;
} }
@ -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))
{ {
@ -443,7 +408,7 @@ bool FourPCS<MeshType>::FindCongruent() { // of base B, on Q, with approximation
for(vii = Invr.vert.begin(); vii != Invr.vert.end();++vii,++i) id[vii] = i; for(vii = Invr.vert.begin(); vii != Invr.vert.end();++vii,++i) id[vii] = i;
vcg::tri::UpdateBounding<PMesh>::Box(Invr); vcg::tri::UpdateBounding<PMesh>::Box(Invr);
// printf("Invr size %d\n",Invr.vn); // printf("Invr size %d\n",Invr.vn);
ugrid = new GridType(); ugrid = new GridType();
ugrid->Set(Invr.vert.begin(),Invr.vert.end()); ugrid->Set(Invr.vert.begin(),Invr.vert.end());
@ -524,8 +489,8 @@ bool FourPCS<MeshType>::FindCongruent() { // of base B, on Q, with approximation
printf("n_closests %5d = (An %5d ) + ( Tr %5d ) + (OK) %5d\n",n_closests,acf,trf,n_congr); printf("n_closests %5d = (An %5d ) + ( Tr %5d ) + (OK) %5d\n",n_closests,acf,trf,n_congr);
return done; return done;
// printf("done n_closests %d congr %d in %f s\n ",n_closests,n_congr,(clock()-start)/(float)CLOCKS_PER_SEC); // 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); // printf("angle:%d %d, trasf %d %d\n",ac,acf,tr,trf);
} }
@ -540,7 +505,7 @@ int FourPCS<MeshType>::EvaluateSample(Candidate & fp, CoordType & tp, CoordType
vcg::Point4<ScalarType> np4; vcg::Point4<ScalarType> np4;
np4 = fp.T * vcg::Point4<ScalarType>(np[0],np[1],np[2],0.0); np4 = fp.T * vcg::Point4<ScalarType>(np[0],np[1],np[2],0.0);
np[0] = np4[0]; np[1] = np4[1]; np[2] = np4[2]; np[0] = np4[0]; np[1] = np4[1]; np[2] = np4[2];
v = 0; v = 0;
if(ugridQ.bbox.IsIn(tp)) if(ugridQ.bbox.IsIn(tp))
@ -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,35 +570,38 @@ 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{ {
n_tries = 0; do
do{ {
n_tries++; n_tries = 0;
found = SelectCoplanarBase(); do
} {
while(!found && (n_tries <50)); n_tries++;
if(!found) { found = SelectCoplanarBase();
par.f*=0.9; }
side = P->bbox.Dim()[P->bbox.MaxDim()]*par.f; //rough implementation while(!found && (n_tries <50));
ComputeR1(); if(!found) {
} par.overlap*=0.9;
} while (!found && (par.f >0.1)); side = P->bbox.Dim()[P->bbox.MaxDim()]*par.overlap; //rough implementation
ComputeR1();
}
} while (!found && (par.overlap >0.1));
if(par.f <0.1) { if(par.overlap <0.1) {
printf("FAILED"); printf("FAILED");
return false; return false;
} }
bases.push_back(B); bases.push_back(B);
if(cb) cb(t*100/L,"trying bases"); if(cb) cb(t*100/L,"trying bases");
if(FindCongruent()) if(FindCongruent())
break; break;
} }
if(U.empty()) return false; if(U.empty()) return false;
@ -651,17 +620,11 @@ 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;
} }
} // namespace tri } // namespace tri
} // namespace vcg } // namespace vcg
#endif #endif