TMP: first move align_pair

This commit is contained in:
alemuntoni 2020-05-29 11:55:51 +02:00
parent e7ce2614ec
commit ec730298fa
1 changed files with 515 additions and 0 deletions

View File

@ -0,0 +1,515 @@
/****************************************************************************
* VCGLib o o *
* Visual and Computer Graphics Library o o *
* _ O _ *
* Copyright(C) 2004-2016 \/)\/ *
* Visual Computing Lab /\/| *
* ISTI - Italian National Research Council | *
* \ *
* All rights reserved. *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
* for more details. *
* *
****************************************************************************/
#ifndef VCG_ALIGN_PAIR_H
#define VCG_ALIGN_PAIR_H
#include <ctime>
#include <vcg/math/histogram.h>
#include <vcg/math/matrix44.h>
#include <vcg/math/random_generator.h>
#include <vcg/math/gen_normal.h>
#include <vcg/space/point_matching.h>
#include <vcg/space/index/grid_static_ptr.h>
#include <vcg/simplex/face/component_ep.h>
#include <vcg/complex/complex.h>
#include <vcg/complex/algorithms/clean.h>
#include <vcg/complex/algorithms/closest.h>
#include <vcg/complex/algorithms/update/normal.h>
#include <vcg/complex/algorithms/update/bounding.h>
#include <vcg/complex/algorithms/update/component_ep.h>
#include <vcg/complex/algorithms/update/position.h>
#include <vcg/complex/algorithms/update/flag.h>
#include <vcg/complex/algorithms/update/normal.h>
#include <vcg/complex/algorithms/update/bounding.h>
namespace vcg {
/*************************************************************************
AlignPair
Classe per gestire un allineamento tra DUE sole mesh.
**************************************************************************/
class AlignPair {
public:
enum ErrorCode {
SUCCESS,
NO_COMMON_BBOX,
TOO_FEW_POINTS,
LSQ_DIVERGE,
TOO_MUCH_SHEAR,
TOO_MUCH_SCALE,
FORBIDDEN,
INVALID,
UNKNOWN_MODE };
/*********************** Classi Accessorie ****************************/
class A2Vertex;
class A2Face;
class A2UsedTypes:
public vcg::UsedTypes < vcg::Use<A2Vertex>::AsVertexType,
vcg::Use<A2Face >::AsFaceType >{};
class A2Vertex : public vcg::Vertex<A2UsedTypes,vcg::vertex::Coord3d,vcg::vertex::Normal3d,vcg::vertex::BitFlags> {};
class A2Face : public vcg::Face< A2UsedTypes,vcg::face::VertexRef, vcg::face::Normal3d,vcg::face::Mark,vcg::face::BitFlags> {};
class A2Mesh : public vcg::tri::TriMesh< std::vector<A2Vertex>, std::vector<A2Face> >
{
public:
//bool Import(const char *filename) { Matrix44d Tr; Tr.SetIdentity(); return Import(filename,Tr);}
//bool Import(const char *filename, Matrix44d &Tr);
inline bool 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;
}
inline bool 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);
return true;
}
};
typedef A2Mesh::FaceContainer FaceContainer;
typedef A2Mesh::FaceType FaceType;
typedef GridStaticPtr<FaceType, double > A2Grid;
typedef GridStaticPtr<A2Mesh::VertexType, double > A2GridVert;
class Stat
{
public:
class IterInfo
{
public:
IterInfo()
{
memset ( (void *) this, 0, sizeof(IterInfo));
}
double MinDistAbs;
int DistanceDiscarded;
int AngleDiscarded;
int BorderDiscarded;
int SampleTested; // quanti punti ho testato con la mindist
int SampleUsed; // quanti punti ho scelto per la computematrix
double pcl50;
double pclhi;
double AVG;
double RMS;
double StdDev;
int Time; // quando e' finita questa iterazione
};
std::vector<IterInfo> I;
double LastPcl50() const
{
return I.back().pcl50;
}
int LastSampleUsed() const {
return I.back().SampleUsed;
}
int MovVertNum;
int FixVertNum;
int FixFaceNum;
int TotTime() {
return I.back().Time-StartTime;
}
int IterTime(unsigned int i) const
{
const int clock_per_ms = std::max<int>(CLOCKS_PER_SEC / 1000,1);
assert(i<I.size());
if(i==0) return (I[i].Time-StartTime )/clock_per_ms;
else return (I[i].Time - I[i-1].Time)/clock_per_ms ;
}
int StartTime;
inline void clear()
{
I.clear();
StartTime = 0;
MovVertNum = 0;
FixVertNum = 0;
FixFaceNum = 0;
}
inline void 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);
}
// Scrive una tabella con tutti i valori
inline void 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");
}
// Restituisce true se nelle ultime <lastiter> iterazioni non e' diminuito
// l'errore
inline bool Stable(int lastiter)
{
if (I.empty())
return false;
int parag = int(I.size()) - lastiter;
if (parag < 0)
parag = 0;
if (I.back().pcl50 < I[parag].pcl50)
return false; // se siamo diminuiti non e' stabile
return true;
}
};
class Param
{
public:
enum MatchModeEnum {MMSimilarity, MMRigid};
enum SampleModeEnum {SMRandom, SMNormalEqualized};
Param()
{
SampleNum = 2000;
MaxPointNum = 100000;
MinPointNum = 30;
MaxIterNum = 75;
TrgDistAbs = 0.005f;
MinDistAbs = 10;
MaxAngleRad = math::ToRad(45.0);
MaxShear = 0.5;
MaxScale = 0.5; // significa che lo scale deve essere compreso tra 1-MaxScale e 1+MaxScale
PassHiFilter = 0.75;
ReduceFactorPerc = 0.80;
MinMinDistPerc = 0.01;
EndStepNum = 5;
MatchMode = MMRigid;
SampleMode = SMNormalEqualized;
UGExpansionFactor=10;
MinFixVertNum=20000;
MinFixVertNumPerc=.25;
UseVertexOnly = false;
}
int SampleNum; // numero di sample da prendere sulla mesh fix, utilizzando
// il SampleMode scelto tra cui poi sceglierne al piu' <MaxPointNum>
// e almeno <MinPointNum> da usare per l'allineamento.
int MaxPointNum; // numero di coppie di punti da usare quando si calcola la matrice
// di allienamento e che poi si mettono da parte per il globale;
// di solito non usato
int MinPointNum; // minimo numero di coppie di punti ammissibile perche' sia considerato
// valido l'allineamento
double MinDistAbs; // distanza minima iniziale perche due punti vengano presi in
// considerazione. NON e' piu espressa in percentuale sul bbox della mesh nella ug.
// Ad ogni passo puo essere ridotta per
// accellerare usando ReduceFactor
double MaxAngleRad; // massimo angolo in radianti tra due normali perche' i due
// punti vengano presi in considerazione.
int MaxIterNum; // massimo numero di iterazioni da fare in align
double TrgDistAbs; // distanza obiettivo entro la quale devono stare almeno la meta'
// dei campioni scelti; di solito non entra in gioco perche' ha un default molto basso
int EndStepNum; // numero di iterazioni da considerare per decidere se icp ha converso.
//double PassLoFilter; // Filtraggio utilizzato per decidere quali punti scegliere tra quello trovati abbastanza
double PassHiFilter; // vicini. Espresso in percentili. Di solito si scarta il quelli sopra il 75 e quelli sotto il 5
double ReduceFactorPerc; // At each step we discard the points farther than a given threshold. The threshold is iterativeley reduced;
// StartMinDist= min(StartMinDist, 5.0*H.Percentile(ap.ReduceFactorPerc))
double MinMinDistPerc; // Ratio between initial starting distance (MinDistAbs) and what can reach by the application of the ReduceFactor.
int UGExpansionFactor; // Grandezza della UG per la mesh fix come rapporto del numero di facce della mesh fix
// Nel caso si usi qualche struttura multiresolution
int MinFixVertNum; // Gli allineamenti si fanno mettendo nella ug come mesh fix una semplificata;
float MinFixVertNumPerc; // si usa il max tra MinFixVertNum e OrigMeshSize*MinFixVertNumPerc
bool UseVertexOnly; // if true all the Alignment pipeline ignores faces and works over point clouds.
double MaxShear;
double MaxScale;
MatchModeEnum MatchMode;
SampleModeEnum SampleMode;
void Dump(FILE *fp,double BoxDiag);
};
// Classe per memorizzare il risultato di un allineamento tra due mesh
// i punti si intendono nel sistema di riferimento iniziale delle due mesh.
//
// se le mesh hanno una trasformazione di base in ingresso,
// questa appare solo durante la A2Mesh::Import e poi e' per sempre dimenticata.
// Questi punti sono quindi nei sistemi di riferimento costruiti durante la Import
// la matrice Tr quella che
//
// Tr*Pmov[i]== Pfix
class Result
{
public:
int MovName;
int FixName;
Matrix44d Tr;
std::vector<Point3d> Pfix; // vertici corrispondenti su fix (rossi)
std::vector<Point3d> Nfix; // normali corrispondenti su fix (rossi)
std::vector<Point3d> Pmov; // vertici scelti su mov (verdi) prima della trasformazione in ingresso (Original Point Target)
std::vector<Point3d> Nmov; // normali scelti su mov (verdi)
Histogramf H;
Stat as;
Param ap;
ErrorCode status;
bool IsValid() {return status==SUCCESS;}
double err;
float area; // the overlapping area, a percentage as computed in Occupancy Grid.
bool operator < (const Result & rr) const {return (err< rr.err);}
bool operator <= (const Result & rr) const {return (err<=rr.err);}
bool operator > (const Result & rr) const {return (err> rr.err);}
bool operator >= (const Result & rr) const {return (err>=rr.err);}
bool operator == (const Result & rr) const {return (err==rr.err);}
bool operator != (const Result & rr) const {return (err!=rr.err);}
std::pair<double,double> ComputeAvgErr() const
{
double sum_before=0;
double sum_after=0;
for(unsigned int ii=0;ii<Pfix.size();++ii)
{
sum_before+=Distance(Pfix[ii], Pmov[ii]);
sum_after+=Distance(Pfix[ii], Tr*Pmov[ii]);
}
return std::make_pair(sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ;
}
};
/******************* Fine Classi Accessorie ************************/
static const char *ErrorMsg( ErrorCode code);
void Clear(){status=SUCCESS;}
AlignPair() { Clear(); myrnd.initialize(time(NULL)); }
/******* Data Members *********/
std::vector<A2Vertex> *mov;
A2Mesh *fix;
ErrorCode status;
AlignPair::Param ap;
math::SubtractiveRingRNG myrnd;
/**** End Data Members *********/
template < class MESH >
void ConvertMesh(MESH &M1, A2Mesh &M2)
{
tri::Append<A2Mesh,MESH>::MeshCopy(M2,M1);
}
template < class VERTEX >
void ConvertVertex(const std::vector<VERTEX> &vert1, std::vector<A2Vertex> &vert2, Box3d *Clip=0)
{
vert2.clear();
typename std::vector<VERTEX>::const_iterator vi;
A2Vertex tv;
Box3<typename VERTEX::ScalarType> bb;
if(Clip){
bb.Import(*Clip);
for(vi=vert1.begin();vi<vert1.end();++vi)
if(!(*vi).IsD() && bb.IsIn((*vi).cP())){
tv.P().Import((*vi).cP());
tv.N().Import((*vi).cN());
vert2.push_back(tv);
}
}
else
for(vi=vert1.begin();vi<vert1.end();++vi)
if(!(*vi).IsD()){
tv.P().Import((*vi).cP());
tv.N().Import((*vi).cN());
vert2.push_back(tv);
}
}
bool SampleMovVert(std::vector<A2Vertex> &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode);
bool SampleMovVertRandom(std::vector<A2Vertex> &vert, int SampleNum);
bool SampleMovVertNormalEqualized(std::vector<A2Vertex> &vert, int SampleNum);
/*
*Una volta trovati <SampleNum> coppie di punti corrispondenti se ne sceglie
al piu' <PointNum> per calcolare la trasformazione che li fa coincidere.
La scelta viene fatta in base ai due parametri PassLo e PassHi;
*/
bool ChoosePoints(
std::vector<Point3d> &Ps, // vertici corrispondenti su fix (rossi)
std::vector<Point3d> &Ns, // normali corrispondenti su fix (rossi)
std::vector<Point3d> &Pt, // vertici scelti su mov (verdi)
std::vector<Point3d> &OPt, // vertici scelti su mov (verdi)
//vector<Point3d> &Nt, // normali scelti su mov (verdi)
double PassHi,
Histogramf &H);
/*
Minimo esempio di codice per l'uso della funzione di allineamento.
AlignPair::A2Mesh Mov,Fix; // le due mesh da allineare
vector<AlignPair::A2Vertex> MovVert; // i punti sulla mov da usare per l'allineamento
Matrix44d In; In.SetIdentity(); // la trasformazione iniziale che applicata a mov la porterebbe su fix.
AlignPair aa; // l'oggetto principale.
AlignPair::Param ap;
UGrid< AlignPair::A2Mesh::face_container > UG;
Fix.LoadPly("FixMesh.ply"); // Standard ply loading
Mov.LoadPly("MovMesh.ply");
Fix.Init( Ident, false); // Inizializzazione necessaria (normali per vert,
Mov.Init( Ident, false); // info per distanza punto faccia ecc)
AlignPair::InitFix(&Fix, ap, UG); // la mesh fix viene messa nella ug.
aa.ConvertVertex(Mov.vert,MovVert); // si campiona la mesh Mov per trovare un po' di vertici.
aa.SampleMovVert(MovVert, ap.SampleNum, ap.SampleMode);
aa.mov=&MovVert; // si assegnano i membri principali dell'oggetto align pair
aa.fix=&Fix;
aa.ap = ap;
aa.Align(In,UG,res); // si spera :)
// il risultato sta nella matrice res.Tr;
res.as.Dump(stdout);
*/
bool Align(const Matrix44d &in, A2Grid &UG, A2GridVert &UGV, Result &res)
{
res.ap=ap;
bool ret=Align(UG, UGV, in, res.Tr, res.Pfix, res.Nfix, res.Pmov, res.Nmov, res.H, res.as);
res.err=res.as.LastPcl50();
res.status=status;
return ret;
}
double Abs2Perc(double val, Box3d bb) const {return val/bb.Diag();}
double Perc2Abs(double val, Box3d bb) const {return val*bb.Diag();}
/************************************************************************************
Versione Vera della Align a basso livello.
Si assume che la mesh fix sia gia' stata messa nella ug u con le debite trasformazioni.
in
************************************************************************************/
bool Align(
A2Grid &u,
A2GridVert &uv,
const Matrix44d &in, // trasformazione Iniziale che porta i punti di mov su fix
Matrix44d &out, // trasformazione calcolata
std::vector<Point3d> &Pfix, // vertici corrispondenti su src (rossi)
std::vector<Point3d> &Nfix, // normali corrispondenti su src (rossi)
std::vector<Point3d> &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target)
std::vector<Point3d> &ONmov, // normali scelti su trg (verdi)
Histogramf &H,
Stat &as);
bool InitMov(
std::vector< Point3d > &movvert,
std::vector< Point3d > &movnorm,
Box3d &movbox,
const Matrix44d &in );
static bool InitFixVert(A2Mesh *fm,
AlignPair::Param &pp,
A2GridVert &u,
int PreferredGridSize=0);
static bool InitFix(A2Mesh *fm,
AlignPair::Param &pp,
A2Grid &u,
int PreferredGridSize=0);
}; // end class
} // end namespace vcg
#endif