Merge pull request #88 from alemuntoni/move_edit_align_to_vcg
Move edit align to vcg
This commit is contained in:
Alessandro Muntoni
GitHub
commit
fb5258c157
|
|
@ -1,11 +1,21 @@
|
|||
DEPENDPATH += . ../../..
|
||||
INCLUDEPATH += . ../../.. ../../../eigenlib
|
||||
CONFIG += console c++11
|
||||
DEPENDPATH += \
|
||||
. \
|
||||
../../..
|
||||
|
||||
INCLUDEPATH += \
|
||||
. \
|
||||
../../.. \
|
||||
../../../eigenlib
|
||||
|
||||
|
||||
CONFIG += c++11
|
||||
TEMPLATE = app
|
||||
|
||||
# Mac specific Config required to avoid to make application bundles
|
||||
CONFIG -= app_bundle
|
||||
|
||||
QMAKE_CXXFLAGS += -std=c++11
|
||||
|
||||
unix {
|
||||
CONFIG(release, debug|release) {
|
||||
DEFINES *= NDEBUG
|
||||
|
|
|
|||
|
|
@ -1,6 +1,22 @@
|
|||
|
||||
TEMPLATE = subdirs
|
||||
SUBDIRS = trimesh_allocate \
|
||||
|
||||
SUBDIRS = \
|
||||
aabb_binary_tree \
|
||||
colorspace \
|
||||
#edgemesh_sampling \
|
||||
polygonmesh_base \
|
||||
polygonmesh_dual \
|
||||
polygonmesh_optimize \
|
||||
polygonmesh_polychord_collapse \
|
||||
#polygonmesh_quadsimpl \
|
||||
polygonmesh_smooth \
|
||||
#polygonmesh_zonohedra \
|
||||
space_index_2d \
|
||||
#space_minimal \
|
||||
space_packer \
|
||||
space_rasterized_packer \
|
||||
trimesh_align_pair \
|
||||
trimesh_allocate \
|
||||
trimesh_attribute \
|
||||
trimesh_attribute_saving \
|
||||
trimesh_ball_pivoting \
|
||||
|
|
@ -45,4 +61,3 @@ SUBDIRS = trimesh_allocate \
|
|||
trimesh_voronoiatlas \
|
||||
trimesh_voronoiclustering \
|
||||
trimesh_voronoisampling \
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,149 @@
|
|||
/****************************************************************************
|
||||
* 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. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/*! \file trimesh_align_pair.cpp
|
||||
\ingroup code_sample
|
||||
|
||||
\brief the minimal example for aligning two meshes
|
||||
|
||||
This file contain a minimal example for aligning two meshs.
|
||||
|
||||
Example call:
|
||||
./trimesh_align_pair mesh1.ply mesh2.ply output.ply
|
||||
|
||||
output.ply will contain mesh2.ply rotated in order to be aligned to mesh1.ply
|
||||
|
||||
*/
|
||||
|
||||
#include <vcg/complex/complex.h>
|
||||
|
||||
#include <vcg/complex/algorithms/align_pair.h>
|
||||
|
||||
#include <wrap/io_trimesh/import.h>
|
||||
#include <wrap/io_trimesh/export_ply.h>
|
||||
|
||||
class MyFace;
|
||||
class MyVertex;
|
||||
|
||||
struct MyUsedTypes :
|
||||
public vcg::UsedTypes< vcg::Use<MyVertex>::AsVertexType,
|
||||
vcg::Use<MyFace>::AsFaceType>
|
||||
{};
|
||||
|
||||
class MyVertex :
|
||||
public vcg::Vertex< MyUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3d, vcg::vertex::Color4b, vcg::vertex::BitFlags>
|
||||
{};
|
||||
|
||||
class MyFace :
|
||||
public vcg::Face < MyUsedTypes, vcg::face::VertexRef, vcg::face::Normal3d, vcg::face::FFAdj, vcg::face::Mark, vcg::face::BitFlags >
|
||||
{};
|
||||
|
||||
class MyMesh :
|
||||
public vcg::tri::TriMesh< std::vector<MyVertex>, std::vector<MyFace> >
|
||||
{};
|
||||
|
||||
using namespace vcg;
|
||||
using namespace std;
|
||||
|
||||
std::vector<vcg::Point3d>* vcg::PointMatchingScale::fix;
|
||||
std::vector<vcg::Point3d>* vcg::PointMatchingScale::mov;
|
||||
vcg::Box3d vcg::PointMatchingScale::b;
|
||||
|
||||
int main(int argc,char ** argv)
|
||||
{
|
||||
if(argc<3) {
|
||||
printf("Usage: trimesh_smooth <filename> <filename>\n");
|
||||
return 0;
|
||||
}
|
||||
std::string outputname = "output.ply";
|
||||
if (argc == 4){
|
||||
outputname = std::string(argv[3]);
|
||||
}
|
||||
|
||||
MyMesh m1, m2;
|
||||
|
||||
//open first mesh
|
||||
int err = tri::io::Importer<MyMesh>::Open(m1,argv[1]);
|
||||
if(err) { // all the importers return 0 in case of success
|
||||
printf("Error in reading %s: '%s'\n", argv[1], tri::io::Importer<MyMesh>::ErrorMsg(err));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
//open second mesh
|
||||
err = tri::io::Importer<MyMesh>::Open(m2,argv[2]);
|
||||
if(err) { // all the importers return 0 in case of success
|
||||
printf("Error in reading %s: '%s'\n", argv[2], tri::io::Importer<MyMesh>::ErrorMsg(err));
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
//update normals
|
||||
vcg::tri::UpdateNormal<MyMesh>::PerVertexNormalizedPerFace(m1);
|
||||
vcg::tri::UpdateNormal<MyMesh>::PerVertexNormalizedPerFace(m2);
|
||||
|
||||
////PARAMS
|
||||
vcg::AlignPair::Result result;
|
||||
vcg::AlignPair::Param ap;
|
||||
vcg::AlignPair::A2Mesh fix;
|
||||
vcg::AlignPair aa;
|
||||
|
||||
// 1) Convert fixed mesh and put it into the grid.
|
||||
aa.convertMesh<MyMesh>(m1,fix);
|
||||
|
||||
vcg::AlignPair::A2Grid UG;
|
||||
vcg::AlignPair::A2GridVert VG;
|
||||
|
||||
if(m1.fn==0 || ap.UseVertexOnly) {
|
||||
fix.initVert(vcg::Matrix44d::Identity());
|
||||
vcg::AlignPair::InitFixVert(&fix,ap,VG);
|
||||
}
|
||||
else {
|
||||
fix.init(vcg::Matrix44d::Identity());
|
||||
vcg::AlignPair::initFix(&fix, ap, UG);
|
||||
}
|
||||
|
||||
|
||||
// 2) Convert the second mesh and sample a <ap.SampleNum> points on it.
|
||||
std::vector<vcg::AlignPair::A2Vertex> tmpmv;
|
||||
aa.convertVertex(m2.vert,tmpmv);
|
||||
aa.sampleMovVert(tmpmv, ap.SampleNum, ap.SampleMode);
|
||||
|
||||
aa.mov=&tmpmv;
|
||||
aa.fix=&fix;
|
||||
aa.ap = ap;
|
||||
|
||||
//use identity as first matrix
|
||||
vcg::Matrix44d In;
|
||||
In.SetIdentity();
|
||||
|
||||
// Perform the ICP algorithm
|
||||
aa.align(In,UG,VG,result);
|
||||
|
||||
//rotate m2 using the resulting transformation
|
||||
tri::UpdatePosition<MyMesh>::Matrix(m2, result.Tr, true);
|
||||
tri::UpdateBounding<MyMesh>::Box(m2);
|
||||
|
||||
//saves the rotated mesh
|
||||
tri::io::ExporterPLY<MyMesh>::Save(m2 ,outputname.c_str());
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
include(../common.pri)
|
||||
|
||||
TARGET = trimesh_align_pair
|
||||
|
||||
SOURCES += \
|
||||
trimesh_align_pair.cpp \
|
||||
../../../wrap/ply/plylib.cpp
|
||||
|
|
@ -0,0 +1,910 @@
|
|||
/****************************************************************************
|
||||
* 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 <stdio.h>
|
||||
#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;
|
||||
}
|
||||
|
||||
/*
|
||||
This function is used to choose remove outliers after each ICP iteration.
|
||||
All the points with a distance over the given Percentile are discarded.
|
||||
It uses two parameters
|
||||
MaxPointNum an (unused) hard limit on the number of points that are chosen
|
||||
MinPointNum the minimum number of points that have to be chosen to be usable
|
||||
*/
|
||||
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
|
||||
|
||||
************************************************************************************/
|
||||
|
||||
/*
|
||||
The Main ICP alignment Function:
|
||||
It assumes that:
|
||||
we have two meshes:
|
||||
- Fix the mesh that does not move and stays in the spatial indexing structure.
|
||||
- Mov the mesh we 'move' e.g. the one for which we search the transforamtion.
|
||||
|
||||
requires normalize normals for vertices AND faces
|
||||
Allinea due mesh;
|
||||
Assume che:
|
||||
la uniform grid sia gia' inizializzata con la mesh fix
|
||||
*/
|
||||
inline bool align(
|
||||
A2Grid &u,
|
||||
A2GridVert &uv,
|
||||
const Matrix44d &in, // starting transformation that matches mov points to fix mesh
|
||||
Matrix44d &out, // computed transformation
|
||||
std::vector<Point3d> &pfix, // (red) corresponding vertices on src
|
||||
std::vector<Point3d> &nfix, // (red) corresponding normals on src
|
||||
std::vector<Point3d> &opmov, // chosen vertices on trg (verdi) before the input transormation (Original Point Target)
|
||||
std::vector<Point3d> &onmov, // chosen normals on trg (verdi)
|
||||
Histogramf &h,
|
||||
Stat &as)
|
||||
{
|
||||
std::vector<char> beyondCntVec; // flag vector to set the movverts that we should not use
|
||||
// every time that a vertex is at a distance beyound max dist, its counter is incremented;
|
||||
// movverts that has been discarded more than MaxCntDist times will not be considered anymore
|
||||
const int maxBeyondCnt = 3;
|
||||
std::vector< Point3d > movvert;
|
||||
std::vector< Point3d > movnorm;
|
||||
std::vector<Point3d> pmov; // vertices chosen after the transformation
|
||||
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()) ) ;
|
||||
|
||||
// the following tuns will use as a initial transformation, the one that has been just found.
|
||||
// in the next loops the starting matrix will be this one.
|
||||
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))));
|
||||
//as.dump(stderr);
|
||||
} 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;
|
||||
}
|
||||
|
||||
/*
|
||||
* Function called by Align at every cycle.
|
||||
* It fills the <MovVert> and <MovNorm> vectors with the coordinates and normals
|
||||
* taken from the the vertex vector "mov" of the mesh to move according to the
|
||||
* matrix <In>.
|
||||
* It computes also the new bounding box of the transformed vertices
|
||||
*/
|
||||
inline bool initMov(
|
||||
std::vector< Point3d > &movvert,
|
||||
std::vector< Point3d > &movnorm,
|
||||
Box3d &movbox,
|
||||
const Matrix44d &in )
|
||||
{
|
||||
Point3d pp, nn;
|
||||
|
||||
movvert.clear();
|
||||
movnorm.clear();
|
||||
movbox.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);
|
||||
movbox.Add(pp);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static inline bool InitFixVert(
|
||||
A2Mesh *fm,
|
||||
AlignPair::Param &pp,
|
||||
A2GridVert &u,
|
||||
int preferredGridSize=0)
|
||||
{
|
||||
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);
|
||||
|
||||
//Inserisco la src nella griglia
|
||||
if (preferredGridSize == 0)
|
||||
preferredGridSize = int(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;
|
||||
}
|
||||
|
||||
static inline bool initFix(
|
||||
A2Mesh *fm,
|
||||
AlignPair::Param &pp,
|
||||
A2Grid &u,
|
||||
int preferredGridSize=0)
|
||||
{
|
||||
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));
|
||||
|
||||
u.SetBBox(bb2);
|
||||
|
||||
//Inserisco la src nella griglia
|
||||
if (preferredGridSize == 0)
|
||||
preferredGridSize = int(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;
|
||||
}
|
||||
|
||||
}; // end class
|
||||
|
||||
} // end namespace vcg
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
/****************************************************************************
|
||||
* 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_POINT_MATCHING_SCALE
|
||||
#define VCG_POINT_MATCHING_SCALE
|
||||
|
||||
#include <vcg/math/matrix44.h>
|
||||
#include <vcg/space/point3.h>
|
||||
#include <vcg/space/box3.h>
|
||||
#include <wrap/newuoa/include/newuoa.h>
|
||||
|
||||
namespace vcg {
|
||||
|
||||
template <class Scalar>
|
||||
struct RotoTranslation
|
||||
{
|
||||
RotoTranslation(){}
|
||||
Scalar _v[6];
|
||||
void toMatrix(vcg::Matrix44<Scalar> & m)
|
||||
{
|
||||
vcg::Matrix44<Scalar> rot,tra;
|
||||
rot.FromEulerAngles(_v[0],_v[1],_v[2]);
|
||||
tra.SetTranslate(vcg::Point3<Scalar>(_v[3],_v[4],_v[5]));
|
||||
m = tra * rot;
|
||||
}
|
||||
};
|
||||
|
||||
class PointMatchingScale {
|
||||
private:
|
||||
static std::vector<vcg::Point3d> *fix;
|
||||
static std::vector<vcg::Point3d> *mov;
|
||||
static vcg::Box3d b;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Compute a scaling transformation that bring PMov point as close as possible to Pfix
|
||||
*/
|
||||
static void computeScalingMatchMatrix(
|
||||
vcg::Matrix44d &res,
|
||||
std::vector<vcg::Point3d> &Pfix,
|
||||
std::vector<vcg::Point3d> &Pmov)
|
||||
{
|
||||
fix = &Pfix;
|
||||
mov = &Pmov;
|
||||
b.SetNull();
|
||||
for(std::vector<vcg::Point3d>::iterator i = Pmov.begin(); i != Pmov.end(); ++i)
|
||||
b.Add(*i);
|
||||
|
||||
double scale = 1.0;
|
||||
min_newuoa(1,&scale,errorScale);
|
||||
|
||||
res.SetTranslate( b.Center()*(1.0-scale));
|
||||
res[0][0] = res[1][1] = res[2][2] = scale;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute a rototranslation + scaling transformation that bring PMov point as close as possible to Pfix
|
||||
*/
|
||||
static void computeRotoTranslationScalingMatchMatrix(
|
||||
vcg::Matrix44d &res,
|
||||
std::vector<vcg::Point3d> &Pfix,
|
||||
std::vector<vcg::Point3d> &Pmov)
|
||||
{
|
||||
fix = &Pfix;
|
||||
mov = &Pmov;
|
||||
b.SetNull();
|
||||
for(std::vector<vcg::Point3d>::iterator i = Pmov.begin(); i != Pmov.end(); ++i)
|
||||
b.Add(*i);
|
||||
|
||||
double x[7]={1.0,0.0,0.0,0.0,0.0,0.0,0.0};
|
||||
min_newuoa(7,&x[0],errorRotoTranslationScale);
|
||||
|
||||
// rtm = rototranslation
|
||||
RotoTranslation<double> rt;
|
||||
vcg::Matrix44d rtm;
|
||||
memcpy(&rt._v[0],&x[1],6*sizeof(double));
|
||||
rt.toMatrix(rtm);
|
||||
|
||||
// res= scaling w.r.t. barycenter
|
||||
res.SetTranslate( b.Center()*(1.0-x[0]));
|
||||
res[0][0] = res[1][1] = res[2][2] = x[0];
|
||||
res = rtm*res;
|
||||
}
|
||||
|
||||
static double errorScale(int n, double *x)
|
||||
{
|
||||
assert(n==1); (void)n;
|
||||
double dist = 0;
|
||||
std::vector<vcg::Point3d>::iterator i = mov->begin();
|
||||
std::vector<vcg::Point3d>::iterator ifix = fix->begin();
|
||||
for(; i != mov->end(); ++i,++ifix)
|
||||
dist += vcg::SquaredDistance(((*i)-b.Center())*(*x)+b.Center() , *ifix);
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
static double errorRotoTranslationScale(int n, double *x) {
|
||||
assert(n==7); (void)n;
|
||||
double dist = 0;
|
||||
std::vector<vcg::Point3d>::iterator i = mov->begin();
|
||||
std::vector<vcg::Point3d>::iterator ifix = fix->begin();
|
||||
|
||||
RotoTranslation<double> rt;
|
||||
vcg::Matrix44d m;
|
||||
memcpy(&rt._v[0],&x[1],6*sizeof(double));
|
||||
rt.toMatrix(m);
|
||||
|
||||
for(; i != mov->end(); ++i,++ifix) {
|
||||
dist += vcg::SquaredDistance( m*(((*i)-b.Center())*(x[0])+b.Center()),*ifix);
|
||||
}
|
||||
return dist;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -192,7 +192,8 @@ public:
|
|||
template <class Q>
|
||||
static inline Matrix44 Construct( const Matrix44<Q> & b )
|
||||
{
|
||||
Matrix44<T> tmp; tmp.FromMatrix(b);
|
||||
Matrix44<T> tmp;
|
||||
tmp.FromMatrix(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1009,7 +1009,7 @@ namespace vcg
|
|||
* \return
|
||||
*/
|
||||
inline bool operator[](const typename UniformGrid::CellCoordinate &at) { return m_Mask[at.X()][at.Y()][at.Z()]; }
|
||||
inline bool& GetFlag(const int i, const int j, const int k)const { return m_Mask[i][j][k]; }
|
||||
inline const bool& GetFlag(const int i, const int j, const int k)const { return m_Mask[i][j][k]; }
|
||||
inline void SetFlat(const int i, const int j, const int k) { m_Mask[i][j][k] = true; }
|
||||
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue