hesuicong
/
openMVS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
29 Commits
1 Branch
0 Tags
13 MiB
 
 
 
 
 
 
Tree: 6f093be3f8
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '6f093be3f8'
${ noResults }
openMVS/vcglib/vcg/complex/algorithms/align_global.h

700 lines
25 KiB
Raw Blame History

#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