diff --git a/vcg/complex/algorithms/occupancy_grid.h b/vcg/complex/algorithms/occupancy_grid.h index bcd6148c..5272f946 100644 --- a/vcg/complex/algorithms/occupancy_grid.h +++ b/vcg/complex/algorithms/occupancy_grid.h @@ -10,426 +10,423 @@ #define OG_MESH_INFO_MAX_STAT 64 namespace vcg { - namespace tri { + template + class OccupancyGrid { - template - class OccupancyGrid { + private: + + GridStaticObj G; + + int mn; + int TotalArea; + /** + * Maximum number of meshes that cross a cell + */ + int MaxCount; + + /** + * SortedVisual Arcs + */ + std::vector SVA; // SortedVirtual Arcs; + /** + * High level information for each mesh. Mapped by mesh id + */ + std::map VM; + + public: + + /** + * Class to keep for each voxel the id of the mesh passing through it. + * based on bitset + */ + class MeshCounter { private: - - GridStaticObj G; - - int mn; - int TotalArea; - /** - * Maximum number of meshes that cross a cell - */ - int MaxCount; - - /** - * SortedVisual Arcs - */ - std::vector SVA; // SortedVirtual Arcs; - /** - * High level information for each mesh. Mapped by mesh id - */ - std::map VM; + std::bitset cnt; public: - /** - * Class to keep for each voxel the id of the mesh passing through it. - * based on bitset - */ - class MeshCounter { - - private: - std::bitset 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 &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; - - 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; // the id of the mesh - int area; // number of voxels in the OG touched by this mesh - int coverage; // 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 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 othermeshes. Sum(densityDistribution) == area; - // if densityDistribution[1] > 0 means that this mesh is the unique to cover some portion of the space. - - OGMeshInfo() { - Init(-1); - } - - void Init(int _id) { - coverage=0;area=0; 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; - } - }; - - bool Init(int _mn, Box3d 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; + static constexpr int MaxVal() { + return OG_MAX_MCB_SIZE; } - void Add(const char *MeshName, Matrix44d &Tr, int id) { - - AlignPair::A2Mesh M; - - vcg::tri::io::Importer::Open(M,MeshName); - vcg::tri::Clean::RemoveUnreferencedVertex(M); - - AddMesh(M,Tr,id); + bool Empty() const { + return cnt.none(); } - void AddMeshes(std::vector &names, std::vector &trv,int size) { - - Box3d bb, totalbb; - - bb.SetNull(); - totalbb.SetNull(); - - std::fprintf(stdout, "OG::AddMesh:Scanning BBoxex\n"); - - for (std::size_t i = 0; i < names.size(); ++i) { - ply::ScanBBox(names[i].c_str(),bb); - 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 Clear() { + cnt.reset(); } - void AddMesh(MeshType &mesh, const Matrix44d &Tr, int ind) { - - Matrix44f Trf; - Trf.Import(Tr); - - for (auto vi = std::begin(M.vert); vi != std::end(M.vert); ++vi) { - - if (!(*vi).IsD()) { - G.Grid( Trf * Point3f::Construct((*vi).P()) ).Set(ind); - } - } - - VM[ind].Init(ind); - VM[ind].used = true; + bool IsSet(size_t i) const { + return cnt.test(i); } - void RemoveMesh(int id) { + void Set(size_t i) { + cnt.set(i); + } - MeshCounter *GridEnd = G.grid+G.size(); + void UnSet(size_t i) { + cnt.reset(i); + } - for (MeshCounter* ig = G.grid; ig != GridEnd; ++ig) { - ig->UnSet(id); - } + size_t Count() const { + return cnt.count(); } /** - * This function is called after we have all the mesh to the OG - * to collect the information about the interferences between the various meshes. + * Return a vector with all the id of the meshes */ - void Compute() { + void Pack(std::vector &v) const { - // 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 + v.clear(); - std::vector VA; // virtual arcs - VA.resize(mn * mn, 0); - - std::map, 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 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(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 UpdArea(mn); - std::vector 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(); - } + for (size_t i = 0; i < MeshCounter::MaxVal(); ++i) { + if (cnt.test(i) { + v.push_back(i); } } } - void Dump(FILE *fp) { + bool operator < (const MeshCounter &c) const { - 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]); + if (cnt==c.cnt) return false; - std::fprintf(fp,"Computed %lu arcs for %i meshes\n", SVA.size(), mn); + size_t ii = 0; - for (std::size_t i=0;i(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); + while (ii < MeshCounter::MaxVal()){ + if (cnt[ii]!=c.cnt[ii]) { + return cnt[ii] < c.cnt[ii]; } + ++ii; } - - std::fprintf(fp, "Computed %lu Arcs :\n", SVA.size()); - - for (std::size_t i = 0; i .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"); + return false; } - - 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(MaxCount)) { - MaxCount = G.grid[i].Count(); - } - } - - TotalArea = ccnt; - } - }; - } + + /** + * 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; // the id of the mesh + int area; // number of voxels in the OG touched by this mesh + int coverage; // 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 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 othermeshes. Sum(densityDistribution) == area; + // if densityDistribution[1] > 0 means that this mesh is the unique to cover some portion of the space. + + OGMeshInfo() { + Init(-1); + } + + void Init(int _id) { + coverage=0;area=0; 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; + } + }; + + bool Init(int _mn, Box3d 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, Matrix44d &Tr, int id) { + + AlignPair::A2Mesh M; + + vcg::tri::io::Importer::Open(M,MeshName); + vcg::tri::Clean::RemoveUnreferencedVertex(M); + + AddMesh(M,Tr,id); + } + + void AddMeshes(std::vector &names, std::vector &trv,int size) { + + Box3d bb, totalbb; + + bb.SetNull(); + totalbb.SetNull(); + + std::fprintf(stdout, "OG::AddMesh:Scanning BBoxex\n"); + + for (std::size_t i = 0; i < names.size(); ++i) { + ply::ScanBBox(names[i].c_str(),bb); + 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 Matrix44d &Tr, int ind) { + + Matrix44f Trf; + Trf.Import(Tr); + + for (auto vi = std::begin(M.vert); vi != std::end(M.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 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 VA; // virtual arcs + VA.resize(mn * mn, 0); + + std::map, 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 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(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 UpdArea(mn); + std::vector 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(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 .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(MaxCount)) { + MaxCount = G.grid[i].Count(); + } + } + + TotalArea = ccnt; + } + + }; } #endif //VCGLIB_OCCUPANCY_GRID_H