mirror of
https://github.com/lucaspalomodevelop/meshlab.git
synced 2026-03-20 03:16:10 +00:00
Bug fixed due to a parenthesis mismatch
This commit is contained in:
Gianpaolo Palma gianpaolopalma
parent
6b9b393976
commit
6037502524
@ -43,92 +43,92 @@ using namespace std;
|
||||
|
||||
bool AlignPair::A2Mesh::Import(const char *filename, Matrix44d &Tr)
|
||||
{
|
||||
int err = tri::io::Importer<A2Mesh>::Open(*this,filename);
|
||||
if(err) {
|
||||
printf("Error in reading %s: '%s'\n",filename,tri::io::Importer<A2Mesh>::ErrorMsg(err));
|
||||
exit(-1);
|
||||
}
|
||||
printf("read mesh `%s'\n", filename);
|
||||
return Init(Tr);
|
||||
int err = tri::io::Importer<A2Mesh>::Open(*this, filename);
|
||||
if (err) {
|
||||
printf("Error in reading %s: '%s'\n", filename, tri::io::Importer<A2Mesh>::ErrorMsg(err));
|
||||
exit(-1);
|
||||
}
|
||||
printf("read mesh `%s'\n", filename);
|
||||
return Init(Tr);
|
||||
}
|
||||
|
||||
bool AlignPair::A2Mesh::InitVert(const Matrix44d &Tr)
|
||||
{
|
||||
Matrix44d Idn; Idn.SetIdentity();
|
||||
if(Tr!=Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this,Tr);
|
||||
tri::UpdateNormal<A2Mesh>::NormalizePerVertex(*this);
|
||||
tri::UpdateBounding<A2Mesh>::Box(*this);
|
||||
return true;
|
||||
Matrix44d Idn; Idn.SetIdentity();
|
||||
if (Tr != Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this, Tr);
|
||||
tri::UpdateNormal<A2Mesh>::NormalizePerVertex(*this);
|
||||
tri::UpdateBounding<A2Mesh>::Box(*this);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AlignPair::A2Mesh::Init(const Matrix44d &Tr)
|
||||
{
|
||||
Matrix44d Idn; Idn.SetIdentity();
|
||||
tri::Clean<A2Mesh>::RemoveUnreferencedVertex(*this);
|
||||
if(Tr!=Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this,Tr);
|
||||
tri::UpdateNormal<A2Mesh>::PerVertexNormalizedPerFaceNormalized(*this);
|
||||
tri::UpdateFlags<A2Mesh>::FaceBorderFromNone(*this);
|
||||
tri::UpdateBounding<A2Mesh>::Box(*this);
|
||||
Matrix44d Idn; Idn.SetIdentity();
|
||||
tri::Clean<A2Mesh>::RemoveUnreferencedVertex(*this);
|
||||
if (Tr != Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this, Tr);
|
||||
tri::UpdateNormal<A2Mesh>::PerVertexNormalizedPerFaceNormalized(*this);
|
||||
tri::UpdateFlags<A2Mesh>::FaceBorderFromNone(*this);
|
||||
tri::UpdateBounding<A2Mesh>::Box(*this);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void AlignPair::Stat::clear()
|
||||
{
|
||||
I.clear();
|
||||
StartTime=0;
|
||||
MovVertNum=0;
|
||||
FixVertNum=0;
|
||||
FixFaceNum=0;
|
||||
I.clear();
|
||||
StartTime = 0;
|
||||
MovVertNum = 0;
|
||||
FixVertNum = 0;
|
||||
FixFaceNum = 0;
|
||||
}
|
||||
|
||||
// Restituisce true se nelle ultime <lastiter> iterazioni non e' diminuito
|
||||
// l'errore
|
||||
bool AlignPair::Stat::Stable(int lastiter)
|
||||
{
|
||||
if(I.empty()) return false;
|
||||
int parag = I.size()-lastiter;
|
||||
if (I.empty()) return false;
|
||||
int parag = I.size() - lastiter;
|
||||
|
||||
if(parag<0) parag=0;
|
||||
if( I.back().pcl50 < I[parag].pcl50 ) return false; // se siamo diminuiti non e' stabile
|
||||
if (parag < 0) parag = 0;
|
||||
if (I.back().pcl50 < I[parag].pcl50) return false; // se siamo diminuiti non e' stabile
|
||||
|
||||
return true;
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void AlignPair::Stat::Dump(FILE *fp)
|
||||
{
|
||||
if(I.size()==0) {
|
||||
fprintf(fp,"Empty AlignPair::Stat\n");
|
||||
return;
|
||||
}
|
||||
fprintf(fp,"Final Err %8.5f In %i iterations Total Time %ims\n",LastPcl50(),(int)I.size(),TotTime());
|
||||
fprintf(fp,"Mindist Med Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n");
|
||||
for(unsigned int qi=0;qi<I.size();++qi)
|
||||
fprintf(fp,"%5.2f (%6.3f:%6.3f) (%6.3f %6.3f %6.3f) %4ims %5i %5i %4i+%4i+%4i\n",
|
||||
I[qi].MinDistAbs,
|
||||
I[qi].pcl50, I[qi].pclhi,
|
||||
I[qi].AVG, I[qi].RMS, I[qi].StdDev ,
|
||||
IterTime(qi),
|
||||
I[qi].SampleTested,I[qi].SampleUsed,I[qi].DistanceDiscarded,I[qi].BorderDiscarded,I[qi].AngleDiscarded);
|
||||
if (I.size() == 0) {
|
||||
fprintf(fp, "Empty AlignPair::Stat\n");
|
||||
return;
|
||||
}
|
||||
fprintf(fp, "Final Err %8.5f In %i iterations Total Time %ims\n", LastPcl50(), (int)I.size(), TotTime());
|
||||
fprintf(fp, "Mindist Med Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n");
|
||||
for (unsigned int qi = 0; qi < I.size(); ++qi)
|
||||
fprintf(fp, "%5.2f (%6.3f:%6.3f) (%6.3f %6.3f %6.3f) %4ims %5i %5i %4i+%4i+%4i\n",
|
||||
I[qi].MinDistAbs,
|
||||
I[qi].pcl50, I[qi].pclhi,
|
||||
I[qi].AVG, I[qi].RMS, I[qi].StdDev,
|
||||
IterTime(qi),
|
||||
I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded);
|
||||
}
|
||||
|
||||
// Scrive una tabella con tutti i valori
|
||||
void AlignPair::Stat::HTMLDump(FILE *fp)
|
||||
{
|
||||
fprintf(fp,"Final Err %8.5f In %i iterations Total Time %ims\n",LastPcl50(),(int)I.size(),TotTime());
|
||||
fprintf(fp,"<table border>\n");
|
||||
fprintf(fp,"<tr> <th>Mindist</th><th> 50ile </th><th> Hi </th><th> Avg </th><th> RMS </th><th> StdDev </th><th> Time </th><th> Tested </th><th> Used </th><th> Dist </th><th> Bord </th><th> Angl \n");
|
||||
for(unsigned int qi=0;qi<I.size();++qi)
|
||||
fprintf(fp,"<tr> <td> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %5.3f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %4ims </td><td align=\"right\"> %5i </td><td align=\"right\"> %5i </td><td align=\"right\"> %4i </td><td align=\"right\"> %4i </td><td align=\"right\">%4i </td><td align=\"right\"></tr>\n",
|
||||
I[qi].MinDistAbs,
|
||||
I[qi].pcl50, I[qi].pclhi,
|
||||
I[qi].AVG, I[qi].RMS, I[qi].StdDev ,
|
||||
IterTime(qi),
|
||||
I[qi].SampleTested,I[qi].SampleUsed,I[qi].DistanceDiscarded,I[qi].BorderDiscarded,I[qi].AngleDiscarded);
|
||||
fprintf(fp,"</table>\n");
|
||||
fprintf(fp, "Final Err %8.5f In %i iterations Total Time %ims\n", LastPcl50(), (int)I.size(), TotTime());
|
||||
fprintf(fp, "<table border>\n");
|
||||
fprintf(fp, "<tr> <th>Mindist</th><th> 50ile </th><th> Hi </th><th> Avg </th><th> RMS </th><th> StdDev </th><th> Time </th><th> Tested </th><th> Used </th><th> Dist </th><th> Bord </th><th> Angl \n");
|
||||
for (unsigned int qi = 0; qi < I.size(); ++qi)
|
||||
fprintf(fp, "<tr> <td> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %5.3f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %4ims </td><td align=\"right\"> %5i </td><td align=\"right\"> %5i </td><td align=\"right\"> %4i </td><td align=\"right\"> %4i </td><td align=\"right\">%4i </td><td align=\"right\"></tr>\n",
|
||||
I[qi].MinDistAbs,
|
||||
I[qi].pcl50, I[qi].pclhi,
|
||||
I[qi].AVG, I[qi].RMS, I[qi].StdDev,
|
||||
IterTime(qi),
|
||||
I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded);
|
||||
fprintf(fp, "</table>\n");
|
||||
}
|
||||
|
||||
|
||||
@ -141,56 +141,56 @@ MaxPointNum an (unused) hard limit on the number of points that are choosen
|
||||
MinPointNum the minimum number of points that have to be chosen to be usable
|
||||
|
||||
*/
|
||||
bool AlignPair::ChoosePoints( vector<Point3d> &Ps, // vertici corrispondenti su src (rossi)
|
||||
vector<Point3d> &Ns, // normali corrispondenti su src (rossi)
|
||||
vector<Point3d> &Pt, // vertici scelti su trg (verdi)
|
||||
vector<Point3d> &OPt, // vertici scelti su trg (verdi)
|
||||
double PassHi,
|
||||
Histogramf &H)
|
||||
bool AlignPair::ChoosePoints(vector<Point3d> &Ps, // vertici corrispondenti su src (rossi)
|
||||
vector<Point3d> &Ns, // normali corrispondenti su src (rossi)
|
||||
vector<Point3d> &Pt, // vertici scelti su trg (verdi)
|
||||
vector<Point3d> &OPt, // vertici scelti su trg (verdi)
|
||||
double PassHi,
|
||||
Histogramf &H)
|
||||
{
|
||||
const int N = ap.MaxPointNum;
|
||||
double newmaxd = H.Percentile(PassHi);
|
||||
//printf("%5.1f of the pairs has a distance less than %g and greater than %g (0..%g) avg %g\n", Perc*100,newmind,newmaxd,H.maxv,H.Percentile(.5));
|
||||
int sz = Ps.size();
|
||||
int fnd=0;
|
||||
int lastgood=sz-1;
|
||||
math::SubtractiveRingRNG myrnd;
|
||||
while(fnd<N && fnd<lastgood)
|
||||
const int N = ap.MaxPointNum;
|
||||
double newmaxd = H.Percentile(PassHi);
|
||||
//printf("%5.1f of the pairs has a distance less than %g and greater than %g (0..%g) avg %g\n", Perc*100,newmind,newmaxd,H.maxv,H.Percentile(.5));
|
||||
int sz = 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)
|
||||
{
|
||||
int index = fnd+myrnd.generate(lastgood-fnd);
|
||||
double dd=Distance(Ps[index],Pt[index]);
|
||||
if(dd<=newmaxd)
|
||||
{
|
||||
swap(Ps[index],Ps[fnd]);
|
||||
swap(Ns[index],Ns[fnd]);
|
||||
swap(Pt[index],Pt[fnd]);
|
||||
swap(OPt[index],OPt[fnd]);
|
||||
++fnd;
|
||||
}
|
||||
else
|
||||
{
|
||||
swap(Ps[index],Ps[lastgood]);
|
||||
swap(Ns[index],Ns[lastgood]);
|
||||
swap(Pt[index],Pt[lastgood]);
|
||||
swap(OPt[index],OPt[lastgood]);
|
||||
lastgood--;
|
||||
}
|
||||
swap(Ps[index], Ps[fnd]);
|
||||
swap(Ns[index], Ns[fnd]);
|
||||
swap(Pt[index], Pt[fnd]);
|
||||
swap(OPt[index], OPt[fnd]);
|
||||
++fnd;
|
||||
}
|
||||
Ps.resize(fnd);
|
||||
Ns.resize(fnd);
|
||||
Pt.resize(fnd);
|
||||
OPt.resize(fnd);
|
||||
printf("Scelte %i coppie tra le %i iniziali, scartate quelle con dist > %f\n",fnd,sz,newmaxd);
|
||||
else
|
||||
{
|
||||
swap(Ps[index], Ps[lastgood]);
|
||||
swap(Ns[index], Ns[lastgood]);
|
||||
swap(Pt[index], Pt[lastgood]);
|
||||
swap(OPt[index], OPt[lastgood]);
|
||||
lastgood--;
|
||||
}
|
||||
}
|
||||
Ps.resize(fnd);
|
||||
Ns.resize(fnd);
|
||||
Pt.resize(fnd);
|
||||
OPt.resize(fnd);
|
||||
printf("Scelte %i coppie tra le %i iniziali, scartate quelle con dist > %f\n", fnd, sz, newmaxd);
|
||||
|
||||
if( (int)Ps.size()<ap.MinPointNum ) {
|
||||
printf("Troppi pochi punti!\n");
|
||||
Ps.clear();
|
||||
Ns.clear();
|
||||
Pt.clear();
|
||||
OPt.clear();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
if ((int)Ps.size() < ap.MinPointNum) {
|
||||
printf("Troppi pochi punti!\n");
|
||||
Ps.clear();
|
||||
Ns.clear();
|
||||
Pt.clear();
|
||||
OPt.clear();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -200,69 +200,69 @@ della mesh da muovere trasformata secondo la matrice <In>
|
||||
Calcola anche il nuovo bounding box di tali vertici trasformati.
|
||||
*/
|
||||
bool AlignPair::InitMov(
|
||||
vector< Point3d > &MovVert,
|
||||
vector< Point3d > &MovNorm,
|
||||
Box3d &trgbox,
|
||||
const Matrix44d &in ) // trasformazione Iniziale (che porta i punti di trg su src)
|
||||
vector< Point3d > &MovVert,
|
||||
vector< Point3d > &MovNorm,
|
||||
Box3d &trgbox,
|
||||
const Matrix44d &in) // trasformazione Iniziale (che porta i punti di trg su src)
|
||||
{
|
||||
Point3d pp,nn;
|
||||
Point3d pp, nn;
|
||||
|
||||
MovVert.clear();
|
||||
MovNorm.clear();
|
||||
trgbox.SetNull();
|
||||
MovVert.clear();
|
||||
MovNorm.clear();
|
||||
trgbox.SetNull();
|
||||
|
||||
A2Mesh::VertexIterator vi;
|
||||
for(vi=mov->begin(); vi!=mov->end(); vi++) {
|
||||
pp=in*(*vi).P();
|
||||
nn=in*Point3d((*vi).P()+(*vi).N())-pp;
|
||||
nn.Normalize();
|
||||
MovVert.push_back(pp);
|
||||
MovNorm.push_back(nn);
|
||||
trgbox.Add(pp);
|
||||
}
|
||||
return true;
|
||||
A2Mesh::VertexIterator vi;
|
||||
for (vi = mov->begin(); vi != mov->end(); vi++) {
|
||||
pp = in*(*vi).P();
|
||||
nn = in*Point3d((*vi).P() + (*vi).N()) - pp;
|
||||
nn.Normalize();
|
||||
MovVert.push_back(pp);
|
||||
MovNorm.push_back(nn);
|
||||
trgbox.Add(pp);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AlignPair::InitFixVert(AlignPair::A2Mesh *fm,
|
||||
AlignPair::Param &pp,
|
||||
A2GridVert &u,
|
||||
int PreferredGridSize)
|
||||
AlignPair::Param &pp,
|
||||
A2GridVert &u,
|
||||
int PreferredGridSize)
|
||||
{
|
||||
Box3d bb2=fm->bbox;
|
||||
double MinDist= pp.MinDistAbs*1.1;
|
||||
//the bbox of the grid should be enflated of the mindist used in the ICP search
|
||||
bb2.Offset(Point3d(MinDist,MinDist,MinDist));
|
||||
Box3d bb2 = fm->bbox;
|
||||
double MinDist = pp.MinDistAbs*1.1;
|
||||
//the bbox of the grid should be enflated of the mindist used in the ICP search
|
||||
bb2.Offset(Point3d(MinDist, MinDist, MinDist));
|
||||
|
||||
u.SetBBox(bb2);
|
||||
u.SetBBox(bb2);
|
||||
|
||||
//Inserisco la src nella griglia
|
||||
if(PreferredGridSize==0) PreferredGridSize=fm->vert.size()*pp.UGExpansionFactor;
|
||||
u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize);
|
||||
printf("UG %i %i %i\n",u.siz[0],u.siz[1],u.siz[2]);
|
||||
return true;
|
||||
//Inserisco la src nella griglia
|
||||
if (PreferredGridSize == 0) PreferredGridSize = fm->vert.size()*pp.UGExpansionFactor;
|
||||
u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize);
|
||||
printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AlignPair::InitFix(AlignPair::A2Mesh *fm,
|
||||
AlignPair::Param &pp,
|
||||
A2Grid &u,
|
||||
int PreferredGridSize)
|
||||
AlignPair::Param &pp,
|
||||
A2Grid &u,
|
||||
int PreferredGridSize)
|
||||
{
|
||||
tri::InitFaceIMark(*fm);
|
||||
tri::InitFaceIMark(*fm);
|
||||
|
||||
Box3d bb2=fm->bbox;
|
||||
// double MinDist= fm->bbox.Diag()*pp.MinDistPerc;
|
||||
double MinDist= pp.MinDistAbs*1.1;
|
||||
//gonfio della distanza utente il BBox della seconda mesh
|
||||
bb2.Offset(Point3d(MinDist,MinDist,MinDist));
|
||||
Box3d bb2 = fm->bbox;
|
||||
// double MinDist= fm->bbox.Diag()*pp.MinDistPerc;
|
||||
double MinDist = pp.MinDistAbs*1.1;
|
||||
//gonfio della distanza utente il BBox della seconda mesh
|
||||
bb2.Offset(Point3d(MinDist, MinDist, MinDist));
|
||||
|
||||
u.SetBBox(bb2);
|
||||
u.SetBBox(bb2);
|
||||
|
||||
//Inserisco la src nella griglia
|
||||
if(PreferredGridSize==0) PreferredGridSize=fm->face.size()*pp.UGExpansionFactor;
|
||||
u.Set(fm->face.begin(), fm->face.end(), PreferredGridSize);
|
||||
printf("UG %i %i %i\n",u.siz[0],u.siz[1],u.siz[2]);
|
||||
return true;
|
||||
//Inserisco la src nella griglia
|
||||
if (PreferredGridSize == 0) PreferredGridSize = fm->face.size()*pp.UGExpansionFactor;
|
||||
u.Set(fm->face.begin(), fm->face.end(), PreferredGridSize);
|
||||
printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]);
|
||||
return true;
|
||||
}
|
||||
/*
|
||||
The Main ICP alignment Function:
|
||||
@ -280,204 +280,210 @@ la uniform grid sia gia' inizializzata con la mesh fix
|
||||
*/
|
||||
|
||||
bool AlignPair::Align(
|
||||
A2Grid &u,
|
||||
A2GridVert &uv,
|
||||
const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix)
|
||||
Matrix44d &out, // trasformazione calcolata
|
||||
vector<Point3d> &Pfix, // vertici corrispondenti su src (rossi)
|
||||
vector<Point3d> &Nfix, // normali corrispondenti su src (rossi)
|
||||
vector<Point3d> &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target)
|
||||
vector<Point3d> &ONmov, // normali scelti su trg (verdi)
|
||||
Histogramf &H,
|
||||
AlignPair::Stat &as)
|
||||
A2Grid &u,
|
||||
A2GridVert &uv,
|
||||
const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix)
|
||||
Matrix44d &out, // trasformazione calcolata
|
||||
vector<Point3d> &Pfix, // vertici corrispondenti su src (rossi)
|
||||
vector<Point3d> &Nfix, // normali corrispondenti su src (rossi)
|
||||
vector<Point3d> &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target)
|
||||
vector<Point3d> &ONmov, // normali scelti su trg (verdi)
|
||||
Histogramf &H,
|
||||
AlignPair::Stat &as)
|
||||
{
|
||||
vector<char> 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;
|
||||
vector< Point3d > movvert;
|
||||
vector< Point3d > movnorm;
|
||||
vector<Point3d> Pmov; // vertici scelti dopo la trasf iniziale
|
||||
status=SUCCESS;
|
||||
int tt0=clock();
|
||||
vector<char> 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;
|
||||
vector< Point3d > movvert;
|
||||
vector< Point3d > movnorm;
|
||||
vector<Point3d> Pmov; // vertici scelti dopo la trasf iniziale
|
||||
status = SUCCESS;
|
||||
int tt0 = clock();
|
||||
|
||||
out=in;
|
||||
out = in;
|
||||
|
||||
int i;
|
||||
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();
|
||||
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);
|
||||
beyondCntVec.resize(mov->size(), 0);
|
||||
|
||||
/**************** BEGIN ICP LOOP ****************/
|
||||
do
|
||||
/**************** BEGIN ICP LOOP ****************/
|
||||
do
|
||||
{
|
||||
Stat::IterInfo ii;
|
||||
Box3d movbox;
|
||||
InitMov(movvert, movnorm, movbox, out);
|
||||
H.SetRange(0, StartMinDist, 512, 2.5);
|
||||
Pfix.clear();
|
||||
Nfix.clear();
|
||||
Pmov.clear();
|
||||
OPmov.clear();
|
||||
ONmov.clear();
|
||||
int tts0 = clock();
|
||||
ii.MinDistAbs = StartMinDist;
|
||||
int LocSampleNum = min(ap.SampleNum, int(movvert.size()));
|
||||
Box3d fixbox;
|
||||
if (u.Empty()) fixbox = uv.bbox;
|
||||
else fixbox = u.bbox;
|
||||
for (i = 0; i < LocSampleNum; ++i)
|
||||
{
|
||||
Stat::IterInfo ii;
|
||||
Box3d movbox;
|
||||
InitMov(movvert,movnorm,movbox,out);
|
||||
H.SetRange(0,StartMinDist,512,2.5);
|
||||
Pfix.clear();
|
||||
Nfix.clear();
|
||||
Pmov.clear();
|
||||
OPmov.clear();
|
||||
ONmov.clear();
|
||||
int tts0=clock();
|
||||
ii.MinDistAbs=StartMinDist;
|
||||
int LocSampleNum=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]))
|
||||
{
|
||||
if( beyondCntVec[i] < maxBeyondCnt ){
|
||||
if(! fixbox.IsIn(movvert[i]) )
|
||||
beyondCntVec[i]=maxBeyondCnt+1;
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
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<vcg::AlignPair::A2Mesh, vcg::AlignPair::A2Grid >(*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<A2Face,double>(*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++;
|
||||
if (movnorm[i].dot(vp->N()) < CosAngleThr) {
|
||||
ii.AngleDiscarded++; continue;
|
||||
}
|
||||
} // 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;
|
||||
closestPoint = vp->P();
|
||||
closestNormal = vp->N();
|
||||
}
|
||||
else // using the standard faces and grid
|
||||
{
|
||||
A2Mesh::FacePointer f = vcg::tri::GetClosestFaceBase<vcg::AlignPair::A2Mesh, vcg::AlignPair::A2Grid >(*fix, u, movvert[i], maxd, error, closestPoint);
|
||||
if (error >= StartMinDist) {
|
||||
ii.DistanceDiscarded++; ++beyondCntVec[i]; continue;
|
||||
}
|
||||
Matrix44d newout;
|
||||
switch(ap.MatchMode) {
|
||||
case AlignPair::Param::MMSimilarity : ComputeRotoTranslationScalingMatchMatrix(newout,Pfix,OPmov); break;
|
||||
case AlignPair::Param::MMRigid : ComputeRigidMatchMatrix(Pfix,OPmov,newout); break;
|
||||
default :
|
||||
status = UNKNOWN_MODE;
|
||||
ii.Time=clock();
|
||||
as.I.push_back(ii);
|
||||
return false;
|
||||
if (movnorm[i].dot(f->N()) < CosAngleThr) {
|
||||
ii.AngleDiscarded++; continue;
|
||||
}
|
||||
Point3d ip;
|
||||
InterpolationParameters<A2Face, double>(*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;
|
||||
}
|
||||
|
||||
// double sum_before=0;
|
||||
// double sum_after=0;
|
||||
// for(unsigned int iii=0;iii<Pfix.size();++iii)
|
||||
// {
|
||||
// sum_before+=Distance(Pfix[iii], out*OPmov[iii]);
|
||||
// sum_after+=Distance(Pfix[iii], newout*OPmov[iii]);
|
||||
// }
|
||||
// //printf("Distance %f -> %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;
|
||||
|
||||
assert(Pfix.size()==Pmov.size());
|
||||
int tts2=clock();
|
||||
ttsearch+=tts1-tts0;
|
||||
ttleast +=tts2-tts1;
|
||||
ii.pcl50=H.Percentile(.5);
|
||||
ii.pclhi=H.Percentile(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 <ReduceFactor> parameter.
|
||||
// We use 5 times the <ReduceFactor> percentile of the found points.
|
||||
if(ap.ReduceFactorPerc<1) StartMinDist=max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist,5.0*H.Percentile(ap.ReduceFactorPerc)));
|
||||
}
|
||||
while (
|
||||
nc<=ap.MaxIterNum &&
|
||||
H.Percentile(.5) > ap.TrgDistAbs &&
|
||||
(nc<ap.EndStepNum+1 || ! as.Stable(ap.EndStepNum) )
|
||||
);
|
||||
/**************** END ICP LOOP ****************/
|
||||
int tt2=clock();
|
||||
Matrix44d ResCopy=out;
|
||||
Point3d scv,shv,rtv,trv;
|
||||
Decompose(ResCopy,scv,shv,rtv,trv);
|
||||
if((ap.MatchMode==vcg::AlignPair::Param::MMRigid) && (math::Abs(1-scv[0])>ap.MaxScale || math::Abs(1-scv[1])>ap.MaxScale || math::Abs(1-scv[2])>ap.MaxScale) ) {
|
||||
status = TOO_MUCH_SCALE;
|
||||
}
|
||||
} // 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;
|
||||
}
|
||||
}
|
||||
if(shv[0]>ap.MaxShear || shv[1]>ap.MaxShear || shv[2]>ap.MaxShear ) {
|
||||
status = TOO_MUCH_SHEAR;
|
||||
return false;
|
||||
Matrix44d newout;
|
||||
switch (ap.MatchMode)
|
||||
{
|
||||
case AlignPair::Param::MMSimilarity: ComputeRotoTranslationScalingMatchMatrix(newout, Pfix, OPmov); break;
|
||||
case AlignPair::Param::MMRigid: ComputeRigidMatchMatrix(Pfix, OPmov, newout); break;
|
||||
default:
|
||||
status = UNKNOWN_MODE;
|
||||
ii.Time = clock();
|
||||
as.I.push_back(ii);
|
||||
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;
|
||||
// double sum_before=0;
|
||||
// double sum_after=0;
|
||||
// for(unsigned int iii=0;iii<Pfix.size();++iii)
|
||||
// {
|
||||
// sum_before+=Distance(Pfix[iii], out*OPmov[iii]);
|
||||
// sum_after+=Distance(Pfix[iii], newout*OPmov[iii]);
|
||||
// }
|
||||
// //printf("Distance %f -> %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;
|
||||
|
||||
assert(Pfix.size() == Pmov.size());
|
||||
int tts2 = clock();
|
||||
ttsearch += tts1 - tts0;
|
||||
ttleast += tts2 - tts1;
|
||||
ii.pcl50 = H.Percentile(.5);
|
||||
ii.pclhi = H.Percentile(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 <ReduceFactor> parameter.
|
||||
// We use 5 times the <ReduceFactor> percentile of the found points.
|
||||
if (ap.ReduceFactorPerc<1) StartMinDist = max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist, 5.0*H.Percentile(ap.ReduceFactorPerc)));
|
||||
} while (
|
||||
nc <= ap.MaxIterNum &&
|
||||
H.Percentile(.5) > ap.TrgDistAbs &&
|
||||
(nc<ap.EndStepNum + 1 || !as.Stable(ap.EndStepNum))
|
||||
);
|
||||
/**************** END ICP LOOP ****************/
|
||||
int tt2 = clock();
|
||||
Matrix44d ResCopy = out;
|
||||
Point3d scv, shv, rtv, trv;
|
||||
Decompose(ResCopy, scv, shv, rtv, trv);
|
||||
if ((ap.MatchMode == vcg::AlignPair::Param::MMRigid) && (math::Abs(1 - scv[0])>ap.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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
const char *AlignPair::ErrorMsg( ErrorCode code)
|
||||
const char *AlignPair::ErrorMsg(ErrorCode code)
|
||||
{
|
||||
switch(code){
|
||||
case SUCCESS: return "Success ";
|
||||
case NO_COMMON_BBOX : return "No Common BBox ";
|
||||
case TOO_FEW_POINTS : return "Too few points ";
|
||||
case LSQ_DIVERGE : return "LSQ not converge";
|
||||
case TOO_MUCH_SHEAR : return "Too much shear ";
|
||||
case TOO_MUCH_SCALE : return "Too much scale ";
|
||||
case UNKNOWN_MODE : return "Unknown mode ";
|
||||
default : assert(0); return "Catastrophic Error";
|
||||
}
|
||||
return 0;
|
||||
switch (code){
|
||||
case SUCCESS: return "Success ";
|
||||
case NO_COMMON_BBOX: return "No Common BBox ";
|
||||
case TOO_FEW_POINTS: return "Too few points ";
|
||||
case LSQ_DIVERGE: return "LSQ not converge";
|
||||
case TOO_MUCH_SHEAR: return "Too much shear ";
|
||||
case TOO_MUCH_SCALE: return "Too much scale ";
|
||||
case UNKNOWN_MODE: return "Unknown mode ";
|
||||
default: assert(0); return "Catastrophic Error";
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/*
|
||||
|
||||
@ -625,38 +631,38 @@ return maxfnd;
|
||||
|
||||
bool AlignPair::SampleMovVert(vector<A2Vertex> &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;
|
||||
switch (SampleMode)
|
||||
{
|
||||
case AlignPair::Param::SMRandom: return SampleMovVertRandom(vert, SampleNum);
|
||||
case AlignPair::Param::SMNormalEqualized: return SampleMovVertNormalEqualized(vert, SampleNum);
|
||||
default: assert(0);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// Function to retrieve a static random number generator object.
|
||||
static math::SubtractiveRingRNG &LocRnd(){
|
||||
static math::SubtractiveRingRNG myrnd(time(NULL));
|
||||
return myrnd;
|
||||
static math::SubtractiveRingRNG myrnd(time(NULL));
|
||||
return myrnd;
|
||||
}
|
||||
// Gets a random number in the interval [0..n].
|
||||
static int LocRnd(int n){
|
||||
return LocRnd().generate(n);
|
||||
return LocRnd().generate(n);
|
||||
}
|
||||
// Scelta a caso semplice
|
||||
bool AlignPair::SampleMovVertRandom(vector<A2Vertex> &vert, int SampleNum)
|
||||
{
|
||||
if(int(vert.size())<=SampleNum) return true;
|
||||
int i;
|
||||
for(i=0;i<SampleNum;++i)
|
||||
{
|
||||
int pos=LocRnd(vert.size());
|
||||
assert(pos>=0 && pos < int(vert.size()));
|
||||
swap(vert[i],vert[pos]);
|
||||
}
|
||||
vert.resize(SampleNum);
|
||||
return true;
|
||||
if (int(vert.size()) <= SampleNum) return true;
|
||||
int i;
|
||||
for (i = 0; i < SampleNum; ++i)
|
||||
{
|
||||
int pos = LocRnd(vert.size());
|
||||
assert(pos >= 0 && pos < int(vert.size()));
|
||||
swap(vert[i], vert[pos]);
|
||||
}
|
||||
vert.resize(SampleNum);
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -673,51 +679,51 @@ e poi un punto all'interno del bucket
|
||||
|
||||
bool AlignPair::SampleMovVertNormalEqualized(vector<A2Vertex> &vert, int SampleNum)
|
||||
{
|
||||
// assert(0);
|
||||
// assert(0);
|
||||
|
||||
// int t0=clock();
|
||||
static vector<Point3d> NV;
|
||||
if(NV.size()==0)
|
||||
// int t0=clock();
|
||||
static vector<Point3d> NV;
|
||||
if (NV.size() == 0)
|
||||
{
|
||||
GenNormal<double>::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
|
||||
vector<vector <int> > BKT(NV.size());
|
||||
for (size_t i = 0; i < vert.size(); ++i)
|
||||
{
|
||||
int ind = GenNormal<double>::BestMatchingNormal(vert[i].N(), NV);
|
||||
BKT[ind].push_back(i);
|
||||
}
|
||||
//int t1=clock();
|
||||
|
||||
// vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket
|
||||
vector <int> BKTpos(BKT.size(), 0);
|
||||
|
||||
if (SampleNum >= int(vert.size())) SampleNum = vert.size() - 1;
|
||||
|
||||
for (int i = 0; i < SampleNum;)
|
||||
{
|
||||
int ind = LocRnd(BKT.size()); // Scelgo un Bucket
|
||||
int &CURpos = BKTpos[ind];
|
||||
vector<int> &CUR = BKT[ind];
|
||||
|
||||
if (CURpos<int(CUR.size()))
|
||||
{
|
||||
GenNormal<double>::Fibonacci(30,NV);
|
||||
printf("Generated %i normals\n",int(NV.size()));
|
||||
swap(CUR[CURpos], CUR[CURpos + LocRnd(BKT[ind].size() - CURpos)]);
|
||||
swap(vert[i], vert[CUR[CURpos]]);
|
||||
++BKTpos[ind];
|
||||
++i;
|
||||
}
|
||||
// Bucket vector dove, per ogni normale metto gli indici
|
||||
// dei vertici ad essa corrispondenti
|
||||
vector<vector <int> > BKT(NV.size());
|
||||
for(size_t i=0;i<vert.size();++i)
|
||||
{
|
||||
int ind=GenNormal<double>::BestMatchingNormal(vert[i].N(),NV);
|
||||
BKT[ind].push_back(i);
|
||||
}
|
||||
//int t1=clock();
|
||||
}
|
||||
vert.resize(SampleNum);
|
||||
// int t2=clock();
|
||||
// printf("Matching %6i\n",t1-t0);
|
||||
// printf("Collecting %6i\n",t2-t1);
|
||||
// printf("Total %6i\n",t2-t0);
|
||||
|
||||
// vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket
|
||||
vector <int> BKTpos(BKT.size(),0);
|
||||
|
||||
if(SampleNum >= int(vert.size())) SampleNum= vert.size()-1;
|
||||
|
||||
for(int i=0;i<SampleNum;)
|
||||
{
|
||||
int ind=LocRnd(BKT.size()); // Scelgo un Bucket
|
||||
int &CURpos = BKTpos[ind];
|
||||
vector<int> &CUR = BKT[ind];
|
||||
|
||||
if(CURpos<int(CUR.size()))
|
||||
{
|
||||
swap(CUR[CURpos], CUR[ CURpos + LocRnd(BKT[ind].size()-CURpos)]);
|
||||
swap(vert[i],vert[CUR[CURpos]]);
|
||||
++BKTpos[ind];
|
||||
++i;
|
||||
}
|
||||
}
|
||||
vert.resize(SampleNum);
|
||||
// int t2=clock();
|
||||
// printf("Matching %6i\n",t1-t0);
|
||||
// printf("Collecting %6i\n",t2-t1);
|
||||
// printf("Total %6i\n",t2-t0);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user