manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
vcglib/vcg/complex/algorithms/align_pair.h

835 lines
26 KiB
C++
Raw Blame History

/****************************************************************************
* 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>
#include <vcg/complex/algorithms/point_matching_scale.h>
namespace vcg {
/*************************************************************************
AlignPair
Classe per gestire un allineamento tra DUE sole mesh.
**************************************************************************/
class AlignPair {
public:
AlignPair()
{
clear();
myrnd.initialize(time(NULL));
}
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 inline const char* 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;
}
void clear()
{
status=SUCCESS;
}
/******* 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);
}
}
}
}
inline bool sampleMovVert(
std::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;
}
}
inline bool SampleMovVertRandom(std::vector<A2Vertex> &vert, int sampleNum)
{
if (int(vert.size()) <= sampleNum)
return true;
for (int i = 0; i < sampleNum; ++i) {
int pos = myrnd.generate(vert.size());
assert(pos >= 0 && pos < int(vert.size()));
std::swap(vert[i], vert[pos]);
}
vert.resize(sampleNum);
return true;
}
bool SampleMovVertNormalEqualized(std::vector<A2Vertex> &vert, int sampleNum)
{
std::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
std::vector<std::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(int(i));
}
// vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket
std::vector <int> BKTpos(BKT.size(), 0);
if (sampleNum >= int(vert.size()))
sampleNum = vert.size() - 1;
for (int i = 0; i < sampleNum;) {
int ind = myrnd.generate(BKT.size()); // Scelgo un Bucket
int &CURpos = BKTpos[ind];
std::vector<int> &CUR = BKT[ind];
if (CURpos<int(CUR.size())) {
std::swap(CUR[CURpos], CUR[CURpos + myrnd.generate(BKT[ind].size() - CURpos)]);
std::swap(vert[i], vert[CUR[CURpos]]);
++BKTpos[ind];
++i;
}
}
vert.resize(sampleNum);
return true;
}
/*
* 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;
*/
inline 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)
{
const int N = ap.MaxPointNum;
double newmaxd = h.Percentile(float(passHi));
int sz = int(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){
std::swap(ps[index], ps[fnd]);
std::swap(ns[index], ns[fnd]);
std::swap(pt[index], pt[fnd]);
std::swap(opt[index], opt[fnd]);
++fnd;
}
else {
std::swap(ps[index], ps[lastgood]);
std::swap(ns[index], ns[lastgood]);
std::swap(pt[index], pt[lastgood]);
std::swap(opt[index], opt[lastgood]);
lastgood--;
}
}
ps.resize(fnd);
ns.resize(fnd);
pt.resize(fnd);
opt.resize(fnd);
if ((int)ps.size() < ap.MinPointNum){
printf("Troppi pochi punti!\n");
ps.clear();
ns.clear();
pt.clear();
opt.clear();
return false;
}
return true;
}
/*
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
************************************************************************************/
inline 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)
{
std::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;
std::vector< Point3d > movvert;
std::vector< Point3d > movnorm;
std::vector<Point3d> Pmov; // vertici scelti dopo la trasf iniziale
status = SUCCESS;
int tt0 = clock();
out = in;
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();
beyondCntVec.resize(mov->size(), 0);
/**************** BEGIN ICP LOOP ****************/
do {
Stat::IterInfo ii;
Box3d movbox;
InitMov(movvert, movnorm, movbox, out);
h.SetRange(0.0f, float(startMinDist), 512, 2.5f);
pfix.clear();
nfix.clear();
Pmov.clear();
opmov.clear();
onmov.clear();
int tts0 = clock();
ii.MinDistAbs = startMinDist;
int LocSampleNum = std::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])) {
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++;
}
else {
beyondCntVec[i] = maxBeyondCnt + 1;
}
}
} // 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;
}
}
Matrix44d newout;
switch (ap.MatchMode){
case AlignPair::Param::MMSimilarity:
vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(newout, pfix, Pmov);
break;
case AlignPair::Param::MMRigid:
ComputeRigidMatchMatrix(pfix, Pmov, newout);
break;
default:
status = UNKNOWN_MODE;
ii.Time = clock();
as.I.push_back(ii);
return false;
}
//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 * out;
assert(Pfix.size() == Pmov.size());
int tts2 = clock();
ttsearch += tts1 - tts0;
ttleast += tts2 - tts1;
ii.pcl50 = h.Percentile(.5);
ii.pclhi = h.Percentile(float(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 = std::max(ap.MinDistAbs*ap.MinMinDistPerc, std::min(startMinDist, 5.0*h.Percentile(float(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();
out[3][0] = 0; out[3][1] = 0; out[3][2] = 0; out[3][3] = 1;
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;
}
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
Reference in New Issue View Git Blame Copy Permalink