manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #177 from gabryon99/devel 

Devel: Add MeshTree and OccupancyGrid classes for Global Alignment
This commit is contained in:
Alessandro Muntoni 2021-09-17 14:05:37 +02:00 committed by GitHub
parent 1980f1a775 b98261e29f
commit bda3161f46
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 1762 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -72,6 +72,9 @@ set(VCG_HEADERS
vcg/complex/algorithms/polygonal_algorithms.h vcg/complex/algorithms/polygonal_algorithms.h
vcg/complex/algorithms/inertia.h vcg/complex/algorithms/inertia.h
vcg/complex/algorithms/mesh_assert.h vcg/complex/algorithms/mesh_assert.h
vcg/complex/algorithms/occupancy_grid.h
vcg/complex/algorithms/meshtree.h
vcg/complex/algorithms/align_global.h
vcg/complex/algorithms/cut_tree.h vcg/complex/algorithms/cut_tree.h
vcg/complex/algorithms/nring.h vcg/complex/algorithms/nring.h
vcg/complex/algorithms/tetra/tetfuse_collapse.h vcg/complex/algorithms/tetra/tetfuse_collapse.h

700
vcg/complex/algorithms/align_global.h Normal file
View File

@ -0,0 +1,700 @@
#include <list>
#include <queue>
#include <wrap/callback.h>
#ifndef MESHLAB_ALIGNGLOBAL_H
#define MESHLAB_ALIGNGLOBAL_H
namespace vcg {
class AlignGlobal {
public:
/**
* Forward declaration for the `VirtAlign` class.
*/
class Node;
/**
* Allineamento virtuale tra due mesh (estratto da un alignresult).
* Nota Importante: la trasformazione e i punti qui memorizzati si intendono al netto delle trasf di base delle due mesh in gioco.
* Quindi se qualcuno sposta una mesh le pos dei punti sono ancora valide ma non la trasf da applicarvi.
*/
class VirtAlign
{
public:
AlignGlobal::Node *Fix, *Mov; // allineamento tra i e j
std::vector<vcg::Point3d> FixP; // punti su Fix
std::vector<vcg::Point3d> MovP; // punti su Mov
std::vector<vcg::Point3d> FixN; // Normali su Fix
std::vector<vcg::Point3d> MovN; // Normali su Mov
vcg::Matrix44d M2F; //la matrice da applicare ai punti di Mov per ottenere quelli su Fix
vcg::Matrix44d F2M; //la matrice da applicare ai punti di Fix per ottenere quelli su Mov
/*
Nel caso semplificato che le mesh avessero come trasf di base l'identita' deve valere:
N2A(N).Apply( P(N)) ~= AdjP(N)
A2N(N).Apply(AdjP(N)) ~= P(N)
In generale un nodo N qualsiasi dell'VirtAlign vale che:
N2A(N).Apply( N->M.Apply( P(N)) ) ~= AdjN(N)->M.Apply( AdjP(N) );
A2M(N).Apply( AdjN(N)->M.Apply(AdjP(N)) ) ~= N->M.Apply( P(N) );
in cui il ~= significa uguale al netto dell'errore di allineamento.
Per ottenere i virtualmate relativi ad un nodo n:
*/
inline vcg::Matrix44d &N2A(AlignGlobal::Node *n) {if(n==Fix) return F2M; else return M2F;}
inline vcg::Matrix44d &A2N(AlignGlobal::Node *n) {if(n==Fix) return M2F; else return F2M;}
inline std::vector<vcg::Point3d> &P(AlignGlobal::Node *n) {if(n==Fix) return FixP; else return MovP;}
inline std::vector<vcg::Point3d> &N(AlignGlobal::Node *n) {if(n==Fix) return FixN; else return MovN;}
inline std::vector<vcg::Point3d> &AdjP(AlignGlobal::Node *n) {if(n==Fix) return MovP; else return FixP;}
inline std::vector<vcg::Point3d> &AdjN(AlignGlobal::Node *n) {if(n==Fix) return MovN; else return FixN;}
AlignGlobal::Node *Adj(Node *n) const {
assert(n == Fix || n == Mov);
if (n == Fix) return Mov;
return Fix;
}
bool Check() const {
if (FixP.size() != MovP.size()) return false;
Point3d mp, fp;
double md = 0, fd = 0;
double md2 = 0, fd2 = 0;
Matrix44d &MovTr=Mov->M;
Matrix44d &FixTr=Fix->M;
for (std::size_t i = 0; i < FixP.size(); ++i) {
mp = MovTr * MovP[i];
fp = FixTr * FixP[i];
md += Distance(fp, M2F * mp);
md2 += SquaredDistance(fp, M2F * mp);
fd += Distance(mp, F2M * fp);
fd2 += SquaredDistance(mp, F2M * fp);
}
int nn = static_cast<int>(MovP.size());
std::fprintf(stdout, "Arc %3i -> %3i : %i pt\n", Fix->id, Mov->id, nn);
std::fprintf(stdout, "SquaredSum Distance %7.3f %7.3f Avg %7.3f %7.3f\n", fd2, md2, fd2/nn, md2/nn);
std::fprintf(stdout, " Sum Distance %7.3f %7.3f Avg %7.3f %7.3f\n", fd , md , fd/nn, md/nn);
return true;
}
};
class Node {
public:
int id; // id della mesh a cui corrisponde il nodo
int sid; // Subgraph id;
Matrix44d M; // La matrice che mette la mesh nella sua posizione di base;
std::list<VirtAlign*> Adj;
/***
* True if the node is inside the active set
*/
bool Active;
/***
* False if it's dormant
*/
bool Queued;
bool Discarded;
Node() : id{-1}, Active{false}, Discarded{false}, Queued{false} {}
// Allinea un nodo con tutti i suoi vicini
double AlignWithActiveAdj(bool Rigid) {
std::printf("--- AlignWithActiveAdj --- \nMoving node %i with respect to nodes:", id);
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
if ((*li)->Adj(this)->Active) {
std::printf(" %i,", (*li)->Adj(this)->id);
}
}
std::printf("\n");
//printf("Base Matrix of Node %i\n",id);print(M);
// Step 1; Costruiamo le due liste di punti da allineare
std::vector<Point3d> FixP, MovP, FixN, MovN;
Box3d FixBox, MovBox;
FixBox.SetNull(); MovBox.SetNull();
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
// scorro tutti i nodi adiacenti attivi
if ((*li)->Adj(this)->Active) {
//printf("Base Matrix of Node %i adj to %i\n",id,(*li)->Adj(this)->id);
//print((*li)->Adj(this)->M);
std::vector<Point3d> &AP=(*li)->AdjP(this); // Punti sul nodo adiacente corrente;
std::vector<Point3d> &AN=(*li)->AdjN(this); // Normali sul nodo adiacente corrente;
//printf("Transf that bring points of %i onto %i\n",id,(*li)->Adj(this)->id);
//print((*li)->A2N(this));
//printf("Transf that bring points of %i onto %i\n",(*li)->Adj(this)->id,id);
//print((*li)->N2A(this));
vcg::Point3d pf, nf;
vcg::Point3d pm;
for (std::size_t i = 0; i < AP.size(); ++i) {
pf = (*li)->Adj(this)->M*AP[i]; // i punti fissi sono quelli sulla sup degli adiacenti messi nella loro pos corrente
FixP.push_back(pf);
FixBox.Add(pf);
nf=(*li)->Adj(this)->M*Point3d(AP[i]+AN[i])-pf;
nf.Normalize();
FixN.push_back(nf);
pm = (*li)->A2N(this)*pf;
MovP.push_back(pm); // i punti che si muovono sono quelli sul adj trasformati in modo tale da cascare sul nodo corr.
MovBox.Add(pm);
}
}
}
vcg::Matrix44d out;
//if(Rigid) ret=ComputeRigidMatchMatrix(out,FixP,MovP);
//else ret=ComputeMatchMatrix2(out,FixP,FixN,MovP);
if (Rigid) {
ComputeRigidMatchMatrix(FixP,MovP,out);
}
else {
vcg::PointMatchingScale::computeRotoTranslationScalingMatchMatrix(out, FixP, MovP);
}
Matrix44d outIn=vcg::Inverse(out);
//double maxdiff = MatrixNorm(out);
double maxdiff = MatrixBoxNorm(out,FixBox);
// printf("Computed Transformation:\n"); print(out);printf("--\n");
// printf("Collected %i points . Err = %f\n",FixP.size(),maxdiff);
// La matrice out calcolata e' quella che applicata ai punti MovP li porta su FixP, quindi i punti della mesh corrente
// La nuova posizione di base della mesh diventa quindi
// M * out
// infatti se considero un punto della mesh originale applicarci la nuova matricie significa fare
// p * M * out
// M=M*out; //--Orig
M = out * M;
// come ultimo step occorre applicare la matrice trovata a tutti gli allineamenti in gioco.
// scorro tutti i nodi adiacenti attivi
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
//print((*li)->N2A(this));
//print((*li)->A2N(this));printf("--\n");
(*li)->N2A(this)=(*li)->N2A(this)*outIn;
(*li)->A2N(this)=(*li)->A2N(this)*out ;
//print((*li)->N2A(this));
//print((*li)->A2N(this));printf("--\n");
}
return maxdiff;
}
double MatrixNorm(vcg::Matrix44d &NewM) const {
double maxDiff = 0;
vcg::Matrix44d diff;
diff.SetIdentity();
diff = diff-NewM;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
maxDiff += (diff[i][j] * diff[i][j]);
}
}
return maxDiff;
}
double MatrixBoxNorm(vcg::Matrix44d &NewM, vcg::Box3d &bb) const {
double maxDiff = 0;
vcg::Point3d pt;
pt = Point3d(bb.min[0], bb.min[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.max[0], bb.min[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.min[0], bb.max[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.max[0], bb.max[1], bb.min[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.min[0], bb.min[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.max[0], bb.min[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.min[0], bb.max[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
pt = Point3d(bb.max[0], bb.max[1], bb.max[2]); maxDiff = std::max(maxDiff, Distance(pt, NewM * pt));
return maxDiff;
}
int PushBackActiveAdj(std::queue<Node *> &Q) {
assert(Active);
int count = 0;
AlignGlobal::Node *pt;
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
pt = (*li)->Adj(this);
if (pt->Active && !pt->Queued) {
++count;
pt->Queued=true;
Q.push(pt);
}
}
return count;
}
int DormantAdjNum() {
int count = 0;
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
if (!(*li)->Adj(this)->Active) ++count;
}
return count;
}
int ActiveAdjNum() {
int count = 0;
for (auto li = std::begin(Adj); li != std::end(Adj); ++li) {
if ((*li)->Adj(this)->Active) ++count;
}
return count;
}
};
class SubGraphInfo {
public:
int sid;
int size;
Node *root;
};
std::list<Node> N;
std::list<VirtAlign*> A;
/**
* Descrittori delle componenti connesse, riempito dalla ComputeConnectedComponents
*/
std::list<SubGraphInfo> CC;
static inline void LOG( FILE *fp, const char * f, ... ) {
if (fp == 0) return;
va_list marker;
va_start(marker, f);
std::vfprintf(fp, f, marker);
va_end(marker);
std::fflush(fp);
}
inline int DormantNum() const {
int count = 0;
for (auto li = std::begin(N); li != std::end(N); ++li) {
if (!(*li).Active) ++count;
}
return count;
}
inline int ActiveNum() const {
int count = 0;
for (auto li = std::begin(N); li != std::end(N); ++li) {
if ((*li).Active) ++count;
}
return count;
}
bool CheckGraph() {
std::vector<bool> Visited(N.size(), false);
std::stack<AlignGlobal::Node*> st;
st.push(&(*N.begin()));
while (!st.empty()) {
AlignGlobal::Node *cur=st.top();
st.pop();
// std::printf("Visiting node %i\n",cur->id);
for (auto li = std::begin(cur->Adj); li != std::end(cur->Adj); ++li) {
if (!Visited[(*li)->Adj(cur)->id]) {
Visited[(*li)->Adj(cur)->id] = true;
st.push((*li)->Adj(cur));
}
}
}
size_t cnt = std::count(std::begin(Visited), std::end(Visited), true);
std::printf("Nodes that can be reached from root %zu on %zu \n", cnt, N.size());
return (cnt == N.size());
}
void Dump(FILE *fp) const {
std::fprintf(fp, "Alignment Graph of %lu nodes and %lu arcs\n", N.size(), A.size());
// list<VirtAlign *>::iterator li;
// for(li=A.begin();li!=A.end();++li)
// printf("Arc : %3i ->%3i\n",(*li)->Fix->id,(*li)->Mov->id);
}
int ComputeConnectedComponents() {
std::printf("Building Connected Components on a graph with %lu nodes and %lu arcs\n", N.size(), A.size());
CC.clear();
std::stack<AlignGlobal::Node *> ToReach; // nodi ancora da visitare
std::stack<AlignGlobal::Node *> st; // nodi che si stanno visitando
for (auto li = std::begin(N); li != std::end(N); ++li) {
(*li).sid = -1;
ToReach.push(&*li);
}
int cnt = 0;
while (!ToReach.empty()) {
SubGraphInfo sg;
st.push(&*ToReach.top());
ToReach.pop();
assert(st.top()->sid==-1);
sg.root=st.top();
sg.sid=cnt;
sg.size=0;
st.top()->sid=cnt;
while (!st.empty()) {
AlignGlobal::Node *cur=st.top();
st.pop();
++sg.size;
assert(cur->sid==cnt);
// std::printf("Visiting node %2i %2i\n",cur->id,cur->sid);
for (auto li = std::begin(cur->Adj); li != std::end(cur->Adj); ++li) {
if ((*li)->Adj(cur)->sid == -1) {
(*li)->Adj(cur)->sid=cnt;
st.push((*li)->Adj(cur));
}
else {
assert((*li)->Adj(cur)->sid == cnt);
}
}
}
cnt++;
CC.push_back(sg);
while (!ToReach.empty() && ToReach.top()->sid != -1) ToReach.pop();
}
return cnt;
}
void Clear() {
for (auto li = std::begin(A); li != std::end(A); ++li) {
delete (*li);
}
N.clear();
A.clear();
}
void MakeAllDormant() {
for (auto li = std::begin(N); li != std::end(N); ++li) {
(*li).Active=false;
}
}
bool GlobalAlign(const std::map<int, std::string> &Names, const double epsilon, int maxiter, bool Rigid, FILE *elfp, vcg::CallBackPos* cb) {
double change;
int step = 0, localmaxiter;
if (cb != NULL) cb(0, "Global Alignment...");
AlignGlobal::LOG(elfp,"----------------\n----------------\nGlobalAlignment (target eps %7.3f)\n", epsilon);
std::queue<AlignGlobal::Node *> Q;
MakeAllDormant();
AlignGlobal::Node *curr = ChooseDormantWithMostDormantLink();
curr->Active = true;
int cursid = curr->sid;
AlignGlobal::LOG(elfp, "Root node %i '%s' with %i dormant link\n", curr->id, Names.find(curr->id)->second.c_str(), curr->DormantAdjNum());
while (DormantNum() > 0) {
AlignGlobal::LOG(elfp,"---------\nGlobalAlignment loop DormantNum = %i\n", DormantNum());
curr = ChooseDormantWithMostActiveLink();
if (!curr) {
// la componente connessa e' finita e si passa alla successiva cercando un dormant con tutti dormant.
AlignGlobal::LOG(elfp,"\nCompleted Connected Component %i\n", cursid);
AlignGlobal::LOG(elfp,"\nDormant Num: %i\n", DormantNum());
curr = ChooseDormantWithMostDormantLink();
if (curr == nullptr) {
AlignGlobal::LOG(elfp,"\nFailed ChooseDormantWithMostDormantLink, chosen id:%i\n" ,0);
break; // non ci sono piu' componenti connesse composte da piu' di una singola mesh.
}
else {
AlignGlobal::LOG(elfp,"\nCompleted ChooseDormantWithMostDormantLink, chosen id:%i\n" ,curr->id);
}
curr->Active = true;
cursid = curr->sid;
curr = ChooseDormantWithMostActiveLink ();
if (curr == nullptr) {
AlignGlobal::LOG(elfp, "\nFailed ChooseDormantWithMostActiveLink, chosen id:%i\n", 0);
}
else {
AlignGlobal::LOG(elfp, "\nCompleted ChooseDormantWithMostActiveLink, chosen id:%i\n", curr->id);
}
}
AlignGlobal::LOG(elfp,"\nAdded node %i '%s' with %i/%i Active link\n",curr->id,Names.find(curr->id)->second.c_str(),curr->ActiveAdjNum(),curr->Adj.size());
curr->Active=true;
curr->Queued=true;
// Si suppone, ad occhio, che per risistemare un insieme di n mesh servano al piu' 10n passi;
localmaxiter = ActiveNum() * 10;
Q.push(curr);
step = 0;
// Ciclo interno di allineamento
while (!Q.empty()) {
curr = Q.front();
Q.pop();
curr->Queued=false;
change = curr->AlignWithActiveAdj(Rigid);
step++;
AlignGlobal::LOG(elfp, " Step %5i Queue size %5i Moved %4i err %10.4f\n", step, Q.size(), curr->id, change);
if (change > epsilon) {
curr->PushBackActiveAdj(Q);
AlignGlobal::LOG(elfp," Large Change pushing back active nodes adj to %i to Q (new size %i)\n",curr->id,Q.size());
if (change > epsilon * 1000) {
std::printf("Large Change Warning\n\n");
}
}
if (step > localmaxiter) return false;
if (step > maxiter) return false;
}
}
if (!curr) {
AlignGlobal::LOG(elfp,"Alignment failed for %i meshes:\n",DormantNum());
for (auto li = std::begin(N); li != std::end(N); ++li){
if (!(*li).Active) {
//(*li).M.SetIdentity();
(*li).Discarded=true;
AlignGlobal::LOG(elfp, "%5i\n", (*li).id);
}
}
}
AlignGlobal::LOG(elfp,"Completed Alignment in %i steps with error %f\n",step,epsilon);
return true;
}
AlignGlobal::Node* ChooseDormantWithMostDormantLink() {
int MaxAdjNum = 0;
AlignGlobal::Node *BestNode = nullptr;
for (auto li = std::begin(N); li != std::end(N); ++li) {
if (!(*li).Active) {
int AdjNum = (*li).DormantAdjNum();
if (AdjNum > MaxAdjNum) {
MaxAdjNum = AdjNum;
BestNode = &(*li);
}
}
}
if (!BestNode){
std::printf("Warning! Unable to find a Node with at least a dormant link!!\n");
return nullptr;
}
assert(BestNode);
assert(!BestNode->Queued);
assert(!BestNode->Active);
return BestNode;
}
AlignGlobal::Node* ChooseDormantWithMostActiveLink() {
int MaxAdjNum = 0;
AlignGlobal::Node* BestNode = nullptr;
for (auto li = std::begin(N); li != std::end(N); ++li) {
if (!(*li).Active) {
int AdjNum = (*li).ActiveAdjNum();
if (AdjNum > MaxAdjNum) {
MaxAdjNum = AdjNum;
BestNode = &(*li);
}
}
}
if (!BestNode){
// Abbiamo finito di sistemare questa componente connessa.
std::printf("Warning! Unable to find a Node with at least an active link!!\n");
return nullptr;
}
assert(BestNode);
assert(!BestNode->Queued);
assert(!BestNode->Active);
return BestNode;
}
void BuildGraph(std::vector<AlignPair::Result *> &Res, std::vector<Matrix44d> &Tr, std::vector<int> &Id) {
Clear();
// si suppone che la matrice Tr[i] sia relativa ad un nodo con id Id[i];
int mn = static_cast<int>(Tr.size());
// printf("building graph\n");
AlignGlobal::Node rgn;
rgn.Active = false;
rgn.Queued = false;
rgn.Discarded = false;
std::map<int, AlignGlobal::Node *> Id2N;
std::map<int, int> Id2I;
for (int i = 0; i < mn; ++i) {
rgn.id = Id[i];
rgn.M = Tr[i];
N.push_back(rgn);
Id2N[rgn.id] = &(N.back());
Id2I[rgn.id] = i;
}
std::printf("building %zu graph arcs\n",Res.size());
AlignGlobal::VirtAlign *tv;
// Ciclo principale in cui si costruiscono i vari archi
// Si assume che i result siano fatti nel sistema di riferimento della matrici fix.
for (auto rii = std::begin(Res); rii != std::end(Res); ++rii) {
AlignPair::Result *ri = *rii;
tv = new VirtAlign();
tv->Fix = Id2N[(*ri).FixName];
tv->Mov = Id2N[(*ri).MovName];
tv->Fix->Adj.push_back(tv);
tv->Mov->Adj.push_back(tv);
tv->FixP = (*ri).Pfix;
tv->MovP = (*ri).Pmov;
tv->FixN = (*ri).Nfix;
tv->MovN = (*ri).Nmov;
/*
Siano:
Pf e Pm i punti sulle mesh fix e mov nei sist di rif originali
Pft e Pmt i punti sulle mesh fix e mov dopo le trasf correnti;
Mf e Mm le trasf che portano le mesh fix e mov nelle posizioni correnti;
If e Im le trasf inverse di cui sopra
Vale:
Pft = Mf*Pf e Pmt = Mm*Pm
Pf = If*Pft e Pm = Im*Pmt
Res * Pm = Pf;
Res * Im * Pmt = If * Pft
Mf * Res * Im * Pmt = Mf * If * Pft
(Mf * Res * Im) * Pmt = Pft
*/
Matrix44d Mm = Tr[Id2I[(*ri).MovName]];
Matrix44d Mf = Tr[Id2I[(*ri).FixName]];
Matrix44d Im = Inverse(Mm);
Matrix44d If = Inverse(Mf);
Matrix44d NewTr = Mf * (*ri).Tr * Im; // --- orig
tv->M2F = NewTr;
tv->F2M = Inverse(NewTr);
assert(tv->Check());
A.push_back(tv);
}
ComputeConnectedComponents();
}
bool GetMatrixVector(std::vector<Matrix44d> &Tr, std::vector<int> &Id) {
std::map<int, AlignGlobal::Node *> Id2N;
Tr.clear();
for (auto li = std::begin(N); li != std::end(N); ++li) {
Id2N[(*li).id] = &*li;
}
Tr.resize(Id.size());
for (std::size_t i = 0; i < Id.size(); ++i) {
if (Id2N[Id[i]] == 0) return false;
Tr[i] = Id2N[Id[i]]->M;
}
return false;
}
};
}
#endif //MESHLAB_ALIGNGLOBAL_H

389
vcg/complex/algorithms/meshtree.h Normal file
View File

@ -0,0 +1,389 @@
#ifndef VCGLIB_MESHTREE_H
#define VCGLIB_MESHTREE_H
#include <vcg/complex/algorithms/align_pair.h>
#include <vcg/complex/algorithms/align_global.h>
#include <vcg/complex/algorithms/occupancy_grid.h>
#ifdef _OPENMP
#include <omp.h>
#endif
namespace vcg {
template<class MeshType, class ScalarType>
class MeshTree {
public:
class MeshNode {
public:
bool glued;
MeshType *m;
explicit MeshNode(MeshType *_m) : m{_m}, glued{false} {}
vcg::Matrix44<ScalarType> &tr() {
return m->cm.Tr;
}
const vcg::Box3<ScalarType> &bbox() const {
return m->cm.bbox;
}
int Id() {
return m->id();
}
};
class Param {
public:
int OGSize = 50000;
float arcThreshold = 0.3f;
float recalcThreshold = 0.1f;
};
std::map<int, MeshNode*> nodeMap;
std::vector<vcg::AlignPair::Result> resultList;
vcg::OccupancyGrid<CMeshO, ScalarType> OG{};
vcg::CallBackPos* cb = vcg::DummyCallBackPos;
MeshTree() = default;
~MeshTree() { clear(); }
MeshType *MM(unsigned int i) {
return nodeMap[i]->m;
}
void clear() {
for (auto& ni : nodeMap) {
delete ni.second;
}
nodeMap.clear();
resultList.clear();
}
void deleteResult(MeshTree::MeshNode *mp) {
auto li = std::begin(resultList);
while (li != resultList.end()) {
if (li->MovName == mp->Id() || li->FixName == mp->Id()) {
li = resultList.erase(li);
}
else {
++li;
}
}
}
vcg::AlignPair::Result* findResult(int id1, int id2) {
for (auto& li : resultList) {
if ((li.MovName == id1 && li.FixName == id2) || (li.MovName == id2 && li.FixName == id1) ) {
return &li;
}
}
return nullptr;
}
MeshTree::MeshNode *find(int id) {
MeshTree::MeshNode *mp = nodeMap[id];
if (mp == nullptr || mp->Id() != id) {
assert("You are trying to find a non existent mesh" == nullptr);
}
return mp;
}
MeshTree::MeshNode *find(MeshType *m) {
for (auto& ni : nodeMap) {
if (ni.second->m == m) return ni.second;
}
assert("You are trying to find a non existent mesh" == nullptr);
return nullptr;
}
int gluedNum() {
int count = 0;
for (auto& ni : nodeMap) {
if (ni.second->glued) ++count;
}
return count;
}
void Process(vcg::AlignPair::Param &ap, MeshTree::Param &mtp) {
char buf[1024];
std::sprintf(buf, "Starting Processing of %i glued meshes out of %zu meshes\n", gluedNum(), nodeMap.size());
cb(0, buf);
/******* Occupancy Grid Computation *************/
std::memset(buf, '\0', 1024);
std::sprintf(buf, "Computing Overlaps %i glued meshes...\n", gluedNum());
cb(0, buf);
OG.Init(static_cast<int>(nodeMap.size()), vcg::Box3<ScalarType>::Construct(gluedBBox()), mtp.OGSize);
for (auto& ni : nodeMap) {
MeshTree::MeshNode *mn = ni.second;
if (mn->glued) {
OG.AddMesh(mn->m->cm, vcg::Matrix44<ScalarType>::Construct(mn->tr()), mn->Id());
}
}
OG.Compute();
OG.Dump(stdout);
// Note: the s and t of the OG translate into fix and mov, respectively.
/*************** The long loop of arc computing **************/
// count existing arcs within current error threshold
float percentileThr = 0;
if (!resultList.empty()) {
vcg::Distribution<float> H;
for (auto& li : resultList) {
H.Add(li.err);
}
percentileThr = H.Percentile(1.0f - mtp.recalcThreshold);
}
std::size_t totalArcNum = 0;
int preservedArcNum = 0, recalcArcNum = 0;
while(totalArcNum < OG.SVA.size() && OG.SVA[totalArcNum].norm_area > mtp.arcThreshold)
{
AlignPair::Result *curResult = findResult(OG.SVA[totalArcNum].s, OG.SVA[totalArcNum].t);
if (curResult) {
if (curResult->err < percentileThr) {
++preservedArcNum;
}
else {
++recalcArcNum;
}
}
else {
resultList.push_back(AlignPair::Result());
resultList.back().FixName = OG.SVA[totalArcNum].s;
resultList.back().MovName = OG.SVA[totalArcNum].t;
resultList.back().err = std::numeric_limits<double>::max();
}
++totalArcNum;
}
//if there are no arcs at all complain and return
if (totalArcNum == 0) {
std::memset(buf, '\0', 1024);
std::sprintf(buf, "\n Failure. There are no overlapping meshes?\n No candidate alignment arcs. Nothing Done.\n");
cb(0, buf);
return;
}
int num_max_thread = 1;
#ifdef _OPENMP
if (totalArcNum > 32) num_max_thread = omp_get_max_threads();
#endif
std::memset(buf, '\0', 1024);
std::sprintf(buf, "Arc with good overlap %6zu (on %6zu)\n", totalArcNum, OG.SVA.size());
cb(0, buf);
std::memset(buf, '\0', 1024);
std::sprintf(buf, " %6i preserved %i Recalc \n", preservedArcNum, recalcArcNum);
cb(0, buf);
bool hasValidAlign = false;
#pragma omp parallel for schedule(dynamic, 1) num_threads(num_max_thread)
// on windows, omp does not support unsigned types for indices on cycles
for (int i = 0 ;i < static_cast<int>(totalArcNum); ++i) {
std::fprintf(stdout,"%4i -> %4i Area:%5i NormArea:%5.3f\n",OG.SVA[i].s,OG.SVA[i].t,OG.SVA[i].area,OG.SVA[i].norm_area);
AlignPair::Result *curResult = findResult(OG.SVA[i].s,OG.SVA[i].t);
// // missing arc and arc with great error must be recomputed.
if (curResult->err >= percentileThr) {
ProcessArc(OG.SVA[i].s, OG.SVA[i].t, *curResult, ap);
curResult->area = OG.SVA[i].norm_area;
if (curResult->isValid()) {
hasValidAlign = true;
std::pair<double, double> dd = curResult->computeAvgErr();
#pragma omp critical
std::memset(buf, '\0', 1024);
std::sprintf(buf, "(%3i/%3zu) %2i -> %2i Aligned AvgErr dd=%f -> dd=%f \n", i+1,totalArcNum,OG.SVA[i].s,OG.SVA[i].t,dd.first,dd.second);
cb(0, buf);
}
else {
#pragma omp critical
std::memset(buf, '\0', 1024);
std::sprintf(buf, "(%3i/%3zu) %2i -> %2i Failed Alignment of one arc %s\n",i+1,totalArcNum,OG.SVA[i].s,OG.SVA[i].t,vcg::AlignPair::errorMsg(curResult->status));
cb(0, buf);
}
}
}
//if there are no valid arcs complain and return
if (!hasValidAlign) {
std::memset(buf, '\0', 1024);
std::sprintf(buf, "\n Failure. No successful arc among candidate Alignment arcs. Nothing Done.\n");
cb(0, buf);
return;
}
vcg::Distribution<float> H; // stat for printing
for (auto& li : resultList) {
if (li.isValid()) H.Add(li.err);
}
std::memset(buf, '\0', 1024);
std::sprintf(buf, "Completed Mesh-Mesh Alignment: Avg Err %5.3f; Median %5.3f; 90%% %5.3f\n", H.Avg(), H.Percentile(0.5f), H.Percentile(0.9f));
cb(0, buf);
ProcessGlobal(ap);
}
void ProcessGlobal(vcg::AlignPair::Param &ap) {
char buff[1024];
std::memset(buff, '\0', 1024);
/************** Preparing Matrices for global alignment *************/
std::vector<int> GluedIdVec;
std::vector<vcg::Matrix44d> GluedTrVec;
std::map<int, std::string> names;
for (auto& ni : nodeMap) {
MeshTree::MeshNode *mn = ni.second;
if (mn->glued) {
GluedIdVec.push_back(mn->Id());
GluedTrVec.push_back(vcg::Matrix44d::Construct(mn->tr()));
names[mn->Id()] = qUtf8Printable(mn->m->label());
}
}
vcg::AlignGlobal AG;
std::vector<vcg::AlignPair::Result *> ResVecPtr;
for (auto& li : resultList) {
if (li.isValid()) {
ResVecPtr.push_back(&li);
}
}
AG.BuildGraph(ResVecPtr, GluedTrVec, GluedIdVec);
float StartGlobErr = 0.001f;
while (!AG.GlobalAlign(names, StartGlobErr, 100, ap.MatchMode == vcg::AlignPair::Param::MMRigid, stdout, cb)) {
StartGlobErr *= 2;
AG.BuildGraph(ResVecPtr,GluedTrVec, GluedIdVec);
}
std::vector<vcg::Matrix44d> GluedTrVecOut(GluedTrVec.size());
AG.GetMatrixVector(GluedTrVecOut,GluedIdVec);
// Now get back the results!
for (std::size_t ii = 0; ii < GluedTrVecOut.size(); ++ii) {
MM(GluedIdVec[ii])->cm.Tr.Import(GluedTrVecOut[ii]);
}
std::sprintf(buff, "Completed Global Alignment (error bound %6.4f)\n", StartGlobErr);
cb(0, buff);
}
void ProcessArc(int fixId, int movId, vcg::AlignPair::Result &result, vcg::AlignPair::Param ap) {
// l'allineatore globale cambia le varie matrici di posizione di base delle mesh
// per questo motivo si aspetta i punti nel sistema di riferimento locale della mesh fix
// Si fanno tutti i conti rispetto al sistema di riferimento locale della mesh fix
vcg::Matrix44d FixM = vcg::Matrix44d::Construct(find(fixId)->tr());
vcg::Matrix44d MovM = vcg::Matrix44d::Construct(find(movId)->tr());
vcg::Matrix44d MovToFix = Inverse(FixM) * MovM;
ProcessArc(fixId, movId, MovToFix, result, ap);
}
void ProcessArc(int fixId, int movId, vcg::Matrix44d &MovM, 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.
MM(fixId)->updateDataMask(MeshType::MeshModel::MM_FACEMARK);
aa.convertMesh<CMeshO>(MM(fixId)->cm,Fix);
vcg::AlignPair::A2Grid UG;
vcg::AlignPair::A2GridVert VG;
if (MM(fixId)->cm.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.
MM(movId)->updateDataMask(MeshType::MeshModel::MM_FACEMARK);
std::vector<vcg::AlignPair::A2Vertex> tmpmv;
aa.convertVertex(MM(movId)->cm.vert,tmpmv);
aa.sampleMovVert(tmpmv, ap.SampleNum, ap.SampleMode);
aa.mov=&tmpmv;
aa.fix=&Fix;
aa.ap = ap;
// Perform the ICP algorithm
aa.align(MovM,UG,VG,result);
result.FixName=fixId;
result.MovName=movId;
}
inline vcg::Box3<ScalarType> bbox() {
vcg::Box3<ScalarType> FullBBox;
for (auto& ni : nodeMap) {
FullBBox.Add(vcg::Matrix44d::Construct(ni.second->tr()), ni.second->bbox());
}
return FullBBox;
}
inline vcg::Box3<ScalarType> gluedBBox() {
vcg::Box3<ScalarType> FullBBox;
for (auto& ni : nodeMap) {
if (ni.second->glued) {
FullBBox.Add(vcg::Matrix44<ScalarType>::Construct(ni.second->tr()), ni.second->bbox());
}
}
return FullBBox;
}
};
}
#endif //VCGLIB_MESHTREE_H

429
vcg/complex/algorithms/occupancy_grid.h Normal file
View File

@ -0,0 +1,429 @@
#include <bitset>
// #include <wrap/ply/plystuff.h>
#include <wrap/io_trimesh/import.h>
#include <vcg/complex/algorithms/align_pair.h>
#include <vcg/space/index/grid_static_obj.h>
#ifndef VCGLIB_OCCUPANCY_GRID_H
#define VCGLIB_OCCUPANCY_GRID_H
#define OG_MAX_MCB_SIZE 2048
#define OG_MESH_INFO_MAX_STAT 64
namespace vcg {
template<class MeshType, class ScalarType>
class OccupancyGrid {
public:
/**
* Class to keep for each voxel the id of the mesh passing through it.
* based on bitset
*/
class MeshCounter {
private:
std::bitset<OG_MAX_MCB_SIZE> cnt;
public:
static constexpr int MaxVal() {
return OG_MAX_MCB_SIZE;
}
bool Empty() const {
return cnt.none();
}
void Clear() {
cnt.reset();
}
bool IsSet(size_t i) const {
return cnt.test(i);
}
void Set(size_t i) {
cnt.set(i);
}
void UnSet(size_t i) {
cnt.reset(i);
}
size_t Count() const {
return cnt.count();
}
/**
* Return a vector with all the id of the meshes
*/
void Pack(std::vector<int> &v) const {
v.clear();
for (size_t i = 0; i < MeshCounter::MaxVal(); ++i) {
if (cnt.test(i)) {
v.push_back(i);
}
}
}
bool operator < (const MeshCounter &c) const {
if (cnt == c.cnt) return false;
std::size_t ii = 0;
while (ii < MeshCounter::MaxVal()){
if (cnt[ii] != c.cnt[ii]) {
return cnt[ii] < c.cnt[ii];
}
++ii;
}
return false;
}
};
/**
* Class for collecting cumulative information about each mesh in the OG.
* This info are collected in the Compute() by scanning the OG after we filled it with all the meshes.
*/
class OGMeshInfo {
public:
int id {-1}; // the id of the mesh
int area {0}; // number of voxels in the OG touched by this mesh
int coverage {0}; // quanto e' ricoperta da altre mesh eccetto se stessa (eg: se ho due mesh di 1000 con overlap al 30% la covrg e' 300)
bool used = false;
std::vector<int> densityDistribution; // Distribution of the of the density of the voxels touched by this mesh:
// densityDistribution[i] says how many voxel (among the ones coverd by this mesh)
// are covered by <i> othermeshes. Sum(densityDistribution) == area;
// if densityDistribution[1] > 0 means that this mesh is the unique to cover some portion of the space.
void Init(int _id) {
id=_id;
}
bool operator < (OGMeshInfo &o) const {
return area < o.area;
}
static constexpr int MaxStat() {
return OG_MESH_INFO_MAX_STAT;
}
};
/**
* Classe con informazioni su un arco plausibile
*/
class OGArcInfo {
public:
enum sort {
AREA,
NORM_AREA,
DEGREE
};
int s, t; // source and target (come indici nel gruppo corrente)
int area; //
float norm_area;
OGArcInfo(int _s, int _t, int _area, float _n) : s{_s}, t{_t}, area{_area}, norm_area{_n} {}
OGArcInfo(int _s, int _t, int _area) : s{_s}, t{_t}, area{_area} {}
bool operator < (const OGArcInfo &p) const {
return norm_area < p.norm_area;
}
};
GridStaticObj<MeshCounter, float> G;
int mn;
int TotalArea;
/**
* Maximum number of meshes that cross a cell
*/
int MaxCount;
/**
* SortedVisual Arcs
*/
std::vector<OGArcInfo> SVA; // SortedVirtual Arcs;
/**
* High level information for each mesh. Mapped by mesh id
*/
std::map<int, OGMeshInfo> VM;
bool Init(int _mn, Box3<ScalarType> bb, int size) {
// the number of meshes (including all the unused ones; eg it is the range of the possible id)
mn = _mn;
if (mn > MeshCounter::MaxVal()) return false;
MeshCounter MC;
MC.Clear();
G.Create(bb,size,MC);
VM.clear();
return true;
}
void Add(const char *MeshName, Matrix44<ScalarType> &Tr, int id) {
AlignPair::A2Mesh M;
vcg::tri::io::Importer<AlignPair::A2Mesh>::Open(M, MeshName);
vcg::tri::Clean<AlignPair::A2Mesh>::RemoveUnreferencedVertex(M);
AddMesh(M,Tr,id);
}
void AddMeshes(std::vector<std::string> &names, std::vector<Matrix44<ScalarType>> &trv,int size) {
Box3<ScalarType> bb, totalbb;
bb.SetNull();
totalbb.SetNull();
std::fprintf(stdout, "OG::AddMesh:Scanning BBoxes\n");
for (std::size_t i = 0; i < names.size(); ++i) {
// vcg::ply::ScanBBox(names[i].c_str(), bb, true);
totalbb.Add(trv[i], bb);
}
Init(names.size(),totalbb,size);
for (std::size_t i = 0; i < names.size(); ++i) {
std::fprintf(stdout, "OG::AddMesh:Adding Mesh %i '%s'\n", i, names[i].c_str());
Add(names[i].c_str(), trv[i], i);
}
}
void AddMesh(MeshType &mesh, const Matrix44<ScalarType> &Tr, int ind) {
Matrix44f Trf;
Trf.Import(Tr);
for (auto vi = std::begin(mesh.vert); vi != std::end(mesh.vert); ++vi) {
if (!(*vi).IsD()) {
G.Grid(Trf * Point3f::Construct((*vi).P())).Set(ind);
}
}
VM[ind].Init(ind);
VM[ind].used = true;
}
void RemoveMesh(int id) {
MeshCounter *GridEnd = G.grid + G.size();
for (MeshCounter* ig = G.grid; ig != GridEnd; ++ig) {
ig->UnSet(id);
}
}
/**
* This function is called after we have <added> all the mesh to the OG
* to collect the information about the interferences between the various meshes.
*/
void Compute() {
// Analisi della griglia
// Si deve trovare l'insieme degli archi piu'plausibili
// un arco ha "senso" in una cella se entrambe le mesh compaiono in quell'arco
// Si considera tutti gli archi possibili e si conta in quante celle ha senso un arco
std::vector<int> VA; // virtual arcs
VA.resize(mn * mn, 0);
std::map<std::pair<int, int>, int> VAMap;
// First Loop:
// Scan the grid and update possible arc count
for (int i = 0; i < G.siz[0]; ++i) {
for (int j = 0; j < G.siz[1]; ++j) {
for (int k = 0; k < G.siz[2]; ++k) {
std::vector<int> vv;
G.Grid(i, j, k).Pack(vv);
std::size_t meshInCell = vv.size();
for (std::size_t ii = 0; ii < vv.size(); ++ii) {
OccupancyGrid::OGMeshInfo &omi_ii = VM[vv[ii]];
++omi_ii.area; // compute mesh area
if (meshInCell > omi_ii.densityDistribution.size()) {
omi_ii.densityDistribution.resize(meshInCell);
}
++(omi_ii.densityDistribution[meshInCell - 1]);
}
for (std::size_t ii = 0; ii < vv.size(); ++ii) {
for (std::size_t jj = ii + 1; jj < vv.size(); ++jj) {
// count intersections of all mesh pairs
++VAMap[std::make_pair(vv[ii], vv[jj])];
}
}
}
}
}
// Find all the arcs, e.g. all the pair of meshes
SVA.clear();
for (auto vi = std::begin(VAMap); vi != std::end(VAMap); ++vi) {
if (vi->second > 0) {
int m_s = vi->first.first;
int m_t = vi->first.second;
int area = vi->second;
SVA.push_back( OGArcInfo (m_s,m_t,area,float(area)/float(std::min(VM[m_s].area,VM[m_t].area)) ));
}
}
// Compute Mesh Coverage
for (std::size_t i = 0; i < SVA.size(); ++i) {
VM[SVA[i].s].coverage += SVA[i].area;
VM[SVA[i].t].coverage += SVA[i].area;
}
std::sort(std::begin(SVA), std::end(SVA));
std::reverse(std::begin(SVA), std::end(SVA));
}
void ComputeUsefulMesh(FILE *elfp) {
int mcnt = 0;
std::vector<int> UpdArea(mn);
std::vector<int> UpdCovg(mn);
for (std::size_t m = 0; m < mn; ++m) {
if (VM[m].used && VM[m].area > 0) {
mcnt++;
UpdCovg[m]=VM[m].coverage;
UpdArea[m]=VM[m].area;
}
}
int sz = G.size();
if (elfp) {
std::fprintf(elfp, "\n\nComputing Usefulness of Meshes of %i(on %i) meshes\n Og with %i / %i fill ratio %i max mesh per cell\n\n", mcnt, mn, TotalArea, sz, MaxCount);
std::fprintf(elfp, "\n");
}
int CumArea = 0;
for (std::size_t m = 0; m < mn-1; ++m) {
int best = max_element(std::begin(UpdArea), std::end(UpdArea)) - std::begin(UpdArea);
CumArea += UpdArea[best];
if (UpdCovg[best] < 0) break;
// se era una mesh fuori del working group si salta tutto.
if (VM[best].area == 0) continue;
if (elfp) {
fprintf(elfp, "%3i %3i %7i (%7i) %7i %5.2f %7i(%7i)\n",
m, best, UpdArea[best], VM[best].area, TotalArea - CumArea,
100.0 - 100 * float(CumArea) / TotalArea, UpdCovg[best], VM[best].coverage);
}
UpdArea[best] = -1;
UpdCovg[best] = -1;
for (std::size_t i = 0; i < sz; ++i) {
MeshCounter &mc = G.grid[i];
if (mc.IsSet(best)) {
mc.UnSet(best);
for (std::size_t j = 0; j < mn; ++j) {
if (mc.IsSet(j)) {
--UpdArea[j];
UpdCovg[j]-=mc.Count();
}
}
mc.Clear();
}
}
}
}
void Dump(FILE *fp) {
std::fprintf(fp, "Occupancy Grid\n");
std::fprintf(fp, "grid of ~%i kcells: %d x %d x %d\n", G.size(), G.siz[0], G.siz[1], G.siz[2]);
std::fprintf(fp, "grid voxel size of %f %f %f\n", G.voxel[0], G.voxel[1], G.voxel[2]);
std::fprintf(fp,"Computed %lu arcs for %i meshes\n", SVA.size(), mn);
for (std::size_t i=0;i<VM.size();++i) {
if (VM[i].used) {
std::fprintf(fp, "mesh %3lu area %6i covg %7i (%5.2f%%) DensDistr:", i, VM[i].area, VM[i].coverage, float(VM[i].coverage)/float(VM[i].area));
for (std::size_t j = 0; j < std::min(static_cast<std::size_t>(8), VM[i].densityDistribution.size()); ++j) {
std::fprintf(fp," %3i ", VM[i].densityDistribution[j]);
}
std::fprintf(fp,"\n");
}
else {
std::fprintf(fp, "mesh %3lu ---- UNUSED\n", i);
}
}
std::fprintf(fp, "Computed %lu Arcs :\n", SVA.size());
for (std::size_t i = 0; i < SVA.size() && SVA[i].norm_area > .1; ++i) {
std::fprintf(fp, "%4i -> %4i Area:%5i NormArea:%5.3f\n", SVA[i].s, SVA[i].t, SVA[i].area, SVA[i].norm_area);
}
std::fprintf(fp, "End OG Dump\n");
}
void ComputeTotalArea() {
using uint = unsigned int;
int ccnt = 0;
MaxCount = 0;
int sz = G.size();
for (int i = 0; i < sz; ++i)
if (!G.grid[i].Empty()) {
ccnt++;
if (G.grid[i].Count() > static_cast<uint>(MaxCount)) {
MaxCount = G.grid[i].Count();
}
}
TotalArea = ccnt;
}
};
}
#endif //VCGLIB_OCCUPANCY_GRID_H