From 621f19741968d99b319bb1cdc568d755d9d8e865 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Wed, 25 Jun 2025 16:48:10 +0200 Subject: [PATCH 1/8] temporarily disabling copy-less insert for eigen --- include/chad/tsdf.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/chad/tsdf.hpp b/include/chad/tsdf.hpp index 2c77e73..45f96b0 100644 --- a/include/chad/tsdf.hpp +++ b/include/chad/tsdf.hpp @@ -102,7 +102,7 @@ namespace chad { // insert pointcloud alongside scanner position void inline insert(const std::vector& points, const Eigen::Vector3f& position){ // when using unpadded Vector3f, we can avoid copies - if (sizeof(Eigen::Vector3f) == 12) { + if (sizeof(Eigen::Vector3f) == 12 && false /*disable this temporarily*/) { const float* points_p = points[0].data(); const float* position_p = position.data(); insert(points_p, points.size(), position_p); From c6d7472fb4ce0e2552bc22d21f5079dacce7691d Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Thu, 26 Jun 2025 10:50:49 +0200 Subject: [PATCH 2/8] fixed raw float interface --- CMakeLists.txt | 1 - cmake/eigen.cmake | 14 -------------- include/chad/tsdf.hpp | 12 ++++-------- 3 files changed, 4 insertions(+), 23 deletions(-) delete mode 100644 cmake/eigen.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 45c6220..1d49338 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,6 @@ include("cmake/fmt.cmake") include("cmake/gtl.cmake") include("cmake/glm.cmake") include("cmake/lvr2.cmake") -include("cmake/eigen.cmake") include("cmake/morton.cmake") # compile executable if top level project diff --git a/cmake/eigen.cmake b/cmake/eigen.cmake deleted file mode 100644 index 93020e1..0000000 --- a/cmake/eigen.cmake +++ /dev/null @@ -1,14 +0,0 @@ -# use either system or FetchContent package -find_package(Eigen3 QUIET) -if (NOT Eigen3_FOUND) - - include(FetchContent) - FetchContent_Declare(eigen - GIT_REPOSITORY "https://gitlab.com/libeigen/eigen.git" - GIT_TAG "3.4.0" - GIT_SHALLOW ON - OVERRIDE_FIND_PACKAGE - EXCLUDE_FROM_ALL) - FetchContent_MakeAvailable(eigen) -endif() -target_link_libraries(${PROJECT_NAME} PRIVATE Eigen3::Eigen) \ No newline at end of file diff --git a/include/chad/tsdf.hpp b/include/chad/tsdf.hpp index 45f96b0..1a6d49b 100644 --- a/include/chad/tsdf.hpp +++ b/include/chad/tsdf.hpp @@ -49,9 +49,8 @@ namespace chad { // insert pointcloud as a raw array of repeating x,y,z coordinates void inline insert(const float* points_p, size_t points_count, const float* position_p) { const auto* vec_p = reinterpret_cast*>(points_p); - size_t vec_count = points_count / 3; // use points_p as the buffer for new vector (as vec_p), not requiring any copies - const auto points = std::vector>(vec_p, vec_p + vec_count); + const auto points = std::vector>(vec_p, vec_p + points_count); // position_p should just be x y and z const auto position = *reinterpret_cast*>(position_p); insert(points, position); @@ -59,9 +58,8 @@ namespace chad { // insert pointcloud as a raw array of repeating x,y,z coordinates void inline insert(const float* points_p, size_t points_count, float position_x, float position_y, float position_z) { const auto* vec_p = reinterpret_cast*>(points_p); - size_t vec_count = points_count / 3; // use points_p as the buffer for new vector (as vec_p), not requiring any copies - const auto points = std::vector>(vec_p, vec_p + vec_count); + const auto points = std::vector>(vec_p, vec_p + points_count); insert(points, { position_x, position_y, position_z }); } @@ -78,8 +76,7 @@ namespace chad { // when using unpadded vec3, we can avoid copies if (sizeof(glm::vec3) == 12) { const float* points_p = &points[0].x; - const float* position_p = &position.x; - insert(points_p, points.size(), position_p); + insert(points_p, points.size(), position.x, position.y, position.z); } else { std::vector> points_vec; @@ -104,8 +101,7 @@ namespace chad { // when using unpadded Vector3f, we can avoid copies if (sizeof(Eigen::Vector3f) == 12 && false /*disable this temporarily*/) { const float* points_p = points[0].data(); - const float* position_p = position.data(); - insert(points_p, points.size(), position_p); + insert(points_p, points.size(), position.x(), position.y(), position.z()); } else { std::vector> points_vec; From 433be069a83dfb0d1850def0ace5e6c031575100 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Thu, 26 Jun 2025 11:00:35 +0200 Subject: [PATCH 3/8] sync changes --- CMakeLists.txt | 1 + cmake/eigen.cmake | 14 ++++++++++++++ src/chad/detail/lvr2.cpp | 1 - src/chad/main.cpp | 7 +------ src/chad/tsdf.cpp | 5 ----- 5 files changed, 16 insertions(+), 12 deletions(-) create mode 100644 cmake/eigen.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d49338..c06ca6a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ target_link_libraries(${PROJECT_NAME} PRIVATE ${CMAKE_DL_LIBS}) include("cmake/fmt.cmake") include("cmake/gtl.cmake") include("cmake/glm.cmake") +include("cmake/eigen.cmake") include("cmake/lvr2.cmake") include("cmake/morton.cmake") diff --git a/cmake/eigen.cmake b/cmake/eigen.cmake new file mode 100644 index 0000000..93020e1 --- /dev/null +++ b/cmake/eigen.cmake @@ -0,0 +1,14 @@ +# use either system or FetchContent package +find_package(Eigen3 QUIET) +if (NOT Eigen3_FOUND) + + include(FetchContent) + FetchContent_Declare(eigen + GIT_REPOSITORY "https://gitlab.com/libeigen/eigen.git" + GIT_TAG "3.4.0" + GIT_SHALLOW ON + OVERRIDE_FIND_PACKAGE + EXCLUDE_FROM_ALL) + FetchContent_MakeAvailable(eigen) +endif() +target_link_libraries(${PROJECT_NAME} PRIVATE Eigen3::Eigen) \ No newline at end of file diff --git a/src/chad/detail/lvr2.cpp b/src/chad/detail/lvr2.cpp index 015ebd0..8bec26b 100644 --- a/src/chad/detail/lvr2.cpp +++ b/src/chad/detail/lvr2.cpp @@ -20,7 +20,6 @@ namespace chad::detail { m_voxelsize = voxel_res; m_truncsize = trunc_dist; BoxT::m_voxelsize = voxel_res; - const float recip = 1.0 / m_voxelsize; // trackers for the traversed path and nodes std::array path_child; diff --git a/src/chad/main.cpp b/src/chad/main.cpp index 9e9e205..7e36064 100644 --- a/src/chad/main.cpp +++ b/src/chad/main.cpp @@ -6,7 +6,7 @@ void static do_sphere_thing() { // generate random point data - std::vector points { 1'000'000 }; + std::vector points { 100'000 }; std::random_device rd; std::mt19937 gen(420); std::uniform_real_distribution dis(-1.0f, 1.0f); @@ -15,10 +15,6 @@ void static do_sphere_thing() { chad::TSDFMap map; std::vector positions { { 0, 0, 0 }, - // { 2, 2, 2 }, - // { 4, 4, 4 }, - // { 6, 6, 6 }, - // { 8, 8, 8 }, }; for (size_t i = 0; i < positions.size(); i++) { for (auto& point: points) { @@ -40,7 +36,6 @@ void static do_sphere_thing() { } } map.save("mesh.ply"); - // map.begin(1); } int main() { do_sphere_thing(); diff --git a/src/chad/tsdf.cpp b/src/chad/tsdf.cpp index 2977c84..e6038eb 100644 --- a/src/chad/tsdf.cpp +++ b/src/chad/tsdf.cpp @@ -106,24 +106,20 @@ namespace chad { // } // auto TSDFMap::begin(uint32_t root_addr) const -> iterator { // iterator it = iterator(*_node_levels_p, root_addr); - // // validate root address // if (root_addr >= _node_levels_p->_nodes[0]._raw_data.size()) { // throw std::runtime_error("chad::TSDFMap::begin() given faulty root address"); // return it; // } - // // walk to the first leaf node // uint32_t depth = 0; // while (true) { // uint8_t child_i = it._node_paths[depth]++; - // // standard node // if (depth < detail::NodeLevels::MAX_DEPTH - 1) { // // try to find the child in current node // uint32_t node_addr = it._node_addrs[depth]; // uint32_t child_addr = it._node_levels.get_child_addr(depth, node_addr, child_i); - // // check if child address is valid // if (child_addr > 0) { // // assign child addr at next depth @@ -140,7 +136,6 @@ namespace chad { // break; // } // } - // // reconstruct morton code from path // uint64_t code = 0; // for (uint64_t k = 0; k < detail::NodeLevels::MAX_DEPTH; k++) { From f39b434011e282eb5b31ae5b9577c17c45db5894 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Thu, 26 Jun 2025 15:42:33 +0200 Subject: [PATCH 4/8] updated lvr2 rec --- src/chad/detail/lvr2.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/chad/detail/lvr2.cpp b/src/chad/detail/lvr2.cpp index 8bec26b..db655e6 100644 --- a/src/chad/detail/lvr2.cpp +++ b/src/chad/detail/lvr2.cpp @@ -93,6 +93,7 @@ namespace chad::detail { glm::vec3(-1, +1, +1), glm::vec3(-1, -1, +1), glm::vec3(+1, -1, +1), + // glm::vec3(+1, +1, -1), glm::vec3(-1, +1, -1), glm::vec3(-1, -1, -1), @@ -102,12 +103,10 @@ namespace chad::detail { // create morton code of cell // convert position back to chunk index glm::ivec3 cell_chunk = leaf_chunk + cell_offsets[i]; - glm::vec3 cell_center = glm::vec3(cell_chunk) * m_voxelsize + m_voxelsize / 2.0f; + // glm::vec3 cell_center = glm::vec3(cell_chunk) * m_voxelsize + m_voxelsize / 2.0f; // emplace cell into map, check if it already existed auto [box_it, emplaced] = m_cells.emplace(MortonCode(cell_chunk)._value, nullptr); - if (emplaced) { - box_it->second = new BoxT(BaseVecT(cell_center.x, cell_center.y, cell_center.z)); - } + if (emplaced) box_it->second = new BoxT(BaseVecT(0, 0, 0) /*unused*/); // place query point at the correct cell index box_it->second->setVertex(i, querypoint_i); } @@ -164,6 +163,7 @@ namespace chad::detail { (void)j; (void)k; (void)distance; + fmt::println("LVR2 addLatticePoint() unimplemented"); } void saveGrid(std::string file) override { (void)file; @@ -228,6 +228,7 @@ namespace chad::detail { (void)bb; (void)duplicates; (void)comparePrecision; + fmt::println("LVR2 getMesh(mesh, bb, duplicates, comparePrecision) unimplemented"); } void getMesh(lvr2::BaseMesh &mesh) override { // Status message for mesh generation @@ -244,12 +245,10 @@ namespace chad::detail { { b = it->second; b->getSurface(mesh, m_grid->getQueryPoints(), global_index); - if(!lvr2::timestamp.isQuiet()) - ++progress; + if(!lvr2::timestamp.isQuiet()) ++progress; } - if(!lvr2::timestamp.isQuiet()) - cout << endl; + if(!lvr2::timestamp.isQuiet()) cout << endl; lvr2::BoxTraits traits; @@ -366,6 +365,7 @@ namespace chad::detail { // begin 3D mesh reconstruction using LVR2 typedef lvr2::BaseVector VecT; typedef lvr2::BilinearFastBox BoxT; + // typedef lvr2::SharpBox BoxT; // create hash grid from entire tree // generate mesh from hash grid From 0abbf4373d11908ad7f90a7ce36adc1036976ec8 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Mon, 30 Jun 2025 10:37:20 +0200 Subject: [PATCH 5/8] synced post-lvr2 fix on chad side --- src/chad/detail/lvr2.cpp | 122 +++++++-------------------------------- 1 file changed, 21 insertions(+), 101 deletions(-) diff --git a/src/chad/detail/lvr2.cpp b/src/chad/detail/lvr2.cpp index db655e6..3251836 100644 --- a/src/chad/detail/lvr2.cpp +++ b/src/chad/detail/lvr2.cpp @@ -81,23 +81,34 @@ namespace chad::detail { // DEBUG // float perf_signed_distance = glm::length(leaf_pos) - 5.0f; // signed_distance = perf_signed_distance; + // signed_distance = -signed_distance; // fmt::println("sd {:.2f}, perf {:.2f}, pos {:.2f} {:.2f} {:.2f}", signed_distance, perf_signed_distance, leaf_pos.x, leaf_pos.y, leaf_pos.z); // create query point size_t querypoint_i = m_queryPoints.size(); m_queryPoints.emplace_back(BaseVecT(leaf_pos.x, leaf_pos.y, leaf_pos.z), signed_distance); - + // 8 cells around the query point - const std::array cell_offsets = { - glm::vec3(+1, +1, +1), - glm::vec3(-1, +1, +1), - glm::vec3(-1, -1, +1), - glm::vec3(+1, -1, +1), + const std::array cell_offsets { + // glm::ivec3(+1, +1, +1), + // glm::ivec3(-1, +1, +1), + // glm::ivec3(-1, -1, +1), + // glm::ivec3(+1, -1, +1), + // // + // glm::ivec3(+1, +1, -1), + // glm::ivec3(-1, +1, -1), + // glm::ivec3(-1, -1, -1), + // glm::ivec3(+1, -1, -1), + // }; + glm::ivec3(+0, +0, +0), + glm::ivec3(-1, +0, +0), + glm::ivec3(-1, -1, +0), + glm::ivec3(+0, -1, +0), // - glm::vec3(+1, +1, -1), - glm::vec3(-1, +1, -1), - glm::vec3(-1, -1, -1), - glm::vec3(+1, -1, -1), + glm::ivec3(+0, +0, -1), + glm::ivec3(-1, +0, -1), + glm::ivec3(-1, -1, -1), + glm::ivec3(+0, -1, -1), }; for (size_t i = 0; i < 8; i++) { // create morton code of cell @@ -252,97 +263,6 @@ namespace chad::detail { lvr2::BoxTraits traits; - if(traits.type == "SharpBox") // Perform edge flipping for extended marching cubes - { - string SFComment = lvr2::timestamp.getElapsedTime() + "Flipping edges "; - lvr2::ProgressBar SFProgress(this->m_grid->getNumberOfCells(), SFComment); - for(it = this->m_grid->firstCell(); it != this->m_grid->lastCell(); it++) - { - - lvr2::SharpBox* sb; - sb = reinterpret_cast* >(it->second); - if(sb->m_containsSharpFeature) - { - lvr2::OptionalVertexHandle v1; - lvr2::OptionalVertexHandle v2; - lvr2::OptionalEdgeHandle e; - - if(sb->m_containsSharpCorner) - { - // 1 - v1 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][0]]; - v2 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][1]]; - - if(v1 && v2) - { - e = mesh.getEdgeBetween(v1.unwrap(), v2.unwrap()); - if(e) - { - mesh.flipEdge(e.unwrap()); - } - } - - // 2 - v1 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][2]]; - v2 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][3]]; - - if(v1 && v2) - { - e = mesh.getEdgeBetween(v1.unwrap(), v2.unwrap()); - if(e) - { - mesh.flipEdge(e.unwrap()); - } - } - - // 3 - v1 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][4]]; - v2 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][5]]; - - if(v1 && v2) - { - e = mesh.getEdgeBetween(v1.unwrap(), v2.unwrap()); - if(e) - { - mesh.flipEdge(e.unwrap()); - } - } - - } - else - { - // 1 - v1 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][0]]; - v2 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][1]]; - - if(v1 && v2) - { - e = mesh.getEdgeBetween(v1.unwrap(), v2.unwrap()); - if(e) - { - mesh.flipEdge(e.unwrap()); - } - } - - // 2 - v1 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][4]]; - v2 = sb->m_intersections[lvr2::ExtendedMCTable[sb->m_extendedMCIndex][5]]; - - if(v1 && v2) - { - e = mesh.getEdgeBetween(v1.unwrap(), v2.unwrap()); - if(e) - { - mesh.flipEdge(e.unwrap()); - } - } - } - } - ++SFProgress; - } - cout << endl; - } - if(traits.type == "BilinearFastBox") { string comment = lvr2::timestamp.getElapsedTime() + "Optimizing plane contours "; From 908f204add430bd7bcc30dd8633381c5b71690e6 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Tue, 1 Jul 2025 11:06:23 +0200 Subject: [PATCH 6/8] adjusted normal calc --- include/chad/detail/normals.hpp | 140 ++++++++++++++++++- src/chad/detail/normals.cpp | 241 -------------------------------- 2 files changed, 139 insertions(+), 242 deletions(-) delete mode 100644 src/chad/detail/normals.cpp diff --git a/include/chad/detail/normals.hpp b/include/chad/detail/normals.hpp index e03b811..3f87d09 100644 --- a/include/chad/detail/normals.hpp +++ b/include/chad/detail/normals.hpp @@ -1,7 +1,145 @@ #pragma once +#include +#include #include +#include #include "chad/detail/morton.hpp" namespace chad::detail { - auto estimate_normals(const MortonVector& points_mc, const glm::vec3 position) -> std::vector; + // sourced from: https://www.ilikebigbits.com/2017_09_25_plane_from_points_2.html + auto inline estimate_normal(const MortonVector::const_iterator beg, const MortonVector::const_iterator end) -> glm::vec3 { + // calculate centroid by through coefficient average + glm::dvec3 centroid { 0, 0, 0 }; + for (auto it = beg; it != end; it++) { + centroid += glm::dvec3(it->first); + } + double recip = 1.0 / double(std::distance(beg, end)); + centroid *= recip; + + // covariance matrix excluding symmetries + double xx = 0.0; double xy = 0.0; double xz = 0.0; + double yy = 0.0; double yz = 0.0; double zz = 0.0; + for (auto it = beg; it != end; it++) { + glm::dvec3 r = glm::dvec3(it->first) - centroid; + xx += r.x * r.x; + xy += r.x * r.y; + xz += r.x * r.z; + yy += r.y * r.y; + yz += r.y * r.z; + zz += r.z * r.z; + } + xx *= recip; + xy *= recip; + xz *= recip; + yy *= recip; + yz *= recip; + zz *= recip; + + // weighting linear regression based on square determinant + glm::dvec3 weighted_dir = { 0, 0, 0 }; + + // determinant x + { + double det_x = yy*zz - yz*yz; + glm::dvec3 axis_dir = { + det_x, + xz*yz - xy*zz, + xy*yz - xz*yy + }; + double weight = det_x * det_x; + if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; + weighted_dir += axis_dir * weight; + } + // determinant y + { + double det_y = xx*zz - xz*xz; + glm::dvec3 axis_dir = { + xz*yz - xy*zz, + det_y, + xy*xz - yz*xx + }; + double weight = det_y * det_y; + if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; + weighted_dir += axis_dir * weight; + } + // determinant z + { + double det_z = xx*yy - xy*xy; + glm::dvec3 axis_dir = { + xy*yz - xz*yy, + xy*xz - yz*xx, + det_z + }; + double weight = det_z * det_z; + if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; + weighted_dir += axis_dir * weight; + } + + // return normalized weighted direction as surface normal + return glm::vec3(glm::normalize(weighted_dir)); + } + auto inline estimate_normals(const MortonVector& points_mc, const glm::vec3 position) -> std::vector { + auto beg = std::chrono::high_resolution_clock::now(); + + std::vector normals; + normals.resize(points_mc.size()); + for (auto it = points_mc.cbegin(); it != points_mc.cend();) { + + const uint32_t min_points = 8; + + // TODO: check that neighbourhood is as box shaped as possible + // check morton neighbourhoods for nearby points with increasing discretization + auto it_neigh_beg = it; + auto it_neigh_end = it + 1; + for (uint64_t depth = 0; depth < 3; depth++) { + // mask to strip the depth*3 LSBs of the morton code to match current discretization level + const uint64_t mc_mask = std::numeric_limits::max() << uint64_t(depth * 3); + const MortonCode mc_neigh = mc_mask & it->second; // discretized morton code + + // walk forward until morton code mismatches + while (it_neigh_end != points_mc.cend() - 1) /*bounds safety*/ { + MortonCode mc_next = mc_mask & it_neigh_end->second; + if (mc_next == mc_neigh) it_neigh_end++; + else break; + } + + // break out of loop once the neighbourhood has sufficient points for normal estimation + if (std::distance(it_neigh_beg, it_neigh_end) >= min_points) break; + } + + // estimate normal via neighbourhood if enough points where found + uint32_t neigh_size = std::distance(it_neigh_beg, it_neigh_end); + if (neigh_size >= min_points) { + // estimate via neighbourhood + glm::vec3 normal = estimate_normal(it_neigh_beg, it_neigh_end); + + // flip normal if needed (TODO: should this be moved into the it_neigh loop?) + float normal_dot = glm::dot(normal, glm::normalize(position - it->first)); + if (normal_dot < 0.0f) normal = -normal; + + // assign normal to all points within neighbourhood + for (auto it_neigh = it_neigh_beg; it_neigh != it_neigh_end; it_neigh++) { + size_t index = std::distance(points_mc.cbegin(), it_neigh); + normals[index] = normal; + } + } + // if not, simply use normalized vector from point to position + else { + // assign normal to all points within neighbourhood + for (auto it_neigh = it_neigh_beg; it_neigh != it_neigh_end; it_neigh++) { + glm::vec3 normal = glm::normalize(position - it_neigh->first); + size_t index = std::distance(points_mc.cbegin(), it_neigh); + normals[index] = normal; + } + } + + // increment point iterator to mark these points as handled + it += neigh_size; + } + + auto end = std::chrono::high_resolution_clock::now(); + auto dur = std::chrono::duration (end - beg).count(); + fmt::println("norm est {:.2f}", dur); + return normals; + } } \ No newline at end of file diff --git a/src/chad/detail/normals.cpp b/src/chad/detail/normals.cpp deleted file mode 100644 index f53757e..0000000 --- a/src/chad/detail/normals.cpp +++ /dev/null @@ -1,241 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "chad/detail/morton.hpp" - -namespace { - // sourced from: https://www.ilikebigbits.com/2017_09_25_plane_from_points_2.html - template - auto inline estimate_normal(const std::vector>& points) -> glm::vec3 { - // calculate centroid by through coefficient average - glm::vec<3, T, P> centroid { 0, 0, 0 }; - for (const auto& point: points) { - centroid += point; - } - T recip = 1.0 / (T)points.size(); - centroid *= recip; - - // covariance matrix excluding symmetries - T xx = 0.0; T xy = 0.0; T xz = 0.0; - T yy = 0.0; T yz = 0.0; T zz = 0.0; - for (auto point_it = points.cbegin(); point_it != points.cend(); point_it++) { - auto r = *point_it - centroid; - xx += r.x * r.x; - xy += r.x * r.y; - xz += r.x * r.z; - yy += r.y * r.y; - yz += r.y * r.z; - zz += r.z * r.z; - } - xx *= recip; - xy *= recip; - xz *= recip; - yy *= recip; - yz *= recip; - zz *= recip; - - // weighting linear regression based on square determinant - glm::vec<3, T, P> weighted_dir = { 0, 0, 0 }; - - // determinant x - { - T det_x = yy*zz - yz*yz; - glm::vec<3, T, P> axis_dir = { - det_x, - xz*yz - xy*zz, - xy*yz - xz*yy - }; - T weight = det_x * det_x; - if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; - weighted_dir += axis_dir * weight; - } - // determinant y - { - T det_y = xx*zz - xz*xz; - glm::vec<3, T, P> axis_dir = { - xz*yz - xy*zz, - det_y, - xy*xz - yz*xx - }; - T weight = det_y * det_y; - if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; - weighted_dir += axis_dir * weight; - } - // determinant z - { - T det_z = xx*yy - xy*xy; - glm::vec<3, T, P> axis_dir = { - xy*yz - xz*yy, - xy*xz - yz*xx, - det_z - }; - T weight = det_z * det_z; - if (glm::dot(weighted_dir, axis_dir) < 0.0) weight = -weight; - weighted_dir += axis_dir * weight; - } - - // return normalized weighted direction as surface normal - weighted_dir = glm::normalize(weighted_dir); - return { float(weighted_dir.x), float(weighted_dir.y), float(weighted_dir.z) }; - } -} - -namespace chad::detail { - struct MortonNeighbourhood { MortonVector::const_iterator beg, end; }; - using MortonNeighbourhoodMap = gtl::parallel_flat_hash_map; - - auto build_neighbourhood_map(const MortonVector& points_mc, const uint32_t level) -> MortonNeighbourhoodMap { - auto beg = std::chrono::high_resolution_clock::now(); - - // create mask to group morton codes up to a certain level - const uint64_t neigh_mask = std::numeric_limits::max() << size_t(level * 3); - - // create hash map for neighbourhoods, reserving generous space beforehand - MortonNeighbourhoodMap neigh_map; - static constexpr size_t min_points_per_neighbourhood = 8; - neigh_map.reserve(points_mc.size() / min_points_per_neighbourhood); - - // insert the first neighbourhood - MortonCode neigh_mc = points_mc[0].second; - neigh_mc._value &= neigh_mask; - MortonNeighbourhood neigh { points_mc.cbegin(), points_mc.cbegin() }; - auto neigh_it = neigh_map.emplace(neigh_mc, neigh).first; - - uint32_t neigh_count = 0; // track number of neighbourhoods - uint32_t neigh_points = 0; // track total points within all neighbourhoods - static constexpr uint32_t neigh_threshhold_checkup = 256; // perform point count check after this neigh_count - static constexpr uint32_t neigh_threshhold_abort = neigh_threshhold_checkup * min_points_per_neighbourhood; - - // iterate through all points to build neighbourhoods - for (auto morton_it = points_mc.cbegin()+1; morton_it != points_mc.cend(); morton_it++) { - // get morton codes from current point and current neighbourhood - MortonCode point_mc = morton_it->second._value & neigh_mask; - MortonCode neigh_mc = neigh_it->second.beg->second; - - // check if the current point doesnt fit the neighbourhood - if (point_mc != neigh_mc) { - // set end() iterator for current neighbourhood - neigh_it->second.end = morton_it; - // count points and neighbourhoods - neigh_count++; - neigh_points += std::distance(neigh_it->second.beg, morton_it); - if (neigh_count >= neigh_threshhold_checkup) { - // ensure enough points were placed in the neighbourhoods - if (neigh_points < neigh_threshhold_abort) { - fmt::println("nei {} (aborted)", level); - return {}; - } - } - // create a new neighbourhood starting at current point - MortonNeighbourhood neigh { morton_it, morton_it }; - neigh_it = neigh_map.emplace(point_mc, neigh).first; - } - } - // set end() iterator for final neighbourhood - neigh_it->second.end = points_mc.cend(); - - auto end = std::chrono::high_resolution_clock::now(); - auto dur = std::chrono::duration (end - beg).count(); - fmt::println("nei {} {:.2f}", level, dur); - return neigh_map; - } - auto estimate_normals(const MortonVector& points_mc, const glm::vec3 position) -> std::vector { - auto beg = std::chrono::high_resolution_clock::now(); - - // build morton code neighbourhood maps starting from the leaf level - uint32_t neigh_level = 0; - MortonNeighbourhoodMap neigh_map; - for (; neigh_level < 21; neigh_level++) { - neigh_map = build_neighbourhood_map(points_mc, neigh_level); - if (!neigh_map.empty()) break; - } - // verify that it found a fitting neighbourhood map - if (neigh_map.empty()) fmt::println("Too few points inserted for this voxel resolution"); - - auto end = std::chrono::high_resolution_clock::now(); - auto dur = std::chrono::duration (end - beg).count(); - fmt::println("nei calc {:.2f}", dur); - - beg = std::chrono::high_resolution_clock::now(); - std::vector normals; - normals.resize(points_mc.size()); - - // iterate over all neighbourhoods - for (const auto& neigh_entry: neigh_map) { - MortonCode neigh_mc = neigh_entry.first; - MortonNeighbourhood neigh = neigh_entry.second; - glm::ivec3 neigh_pos = neigh_mc.decode(); - - // gather adjacent neighbourhoods - uint32_t point_count = 0; - std::vector neigh_adj; - for (int32_t z = -1; z <= +1; z++) { - for (int32_t y = -1; y <= +1; y++) { - for (int32_t x = -1; x <= +1; x++) { - // construct neighbourhood position with offset - glm::ivec3 offset { x, y, z }; - offset *= 1 << neigh_level; - MortonCode mc_adj { neigh_pos + offset }; - // attempt to find this neighbourhood in the map - auto neigh_adj_it = neigh_map.find(mc_adj._value); - if (neigh_adj_it != neigh_map.cend()) { - MortonNeighbourhood neigh = neigh_adj_it->second; - neigh_adj.push_back(neigh); - // add point count from this neighbourhood - point_count += std::distance(neigh.beg, neigh.end); - } - }}} - - // supersample in case there are too many points - static constexpr uint32_t max_points = 64; - uint32_t step = point_count / max_points + 1; - - // gather points of all adjacent neighbourhoods - std::vector points_adj; - points_adj.reserve(max_points); - uint32_t counter = 0; - for (const auto& neigh: neigh_adj) { - for (auto it = neigh.beg; it != neigh.end; it++) { - if (counter++ >= step) { - points_adj.push_back(it->first); - counter = 0; - } - } - } - - if (points_adj.size() > 8) { - // estimate normal of points when theres sufficient data - glm::vec3 normal = estimate_normal(points_adj); - - // assign this normal to all center neighbourhood points - for (auto point_it = neigh.beg; point_it != neigh.end; point_it++) { - // flip normal if needed - float normal_dot = glm::dot(normal, glm::normalize(position - point_it->first)); - if (normal_dot > 0.0f) normal = -normal; - - // store normal - size_t point_index = std::distance(points_mc.cbegin(), point_it); - normals[point_index] = normal; - } - } - else { - // otherwise use normalized vector from point to position as normal - for (auto point_it = neigh.beg; point_it != neigh.end; point_it++) { - glm::vec3 normal = glm::normalize(position - point_it->first); - - // store normal - size_t point_index = std::distance(points_mc.cbegin(), point_it); - normals[point_index] = normal; - } - } - } - - end = std::chrono::high_resolution_clock::now(); - dur = std::chrono::duration (end - beg).count(); - fmt::println("norm est {:.2f}", dur); - return normals; - } -} \ No newline at end of file From b1c829c4052eb904d6825d7cfbb2832ca39e18d7 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Tue, 1 Jul 2025 11:06:33 +0200 Subject: [PATCH 7/8] simplified .cpp setup --- CMakeLists.txt | 2 - include/chad/cluster.hpp | 4 +- include/chad/detail/lvr2.hpp | 1 - include/chad/detail/morton.hpp | 73 +++++++++++++++++++++++++++++++--- src/chad/detail/morton.cpp | 51 ------------------------ src/chad/main.cpp | 4 +- 6 files changed, 72 insertions(+), 63 deletions(-) delete mode 100644 src/chad/detail/morton.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c06ca6a..5b7a00a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,8 +14,6 @@ include("cmake/options_compiler.cmake") set(CPP_SOURCE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/src/chad/tsdf.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/chad/detail/lvr2.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/src/chad/detail/morton.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/src/chad/detail/normals.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/chad/detail/virtual_array.cpp") add_library(${PROJECT_NAME} ${CPP_SOURCE_FILES}) add_library(chad::tsdf ALIAS ${PROJECT_NAME}) diff --git a/include/chad/cluster.hpp b/include/chad/cluster.hpp index 9a8fb0c..39381ff 100644 --- a/include/chad/cluster.hpp +++ b/include/chad/cluster.hpp @@ -5,7 +5,7 @@ #include namespace chad { - // cluster of 8 separate 8-bit leaves + // cluster of 8 separate 8-bit leaves, choose only one of the union partitions struct LeafCluster { // wrapper for signed distance cluster struct TSDFs { @@ -94,7 +94,7 @@ namespace chad { } union { - uint64_t _value; + uint64_t _value; // raw cluster data interpreted as 64-bit uint TSDFs _tsdfs; Weights _weigh; Ufloats _ufloats; diff --git a/include/chad/detail/lvr2.hpp b/include/chad/detail/lvr2.hpp index 39829aa..71c1bd6 100644 --- a/include/chad/detail/lvr2.hpp +++ b/include/chad/detail/lvr2.hpp @@ -4,6 +4,5 @@ #include "chad/detail/submap.hpp" namespace chad::detail { - void reconstruct(const detail::Submap& submap, const NodeLevels& node_levels, float voxel_res, float trunc_dist, std::string_view filename); } \ No newline at end of file diff --git a/include/chad/detail/morton.hpp b/include/chad/detail/morton.hpp index 08ce0f0..b297863 100644 --- a/include/chad/detail/morton.hpp +++ b/include/chad/detail/morton.hpp @@ -1,10 +1,13 @@ #pragma once #include +#include #include +#include #include #if !defined(__BMI2__) # error "Requires BMI2 instruction set" #endif +#include #include #include @@ -42,20 +45,80 @@ namespace chad::detail { bool inline operator>(const MortonCode& other) const noexcept { return _value > other._value; } + auto friend operator&(MortonCode lhs, const MortonCode& rhs) noexcept -> MortonCode { + return lhs._value & rhs._value; + } + auto friend operator&(MortonCode lhs, const uint64_t& rhs) noexcept -> MortonCode { + return lhs._value & rhs; + } + uint64_t _value; }; - using MortonVector = std::vector>; - auto calc_morton_vector(const std::vector>& points, const float sdf_res) -> MortonVector; - auto sort_morton_vector(MortonVector& points_mc) -> std::vector; + auto inline calc_morton_vector(const std::vector>& points, const float sdf_res) -> MortonVector { + auto beg = std::chrono::high_resolution_clock::now(); + + // calc reciprocal of voxel resolution for later + const float voxel_reciprocal = float(1.0 / double(sdf_res)); + + // generate morton codes from discretized points + MortonVector points_mc; + points_mc.reserve(points.size()); + for (const auto& point_arr: points) { + glm::vec3 point { point_arr[0], point_arr[1], point_arr[2] }; + // convert to voxel coordinate and discretize with floor() + glm::vec3 point_discretized = glm::floor(point * voxel_reciprocal); + // create morton code from discretized integer position + points_mc.emplace_back(point, glm::ivec3(point_discretized)); + } + + auto end = std::chrono::high_resolution_clock::now(); + auto dur = std::chrono::duration (end - beg).count(); + fmt::println("mc calc {:.2f}", dur); + return points_mc; + } + auto inline sort_morton_vector(MortonVector& points_mc) -> std::vector { + auto beg = std::chrono::high_resolution_clock::now(); + + // sort points using morton codes + auto mc_sorter = [](const auto& a, const auto& b){ + return a.second._value > b.second._value; + }; + // std::sort(std::execution::par_unseq, points_mc.begin(), points_mc.end(), mc_sorter); + std::sort(points_mc.begin(), points_mc.end(), mc_sorter); + + // store isolated sorted points + std::vector points_sorted; + points_sorted.reserve(points_mc.size()); + for (const auto& mc_point: points_mc) { + points_sorted.push_back(mc_point.first); + } + + auto end = std::chrono::high_resolution_clock::now(); + auto dur = std::chrono::duration (end - beg).count(); + fmt::println("mc sort {:.2f}", dur); + return points_sorted; + } } -// overload the standard hashing operator for MortonCode +// specialize the std hashing operator for MortonCode namespace std { - template<> struct hash { + template<> + struct hash { size_t inline operator()(const chad::detail::MortonCode& mc) const noexcept { return mc._value; } }; +} + +// specialize the fmt formatter for MortonCode +namespace fmt { + template<> + struct formatter: formatter { + auto format(const chad::detail::MortonCode& mc, format_context& ctx) const -> format_context::iterator { + std::string str = std::bitset<63>(mc._value).to_string(); + return formatter::format(str, ctx); + } + }; } \ No newline at end of file diff --git a/src/chad/detail/morton.cpp b/src/chad/detail/morton.cpp deleted file mode 100644 index 10fe226..0000000 --- a/src/chad/detail/morton.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include -#include -#include -#include "chad/detail/morton.hpp" - -namespace chad::detail { - auto calc_morton_vector(const std::vector>& points, const float sdf_res) -> MortonVector { - auto beg = std::chrono::high_resolution_clock::now(); - - // calc reciprocal of voxel resolution for later - const float voxel_reciprocal = float(1.0 / double(sdf_res)); - - // generate morton codes from discretized points - MortonVector points_mc; - points_mc.reserve(points.size()); - for (const auto& point_arr: points) { - glm::vec3 point { point_arr[0], point_arr[1], point_arr[2] }; - // convert to voxel coordinate and discretize with floor() - glm::vec3 point_discretized = glm::floor(point * voxel_reciprocal); - // create morton code from discretized integer position - points_mc.emplace_back(point, glm::ivec3(point_discretized)); - } - - auto end = std::chrono::high_resolution_clock::now(); - auto dur = std::chrono::duration (end - beg).count(); - fmt::println("mc calc {:.2f}", dur); - return points_mc; - } - auto sort_morton_vector(MortonVector& points_mc) -> std::vector { - auto beg = std::chrono::high_resolution_clock::now(); - - // sort points using morton codes - auto mc_sorter = [](const auto& a, const auto& b){ - return a.second._value > b.second._value; - }; - // std::sort(std::execution::par_unseq, points_mc.begin(), points_mc.end(), mc_sorter); - std::sort(points_mc.begin(), points_mc.end(), mc_sorter); - - // store isolated sorted points - std::vector points_sorted; - points_sorted.reserve(points_mc.size()); - for (const auto& mc_point: points_mc) { - points_sorted.push_back(mc_point.first); - } - - auto end = std::chrono::high_resolution_clock::now(); - auto dur = std::chrono::duration (end - beg).count(); - fmt::println("mc sort {:.2f}", dur); - return points_sorted; - } -} \ No newline at end of file diff --git a/src/chad/main.cpp b/src/chad/main.cpp index 7e36064..158e02c 100644 --- a/src/chad/main.cpp +++ b/src/chad/main.cpp @@ -6,13 +6,13 @@ void static do_sphere_thing() { // generate random point data - std::vector points { 100'000 }; + std::vector points { 1'000'000 }; std::random_device rd; std::mt19937 gen(420); std::uniform_real_distribution dis(-1.0f, 1.0f); // insert into CHAD TSDF - chad::TSDFMap map; + chad::TSDFMap map{ 0.05f, 0.1f }; std::vector positions { { 0, 0, 0 }, }; From 9da75b4bd3b1a28379b690ed65b4f8f2429a4536 Mon Sep 17 00:00:00 2001 From: Jan Kuhlmann <33833587+M2-TE@users.noreply.github.com> Date: Wed, 2 Jul 2025 09:58:25 +0200 Subject: [PATCH 8/8] reconstruction fixes --- include/chad/cluster.hpp | 16 ++++++++-------- include/chad/detail/normals.hpp | 4 ++++ src/chad/detail/lvr2.cpp | 21 ++++++--------------- src/chad/detail/virtual_array.cpp | 2 +- src/chad/main.cpp | 8 ++++---- 5 files changed, 23 insertions(+), 28 deletions(-) diff --git a/include/chad/cluster.hpp b/include/chad/cluster.hpp index 39381ff..700b91a 100644 --- a/include/chad/cluster.hpp +++ b/include/chad/cluster.hpp @@ -5,9 +5,9 @@ #include namespace chad { - // cluster of 8 separate 8-bit leaves, choose only one of the union partitions + // Cluster of 8 separate 8-bit leaves. Choose only one of the union partitions. struct LeafCluster { - // wrapper for signed distance cluster + // Wrapper for cluster of 8 TSDF values struct TSDFs { // set 8 bits to represent signed distance, normalized within truncation distance void inline set(uint8_t leaf_i, float signed_distance, float sdf_trunc_recip) noexcept { @@ -52,7 +52,7 @@ namespace chad { } uint64_t _value; }; - // wrapper for weight cluster + // Wrapper for cluster of 8 weights struct Weights { // set 8 bits to represent a single weight void inline set(uint8_t leaf_i, uint8_t weight) noexcept { @@ -67,19 +67,19 @@ namespace chad { // } uint64_t _value; }; - // wrapper for cluster of 8 unsigned floats (+0.0f to +1.0f) + // Wrapper for cluster of 8 unsigned floats (+0.0f to +1.0f) struct Ufloats { // TODO }; - // wrapper for cluster of 8 signed floats (-1.0f to +1.0f) + // Wrapper for cluster of 8 signed floats (-1.0f to +1.0f) struct Sfloats { // TODO }; - // wrapper for cluster of 8 uint8_t values (0xff is reserved) + // Wrapper for cluster of 8 uint8_t values (0xff is reserved) struct Uints { // TODO }; - // wrapper for cluster of 8 int8_t values (0xff is reserved) + // Wrapper for cluster of 8 int8_t values (0xff is reserved) struct Sints { // TODO }; @@ -94,7 +94,7 @@ namespace chad { } union { - uint64_t _value; // raw cluster data interpreted as 64-bit uint + uint64_t _value; // Raw cluster data as 64-bit uint TSDFs _tsdfs; Weights _weigh; Ufloats _ufloats; diff --git a/include/chad/detail/normals.hpp b/include/chad/detail/normals.hpp index 3f87d09..3dc6d15 100644 --- a/include/chad/detail/normals.hpp +++ b/include/chad/detail/normals.hpp @@ -137,6 +137,10 @@ namespace chad::detail { it += neigh_size; } + + // for (auto& normal: normals) normal = -normal; + // fmt::println("FLIPPED NORMALS FOR DEBUG RUN"); + auto end = std::chrono::high_resolution_clock::now(); auto dur = std::chrono::duration (end - beg).count(); fmt::println("norm est {:.2f}", dur); diff --git a/src/chad/detail/lvr2.cpp b/src/chad/detail/lvr2.cpp index 3251836..197a8d5 100644 --- a/src/chad/detail/lvr2.cpp +++ b/src/chad/detail/lvr2.cpp @@ -90,16 +90,6 @@ namespace chad::detail { // 8 cells around the query point const std::array cell_offsets { - // glm::ivec3(+1, +1, +1), - // glm::ivec3(-1, +1, +1), - // glm::ivec3(-1, -1, +1), - // glm::ivec3(+1, -1, +1), - // // - // glm::ivec3(+1, +1, -1), - // glm::ivec3(-1, +1, -1), - // glm::ivec3(-1, -1, -1), - // glm::ivec3(+1, -1, -1), - // }; glm::ivec3(+0, +0, +0), glm::ivec3(-1, +0, +0), glm::ivec3(-1, -1, +0), @@ -111,11 +101,8 @@ namespace chad::detail { glm::ivec3(+0, -1, -1), }; for (size_t i = 0; i < 8; i++) { - // create morton code of cell - // convert position back to chunk index - glm::ivec3 cell_chunk = leaf_chunk + cell_offsets[i]; - // glm::vec3 cell_center = glm::vec3(cell_chunk) * m_voxelsize + m_voxelsize / 2.0f; // emplace cell into map, check if it already existed + glm::ivec3 cell_chunk = leaf_chunk + cell_offsets[i]; auto [box_it, emplaced] = m_cells.emplace(MortonCode(cell_chunk)._value, nullptr); if (emplaced) box_it->second = new BoxT(BaseVecT(0, 0, 0) /*unused*/); // place query point at the correct cell index @@ -140,10 +127,14 @@ namespace chad::detail { delete cell->second; m_cells.erase(cell); } + + // TODO: connect cell "neighbours" std::cout << "ChadGrid created" << std::endl; } ~ChadGrid() { - lvr2::GridBase::~GridBase(); + for (auto& [ _, cell ]: m_cells) { + delete cell; + } } auto getNumberOfCells() -> std::size_t { diff --git a/src/chad/detail/virtual_array.cpp b/src/chad/detail/virtual_array.cpp index 4830224..d72a8b4 100644 --- a/src/chad/detail/virtual_array.cpp +++ b/src/chad/detail/virtual_array.cpp @@ -1,7 +1,7 @@ #include #include #include -#if defined(linux) || defined (__unix__) +#if defined (__unix__) # include # include #elif defined(_WIN32) || defined(_WIN64) diff --git a/src/chad/main.cpp b/src/chad/main.cpp index 158e02c..3b5c2ac 100644 --- a/src/chad/main.cpp +++ b/src/chad/main.cpp @@ -30,10 +30,10 @@ void static do_sphere_thing() { } map.insert(points, positions[i]); - std::ofstream ofs("points.asc"); - for (const auto& point: points) { - ofs << point.x << ' ' << point.y << ' ' << point.z << '\n'; - } + // std::ofstream ofs("points.asc"); + // for (const auto& point: points) { + // ofs << point.x << ' ' << point.y << ' ' << point.z << '\n'; + // } } map.save("mesh.ply"); }