Cleaned up a bit. Only a bit.
This commit is contained in:
parent
17f61b2774
commit
391e376be7
|
|
@ -1,5 +1,3 @@
|
|||
#ifndef _4PCS_
|
||||
#define _4PCS_
|
||||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
|
|
@ -22,6 +20,9 @@
|
|||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
#ifndef _AUTOALIGN_4PCS_H_
|
||||
#define _AUTOALIGN_4PCS_H_
|
||||
|
||||
/**
|
||||
implementation of the 4PCS method from the paper:
|
||||
"4-Points Congruent Sets for Robust Pairwise Surface Registration"
|
||||
|
|
@ -29,78 +30,66 @@ D.Aiger, N.Mitra D.Cohen-Or, SIGGRAPH 2008
|
|||
ps: the name of the variables are out of vcg standard but like the one
|
||||
used in the paper pseudocode.
|
||||
*/
|
||||
#include <vcg/space/point3.h>
|
||||
#include <vcg/space/point4.h>
|
||||
#include <vcg/space/line3.h>
|
||||
#include <vcg/space/plane3.h>
|
||||
|
||||
#include <vcg/space/point_matching.h>
|
||||
#include <vcg/space/index/grid_static_ptr.h>
|
||||
#include <vcg/complex/algorithms/closest.h>
|
||||
#include <vcg/complex/algorithms/update/bounding.h>
|
||||
|
||||
#include <vcg/simplex/vertex/base.h>
|
||||
#include <vcg/simplex/face/base.h>
|
||||
#include <vcg/complex/complex.h>
|
||||
#include <vcg/complex/algorithms/stat.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 tri{
|
||||
namespace tri{
|
||||
|
||||
template <class MeshType>
|
||||
class FourPCS {
|
||||
public:
|
||||
/* mesh only for using spatial indexing functions (to remove) */
|
||||
/* mesh only for using spatial indexing functions (to remove) */
|
||||
class PVertex; // dummy prototype never used
|
||||
class PFace;
|
||||
|
||||
class PUsedTypes: public vcg::UsedTypes < vcg::Use<PVertex>::template AsVertexType,
|
||||
vcg::Use<PFace >::template AsFaceType >{};
|
||||
|
||||
|
||||
class PVertex : public vcg::Vertex< PUsedTypes,vcg::vertex::BitFlags,vcg::vertex::Coord3f ,vcg::vertex::Mark>{};
|
||||
/*same as for the vertes */
|
||||
class PFace : public vcg::Face< PUsedTypes> {};
|
||||
/*the mesh is a container of vertices and a container of faces */
|
||||
class PMesh : public vcg::tri::TriMesh< std::vector<PVertex>, std::vector<PFace> > {};
|
||||
class PMesh : public vcg::tri::TriMesh< std::vector<PVertex>, std::vector<PFace> > {};
|
||||
|
||||
typedef typename MeshType::ScalarType ScalarType;
|
||||
typedef typename MeshType::CoordType CoordType;
|
||||
typedef typename MeshType::VertexIterator VertexIterator;
|
||||
typedef typename MeshType::VertexType VertexType;
|
||||
typedef vcg::Point4< vcg::Point3<ScalarType> > FourPoints;
|
||||
typedef vcg::GridStaticPtr<typename PMesh::VertexType, ScalarType > GridType;
|
||||
typedef typename MeshType::ScalarType ScalarType;
|
||||
typedef typename MeshType::CoordType CoordType;
|
||||
typedef typename MeshType::VertexIterator VertexIterator;
|
||||
typedef typename MeshType::VertexType VertexType;
|
||||
typedef vcg::Point4< vcg::Point3<ScalarType> > FourPoints;
|
||||
typedef vcg::GridStaticPtr<typename PMesh::VertexType, ScalarType > GridType;
|
||||
|
||||
/* class for Parameters */
|
||||
struct Parameters{
|
||||
ScalarType delta;
|
||||
int feetsize; // how many points in the neighborhood of each of the 4 points
|
||||
ScalarType f; // overlapping estimation
|
||||
int scoreFeet, // how many of the feetsize points must match (max feetsize*4) to try an early interrupt
|
||||
scoreAln; // how good must be the alignement to end the process successfully
|
||||
/* class for Parameters */
|
||||
struct Parameters
|
||||
{
|
||||
ScalarType delta;
|
||||
int feetsize; // how many points in the neighborhood of each of the 4 points
|
||||
ScalarType f; // overlap estimation
|
||||
int scoreFeet, // how many of the feetsize points must match (max feetsize*4) to try an early interrupt
|
||||
scoreAln; // how good must be the alignement to end the process successfully
|
||||
|
||||
void Default(){
|
||||
delta = 0.5;
|
||||
feetsize = 25;
|
||||
f = 0.5;
|
||||
scoreFeet = 50;
|
||||
scoreAln = 200;
|
||||
}
|
||||
};
|
||||
void Default(){
|
||||
delta = 0.5;
|
||||
feetsize = 25;
|
||||
f = 0.5;
|
||||
scoreFeet = 50;
|
||||
scoreAln = 200;
|
||||
}
|
||||
};
|
||||
|
||||
Parameters prs; /// parameters
|
||||
Parameters prs; /// parameters
|
||||
|
||||
public:
|
||||
void Init(MeshType &_P,MeshType &_Q);
|
||||
bool Align( int L, vcg::Matrix44f & result, AACb * 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(float d):std::pair<int,int>(0,0),dist(d){}
|
||||
float dist;
|
||||
|
|
@ -110,7 +99,6 @@ private:
|
|||
|
||||
|
||||
|
||||
|
||||
/* 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)
|
||||
{
|
||||
|
|
@ -122,7 +110,7 @@ private:
|
|||
|
||||
|
||||
struct CandiType{
|
||||
CandiType(){};
|
||||
CandiType(){}
|
||||
CandiType(FourPoints _p,vcg::Matrix44<ScalarType>_T):p(_p),T(_T){}
|
||||
FourPoints p;
|
||||
vcg::Matrix44<ScalarType> T;
|
||||
|
|
@ -167,13 +155,8 @@ private:
|
|||
vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridQ;
|
||||
vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType > ugridP;
|
||||
|
||||
//FILE * f;
|
||||
|
||||
//private:
|
||||
bool SelectCoplanarBase(); // on P
|
||||
bool FindCongruent() ; // of base B, on Q, with approximation delta
|
||||
|
||||
//private:
|
||||
void ComputeR1R2(ScalarType d1,ScalarType d2);
|
||||
|
||||
bool IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float & trerr);
|
||||
|
|
@ -220,21 +203,18 @@ FourPCS<MeshType>:: Init(MeshType &_P,MeshType &_Q){
|
|||
P = &_P;Q=&_Q;
|
||||
ugridQ.Set(Q->vert.begin(),Q->vert.end());
|
||||
ugridP.Set(P->vert.begin(),P->vert.end());
|
||||
int vi;
|
||||
// float areaP = vcg::tri::Stat<MeshType>::ComputeMeshArea(*P);
|
||||
// float areaQ = vcg::tri::Stat<MeshType>::ComputeMeshArea(*Q);
|
||||
|
||||
float ratio = 800 / (float) Q->vert.size();
|
||||
for(vi = 0; vi < Q->vert.size(); ++vi)
|
||||
float ratio = 800 / (float) Q->vert.size();
|
||||
for(int vi = 0; vi < Q->vert.size(); ++vi)
|
||||
if(rand()/(float) RAND_MAX < ratio)
|
||||
mapsub.push_back(vi);
|
||||
|
||||
for(vi = 0; vi < P->vert.size(); ++vi)
|
||||
for(int vi = 0; vi < P->vert.size(); ++vi)
|
||||
if(rand()/(float) RAND_MAX < ratio)
|
||||
subsetP.push_back(&P->vert[vi]);
|
||||
|
||||
// estimate neigh distance
|
||||
float avD = 0.0,dist;
|
||||
float avD = 0.0;
|
||||
for(int i = 0 ; i < 100; ++i){
|
||||
int ri = rand()/(float) RAND_MAX * Q->vert.size() -1;
|
||||
std::vector< CoordType > samples,d_samples;
|
||||
|
|
@ -337,7 +317,7 @@ FourPCS<MeshType>::SelectCoplanarBase(){
|
|||
// compute r1 and r2
|
||||
CoordType x;
|
||||
std::swap(B[1],B[2]);
|
||||
IntersectionLineLine(B[0],B[1],B[2],B[3],x);
|
||||
IntersectionLineLine(B[0],B[1],B[2],B[3],x);
|
||||
|
||||
r1 = (x - B[0]).dot(B[1]-B[0]) / (B[1]-B[0]).SquaredNorm();
|
||||
r2 = (x - B[2]).dot(B[3]-B[2]) / (B[3]-B[2]).SquaredNorm();
|
||||
|
|
@ -358,39 +338,38 @@ FourPCS<MeshType>::SelectCoplanarBase(){
|
|||
std::vector< CoordType > >(*P,ugridP, prs.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("\n");
|
||||
// printf("\n");
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
template <class MeshType>
|
||||
bool
|
||||
FourPCS<MeshType>::IsTransfCongruent(FourPoints fp,vcg::Matrix44<ScalarType> & mat, float & trerr){
|
||||
bool FourPCS<MeshType>::IsTransfCongruent(FourPoints fp, vcg::Matrix44<ScalarType> & mat, float & trerr){
|
||||
|
||||
std::vector<vcg::Point3<ScalarType> > fix;
|
||||
std::vector<vcg::Point3<ScalarType> > mov;
|
||||
for(int i = 0 ; i < 4; ++i) mov.push_back(B[i]);
|
||||
for(int i = 0 ; i < 4; ++i) fix.push_back(fp[i]);
|
||||
std::vector<vcg::Point3<ScalarType> > fix;
|
||||
std::vector<vcg::Point3<ScalarType> > mov;
|
||||
for(int i = 0 ; i < 4; ++i) mov.push_back(B[i]);
|
||||
for(int i = 0 ; i < 4; ++i) fix.push_back(fp[i]);
|
||||
|
||||
vcg::Point3<ScalarType> n,p;
|
||||
n = (( B[1]-B[0]).normalized() ^ ( B[2]- B[0]).normalized())*( B[1]- B[0]).Norm();
|
||||
p = B[0] + n;
|
||||
mov.push_back(p);
|
||||
n = (( fp[1]-fp[0]).normalized() ^ (fp[2]- fp[0]).normalized())*( fp[1]- fp[0]).Norm();
|
||||
p = fp[0] + n;
|
||||
fix.push_back(p);
|
||||
vcg::Point3<ScalarType> n,p;
|
||||
n = (( B[1]-B[0]).normalized() ^ ( B[2]- B[0]).normalized())*( B[1]- B[0]).Norm();
|
||||
p = B[0] + n;
|
||||
mov.push_back(p);
|
||||
n = (( fp[1]-fp[0]).normalized() ^ (fp[2]- fp[0]).normalized())*( fp[1]- fp[0]).Norm();
|
||||
p = fp[0] + n;
|
||||
fix.push_back(p);
|
||||
|
||||
vcg::ComputeRigidMatchMatrix(fix,mov,mat);
|
||||
vcg::ComputeRigidMatchMatrix(fix,mov,mat);
|
||||
|
||||
ScalarType err = 0.0;
|
||||
for(int i = 0; i < 4; ++i) err+= (mat * mov[i] - fix[i]).SquaredNorm();
|
||||
ScalarType err = 0.0;
|
||||
for(int i = 0; i < 4; ++i) err+= (mat * mov[i] - fix[i]).SquaredNorm();
|
||||
|
||||
trerr = vcg::math::Sqrt(err);
|
||||
return err < prs.delta* prs.delta*4.0;
|
||||
}
|
||||
trerr = vcg::math::Sqrt(err);
|
||||
return err < prs.delta* prs.delta*4.0;
|
||||
}
|
||||
|
||||
template <class MeshType>
|
||||
void
|
||||
|
|
@ -401,7 +380,7 @@ FourPCS<MeshType>::ComputeR1R2(ScalarType d1,ScalarType d2){
|
|||
int start = clock();
|
||||
for(vi = 0; vi < mapsub.size(); ++vi) for(vj = vi; vj < mapsub.size(); ++vj){
|
||||
ScalarType d = ((Q->vert[mapsub[vi]]).P()-(Q->vert[mapsub[vj]]).P()).Norm();
|
||||
if( (d < d1+ side*0.5) && (d > d1-side*0.5))
|
||||
if( (d < d1+ side*0.5) && (d > d1-side*0.5))
|
||||
{
|
||||
R1.push_back(Couple(mapsub[vi],mapsub[vj],d ));
|
||||
R1.push_back(Couple(mapsub[vj],mapsub[vi],d));
|
||||
|
|
@ -416,7 +395,7 @@ FourPCS<MeshType>::ComputeR1R2(ScalarType d1,ScalarType d2){
|
|||
// }
|
||||
//}
|
||||
|
||||
std::sort(R1.begin(),R1.end());
|
||||
std::sort(R1.begin(),R1.end());
|
||||
// std::sort(R2.begin(),R2.end());
|
||||
}
|
||||
|
||||
|
|
@ -457,10 +436,10 @@ FourPCS<MeshType>::FindCongruent() { // of base B, on Q, with approximation delt
|
|||
}
|
||||
if(Invr.vert.empty() ) return false;
|
||||
|
||||
// index remaps a vertex of Invr to its corresponding point in R1
|
||||
typename PMesh::template PerVertexAttributeHandle<int> id = vcg::tri::Allocator<PMesh>::template AddPerVertexAttribute<int>(Invr,std::string("index"));
|
||||
i = &(*bR1)-&(*R1.begin());
|
||||
for(vii = Invr.vert.begin(); vii != Invr.vert.end();++vii,++i) id[vii] = i;
|
||||
// index remaps a vertex of Invr to its corresponding point in R1
|
||||
typename PMesh::template PerVertexAttributeHandle<int> id = vcg::tri::Allocator<PMesh>::template AddPerVertexAttribute<int>(Invr,std::string("index"));
|
||||
i = &(*bR1)-&(*R1.begin());
|
||||
for(vii = Invr.vert.begin(); vii != Invr.vert.end();++vii,++i) id[vii] = i;
|
||||
|
||||
vcg::tri::UpdateBounding<PMesh>::Box(Invr);
|
||||
// printf("Invr size %d\n",Invr.vn);
|
||||
|
|
@ -511,7 +490,7 @@ FourPCS<MeshType>::FindCongruent() { // of base B, on Q, with approximation delt
|
|||
}
|
||||
else{
|
||||
tr++;
|
||||
n_congr++;
|
||||
n_congr++;
|
||||
U.push_back(CandiType(p,mat));
|
||||
EvaluateAlignment(U.back());
|
||||
U.back().base = bases.size()-1;
|
||||
|
|
@ -549,11 +528,11 @@ int FourPCS<MeshType>::EvaluateSample(CandiType & fp, CoordType & tp, CoordType
|
|||
radius = prs.delta;
|
||||
tp = fp.T * tp;
|
||||
|
||||
vcg::Point4<ScalarType> np4;
|
||||
vcg::Point4<ScalarType> np4;
|
||||
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];
|
||||
|
||||
v = 0;
|
||||
v = 0;
|
||||
//v = vcg::tri::GetClosestVertex<
|
||||
// MeshType,
|
||||
// vcg::GridStaticPtr<typename MeshType::VertexType, ScalarType >
|
||||
|
|
@ -575,7 +554,7 @@ int FourPCS<MeshType>::EvaluateSample(CandiType & fp, CoordType & tp, CoordType
|
|||
template <class MeshType>
|
||||
void
|
||||
FourPCS<MeshType>::EvaluateAlignment(CandiType & fp){
|
||||
int n_delta_close = 0;
|
||||
int n_delta_close = 0;
|
||||
for(int i = 0 ; i< 4; ++i) {
|
||||
for(uint j = 0; j < ExtB[i].size();++j){
|
||||
CoordType np = ExtB[i][j]->cN();;
|
||||
|
|
@ -649,16 +628,16 @@ FourPCS<MeshType>:: Align( int L, vcg::Matrix44f & result, AACb * cb ){ // mai
|
|||
bestv = -std::numeric_limits<float>::max();
|
||||
iwinner = 0;
|
||||
|
||||
for(int i = 0 ; i < U.size() ;++i)
|
||||
{
|
||||
TestAlignment(U[i]);
|
||||
if(U[i].score > bestv){
|
||||
bestv = U[i].score;
|
||||
iwinner = i;
|
||||
}
|
||||
}
|
||||
for(int i = 0 ; i < U.size() ;++i)
|
||||
{
|
||||
TestAlignment(U[i]);
|
||||
if(U[i].score > bestv){
|
||||
bestv = U[i].score;
|
||||
iwinner = i;
|
||||
}
|
||||
}
|
||||
|
||||
printf("Best score: %d \n", bestv);
|
||||
printf("Best score: %d \n", bestv);
|
||||
|
||||
winner = U[iwinner];
|
||||
result = winner.T;
|
||||
|
|
|
|||
Loading…
Reference in New Issue