Merge from dev

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})
@@ -25,8 +23,8 @@ target_link_libraries(${PROJECT_NAME} PRIVATE ${CMAKE_DL_LIBS})
 include("cmake/fmt.cmake")
 include("cmake/gtl.cmake")
 include("cmake/glm.cmake")
-include("cmake/lvr2.cmake")
 include("cmake/eigen.cmake")
+include("cmake/lvr2.cmake")
 include("cmake/morton.cmake")
 
 # compile executable if top level project

include/chad/cluster.hpp

@@ -5,9 +5,9 @@
 #include <algorithm>
 
 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
+        // 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;
+            uint64_t _value; // Raw cluster data as 64-bit uint
             TSDFs    _tsdfs;
             Weights  _weigh;
             Ufloats  _ufloats;

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);
 }

include/chad/detail/morton.hpp

@@ -1,10 +1,13 @@
 #pragma once
 #include <array>
+#include <chrono>
 #include <vector>
+#include <bitset>
 #include <functional>
 #if !defined(__BMI2__)
 #   error "Requires BMI2 instruction set"
 #endif
+#include <fmt/format.h>
 #include <glm/glm.hpp>
 #include <libmorton/morton.h>
 
@@ -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<std::pair<glm::vec3, MortonCode>>;
-    auto calc_morton_vector(const std::vector<std::array<float, 3>>& points, const float sdf_res) -> MortonVector;
-    auto sort_morton_vector(MortonVector& points_mc) -> std::vector<glm::vec3>;
+    auto inline calc_morton_vector(const std::vector<std::array<float, 3>>& 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<double, std::milli> (end - beg).count();
+        fmt::println("mc  calc {:.2f}", dur);
+        return points_mc;
+    }
+    auto inline sort_morton_vector(MortonVector& points_mc) -> std::vector<glm::vec3> {
+        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<glm::vec3> 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<double, std::milli> (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<chad::detail::MortonCode> {
+    template<>
+    struct hash<chad::detail::MortonCode> {
         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<chad::detail::MortonCode>: formatter<std::string> {
+        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<std::string>::format(str, ctx);
+        }
+    };
 }

include/chad/detail/normals.hpp

@@ -1,7 +1,149 @@
 #pragma once
+#include <chrono>
+#include <cstdint>
 #include <glm/glm.hpp>
+#include <gtl/phmap.hpp>
 #include "chad/detail/morton.hpp"
 
 namespace chad::detail {
-    auto estimate_normals(const MortonVector& points_mc, const glm::vec3 position) -> std::vector<glm::vec3>;
+    // 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<glm::vec3> {
+        auto beg = std::chrono::high_resolution_clock::now();
+
+        std::vector<glm::vec3> 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<uint64_t>::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;
+        }
+
+
+        // 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<double, std::milli> (end - beg).count();
+        fmt::println("norm est {:.2f}", dur);
+        return normals;
+    }
 }

include/chad/tsdf.hpp

@@ -49,19 +49,17 @@ 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<const std::array<float, 3>*>(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<std::array<float, 3>>(vec_p, vec_p + vec_count);
+            const auto points = std::vector<std::array<float, 3>>(vec_p, vec_p + points_count);
             // position_p should just be x y and z
             const auto position = *reinterpret_cast<const std::array<float, 3>*>(position_p);
             insert(points, position);
         }
         // 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<const std::array<float, 3>*>(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<std::array<float, 3>>(vec_p, vec_p + vec_count);
+            const auto points = std::vector<std::array<float, 3>>(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<std::array<float, 3>> points_vec;
@@ -102,10 +99,9 @@ namespace chad {
             // insert pointcloud alongside scanner position
             void inline insert(const std::vector<Eigen::Vector3f>& 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);
+                    insert(points_p, points.size(), position.x(), position.y(), position.z());
                 }
                 else {
                     std::vector<std::array<float, 3>> points_vec;