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

438 lines
12 KiB
C++
Raw Blame History

#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)
{
std::array<char, 1024> buf;
std::snprintf(
buf.data(),
1024,
"Starting Processing of %i glued meshes out of %zu meshes\n",
gluedNum(),
nodeMap.size());
cb(0, buf.data());
/******* Occupancy Grid Computation *************/
buf.fill('\0');
std::snprintf(buf.data(), 1024, "Computing Overlaps %i glued meshes...\n", gluedNum());
cb(0, buf.data());
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) {
buf.fill('\0');
std::snprintf(
buf.data(),
1024,
"\n Failure. There are no overlapping meshes?\n No candidate alignment arcs. "
"Nothing Done.\n");
cb(0, buf.data());
return;
}
int num_max_thread = 1;
#ifdef _OPENMP
if (totalArcNum > 32)
num_max_thread = omp_get_max_threads();
#endif
buf.fill('\0');
std::snprintf(
buf.data(), 1024,"Arc with good overlap %6zu (on %6zu)\n", totalArcNum, OG.SVA.size());
cb(0, buf.data());
buf.fill('\0');
std::snprintf(buf.data(), 1024," %6i preserved %i Recalc \n", preservedArcNum, recalcArcNum);
cb(0, buf.data());
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
buf.fill('\0');
std::snprintf(
buf.data(),
1024,
"(%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.data());
}
else {
#pragma omp critical
buf.fill('\0');
std::snprintf(
buf.data(),
1024,
"(%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.data());
}
}
}
// if there are no valid arcs complain and return
if (!hasValidAlign) {
buf.fill('\0');
std::snprintf(
buf.data(),
1024,
"\n Failure. No successful arc among candidate Alignment arcs. Nothing "
"Done.\n");
cb(0, buf.data());
return;
}
vcg::Distribution<float> H; // stat for printing
for (auto& li : resultList) {
if (li.isValid())
H.Add(li.err);
}
buf.fill('\0');
std::snprintf(
buf.data(),
1024,
"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.data());
ProcessGlobal(ap);
}
void ProcessGlobal(vcg::AlignPair::Param& ap)
{
/************** 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::string str =
"Completed Global Alignment (error bound " + std::to_string(StartGlobErr) + ")\n";
cb(0, str.c_str());
}
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
Reference in New Issue View Git Blame Copy Permalink