diff --git a/vcg/complex/algorithms/align_pair.h b/vcg/complex/algorithms/align_pair.h new file mode 100644 index 00000000..8fbc0d4b --- /dev/null +++ b/vcg/complex/algorithms/align_pair.h @@ -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 +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +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::AsVertexType, + vcg::Use::AsFaceType >{}; + + class A2Vertex : public vcg::Vertex {}; + 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, std::vector > + { + 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::Matrix(*this, Tr); + tri::UpdateNormal::NormalizePerVertex(*this); + tri::UpdateBounding::Box(*this); + return true; + } + inline bool Init(const Matrix44d &Tr) { + Matrix44d Idn; Idn.SetIdentity(); + tri::Clean::RemoveUnreferencedVertex(*this); + if (Tr != Idn) + tri::UpdatePosition::Matrix(*this, Tr); + tri::UpdateNormal::PerVertexNormalizedPerFaceNormalized(*this); + tri::UpdateFlags::FaceBorderFromNone(*this); + tri::UpdateBounding::Box(*this); + + return true; + } + }; + + typedef A2Mesh::FaceContainer FaceContainer; + typedef A2Mesh::FaceType FaceType; + typedef GridStaticPtr A2Grid; + typedef GridStaticPtr 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 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(CLOCKS_PER_SEC / 1000,1); + assert(i\n"); + fprintf(fp, " Mindist 50ile Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n"); + for (unsigned int qi = 0; qi < I.size(); ++qi) + fprintf( + fp, " %8.5f %9.6f %8.5f %5.3f %8.5f %9.6f %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); + fprintf(fp, "\n"); + } + + // Restituisce true se nelle ultime 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' + // e almeno 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 Pfix; // vertici corrispondenti su fix (rossi) + std::vector Nfix; // normali corrispondenti su fix (rossi) + std::vector Pmov; // vertici scelti su mov (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector 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 ComputeAvgErr() const + { + double sum_before=0; + double sum_after=0; + for(unsigned int ii=0;ii *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::MeshCopy(M2,M1); + } + + template < class VERTEX > + void ConvertVertex(const std::vector &vert1, std::vector &vert2, Box3d *Clip=0) + { + + vert2.clear(); + typename std::vector::const_iterator vi; + A2Vertex tv; + Box3 bb; + if(Clip){ + bb.Import(*Clip); + for(vi=vert1.begin();vi &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode); + bool SampleMovVertRandom(std::vector &vert, int SampleNum); + bool SampleMovVertNormalEqualized(std::vector &vert, int SampleNum); +/* +*Una volta trovati coppie di punti corrispondenti se ne sceglie +al piu' per calcolare la trasformazione che li fa coincidere. +La scelta viene fatta in base ai due parametri PassLo e PassHi; +*/ + bool ChoosePoints( + std::vector &Ps, // vertici corrispondenti su fix (rossi) + std::vector &Ns, // normali corrispondenti su fix (rossi) + std::vector &Pt, // vertici scelti su mov (verdi) + std::vector &OPt, // vertici scelti su mov (verdi) + //vector &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 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 &Pfix, // vertici corrispondenti su src (rossi) + std::vector &Nfix, // normali corrispondenti su src (rossi) + std::vector &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target) + std::vector &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