WIP

CMakeLists.txt

@@ -5,17 +5,24 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-set (CMAKE_CXX_DEBUG_FLAGS "${CMAKE_CXX_FLAGS} -fno-omit-frame-pointer -fsanitize=address")
-set (CMAKE_LINKER_DEBUG_FLAGS "${CMAKE_LINKER_FLAGS} -fno-omit-frame-pointer -fsanitize=address")
+set (CMAKE_CXX_DEBUG_FLAGS "${CMAKE_CXX_DEBUG_FLAGS} -fno-omit-frame-pointer -fsanitize=address")
+set (CMAKE_LINKER_DEBUG_FLAGS "${CMAKE_LINKER_DEBUG_FLAGS} -fno-omit-frame-pointer -fsanitize=address")
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -msse4.1)
 endif()
 
+
 option(BENCHMARK_OCTOMAP "Benchmark against Octomap" ON)
 option(BENCHMARK_OPENVDB "Benchmark against OpenVDB" ON)
+option(ENABLE_SSE4_1 "Compile with SSE 4.1 support" ON)
 #option(BENCHMARK_OPEN3D "Benchmark against Open3D" OFF)
 
+if(ENABLE_SSE4_1)
+    add_compile_options(-msse4.1)
+    set ( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DTREEXY_USE_SSE")
+endif()
+
 find_package(benchmark REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common io)
 
@@ -68,3 +75,6 @@ if( BENCHMARK_OCTOMAP AND BENCHMARK_OPENVDB)
     target_link_libraries(get_memory ${BENCHMARK_DEPENDENCIES} )
 endif()
 
+add_executable(test test/test.cpp )
+
+

benchmark/benchmark_treexy.cpp

@@ -15,7 +15,7 @@ static void Treexy_Create(benchmark::State& state)
 
     for (const auto& point : *cloud)
     {
-      auto coord = grid.posToCoord({ point.x, point.y, point.z });
+      auto coord = grid.posToCoord(point.x, point.y, point.z);
       accessor.setValue(coord, 42);
     }
   }

include/treexy/node_mask.hpp

@@ -96,6 +96,16 @@ class Mask
     return WORD_COUNT;
   }
 
+  uint64_t getWord(size_t n) const
+  {
+    return mWords[n];
+  }
+
+  void setWord(size_t n, uint64_t v)
+  {
+    mWords[n] = v;
+  }
+
   uint32_t countOn() const
   {
     uint32_t sum = 0, n = WORD_COUNT;

include/treexy/serialization.hpp

@@ -0,0 +1,192 @@
+#ifndef SERIALIZATION_HPP
+#define SERIALIZATION_HPP
+
+#include <iostream>
+#include <stdio.h>
+#include <cstring>
+#include <typeinfo>
+#include <exception>
+#include <type_traits>
+#include <fstream>
+#include "treexy/treexy.hpp"
+
+namespace Treexy
+{
+
+#ifdef __GNUG__
+#include <cstdlib>
+#include <memory>
+#include <cxxabi.h>
+
+std::string demangle(const char* name)
+{
+    int status = -4; // some arbitrary value to eliminate the compiler warning
+
+    std::unique_ptr<char, void(*)(void*)> res {
+        abi::__cxa_demangle(name, NULL, NULL, &status),
+        std::free
+    };
+    return (status==0) ? res.get() : name ;
+}
+
+#else
+
+// does nothing if not g++
+std::string demangle(const char* name)
+{
+    return name;
+}
+#endif
+
+template <typename T>
+inline void Write(std::ostream& out, const T& val)
+{
+  static_assert(std::is_trivially_copyable_v<T>, "Must be trivially copyable");
+  out.write(reinterpret_cast<const char*>(&val), sizeof(T));
+}
+
+template <typename DataT, int IBITS, int LBITS> inline
+void Serialize(std::ostream& out, const VoxelGrid<DataT, IBITS, LBITS>& grid)
+{
+  static_assert(std::is_trivially_copyable_v<DataT>,
+                "DataT must ne trivially copyable");
+
+  char header[256];
+  std::string type_name = demangle(typeid(DataT).name());
+
+  sprintf(header, "Treexy::VoxelGrid<%s,%d,%d>(%lf)\n",
+          type_name.c_str(), IBITS, LBITS,grid.resolution );
+
+  out.write(header, std::strlen(header));
+
+  //------------
+  Write(out, uint32_t(grid.root_map.size()));
+
+  for (const auto& it : grid.root_map)
+  {
+    const CoordT& root_coord = it.first;
+    Write(out, root_coord.x);
+    Write(out, root_coord.y);
+    Write(out, root_coord.z);
+
+    const auto& inner_grid = it.second;
+    for (size_t w = 0; w < inner_grid.mask.wordCount(); w++)
+    {
+      Write(out, inner_grid.mask.getWord(w));
+    }
+    for (auto inner = inner_grid.mask.beginOn(); inner; ++inner)
+    {
+      const uint32_t inner_index = *inner;
+      const auto& leaf_grid = *(inner_grid.data[inner_index]);
+
+      for (size_t w = 0; w < leaf_grid.mask.wordCount(); w++)
+      {
+        Write(out, leaf_grid.mask.getWord(w));
+      }
+      for (auto leaf = leaf_grid.mask.beginOn(); leaf; ++leaf)
+      {
+        const uint32_t leaf_index = *leaf;
+        Write(out, leaf_grid.data[leaf_index]);
+      }
+    }
+  }
+}
+
+template <typename T>
+inline T Read(std::istream& input)
+{
+  T out;
+  static_assert(std::is_trivially_copyable_v<T>, "Must be trivially copyable");
+  input.read(reinterpret_cast<char*>(&out), sizeof(T));
+  return out;
+}
+
+template <typename DataT, int IBITS=2, int LBITS=3> inline
+VoxelGrid<DataT, IBITS, LBITS> Deserialize(std::istream& input)
+{
+    char header[256];
+    input.getline(header, 256);
+
+    char type[100];
+    int inner_bits = 2;
+    int leaf_bits = 3;
+    double resolution = 1.0;
+
+    const char expected_prefix[] = "Treexy::VoxelGrid<";
+    char prefix[18];
+    input.read(prefix, 18);
+
+    if( std::strncmp(prefix, expected_prefix, 18) != 0 )
+    {
+        throw std::runtime_error("Header wasn't recognized");
+    }
+
+    char part[100];
+
+    int res = sscanf(header, "Treexy::VoxelGrid<%s,%d,%d>(%lf)\n",
+            type, &inner_bits, &leaf_bits, &resolution);
+
+    if( res != 4 )
+    {
+        throw std::runtime_error("Header wasn't recognized");
+    }
+
+    std::string type_name = demangle(typeid(DataT).name());
+    if( type_name == type )
+    {
+        throw std::runtime_error("DataT does not match");
+    }
+    if( inner_bits != IBITS )
+    {
+        throw std::runtime_error("INNER_BITS does not match");
+    }
+    if( leaf_bits != LBITS )
+    {
+        throw std::runtime_error("LEAF_BITS does not match");
+    }
+
+    //------------
+
+    VoxelGrid<DataT, IBITS, LBITS> grid(resolution);
+
+    uint32_t root_count = Read<uint32_t>(input);
+
+    for (size_t root_index = 0; root_index < root_count; root_index++)
+    {
+      CoordT root_coord;
+      root_coord.x = Read<int32_t>(input);
+      root_coord.y = Read<int32_t>(input);
+      root_coord.z = Read<int32_t>(input);
+
+      auto& inner_grid = grid.root_map[root_coord];
+
+      for (size_t w = 0; w < inner_grid.mask.wordCount(); w++)
+      {
+        uint64_t word = Read<uint64_t>(input);
+        inner_grid.mask.setWord(w, word);
+      }
+      for (auto inner = inner_grid.mask.beginOn(); inner; ++inner)
+      {
+        const uint32_t inner_index = *inner;
+        using LeafGridT = typename VoxelGrid<DataT, IBITS, LBITS>::LeafGrid;
+        inner_grid.data[inner_index] = std::make_unique<LeafGridT>();
+        auto& leaf_grid = inner_grid.data[inner_index];
+
+        for (size_t w = 0; w < leaf_grid->mask.wordCount(); w++)
+        {
+          uint64_t word = Read<uint64_t>(input);
+          leaf_grid->mask.setWord(w, word);
+        }
+        for (auto leaf = leaf_grid->mask.beginOn(); leaf; ++leaf)
+        {
+          const uint32_t leaf_index = *leaf;
+          leaf_grid->data[leaf_index] = Read<DataT>(input);
+        }
+      }
+    }
+}
+
+}
+// namespace Treexy
+
+#endif  // SERIALIZATION_HPP

include/treexy/treexy.hpp

@@ -81,6 +81,8 @@ struct VoxelGrid
   /**
    * @brief posToCoord MUST be used to convert real coordinates to CoordT indexes.
    */
+  inline CoordT posToCoord(float x, float y, float z);
+
   inline CoordT posToCoord(double x, double y, double z);
 
   inline CoordT posToCoord(const Point3D& pos)
@@ -196,6 +198,6 @@ struct VoxelGrid
 
 }  // namespace Treexy
 
-#endif  // TREEXY_HPP
-
 #include "treexy_impl.hpp"
+
+#endif  // TREEXY_HPP

include/treexy/treexy_impl.hpp

@@ -1,8 +1,10 @@
-#ifndef TREEXY_IMPL_HPP
-#define TREEXY_IMPL_HPP
+#ifdef TREEXY_USE_SSE
 
-#include "treexy.hpp"
+#include <xmmintrin.h>
+#include <emmintrin.h>
+#include <smmintrin.h>
 
+#endif
 namespace std
 {
 template <>
@@ -18,6 +20,30 @@ struct hash<Treexy::CoordT>
 
 namespace Treexy
 {
+template <typename DataT, int INNER_BITS, int LEAF_BITS>
+inline CoordT VoxelGrid<DataT, INNER_BITS, LEAF_BITS>::posToCoord(float x,
+                                                                  float y,
+                                                                  float z)
+{
+#ifdef TREEXY_USE_SSE
+  union VI
+  {
+    __m128i m;
+    int32_t i[4];
+  };
+  static __m128 RES = _mm_set1_ps(inv_resolution);
+  __m128 vect = _mm_set_ps(x, y, z, 0.0);
+  __m128 res = _mm_mul_ps(vect, RES);
+  VI out;
+  out.m = _mm_cvttps_epi32(_mm_floor_ps(res));
+  return { out.i[3], out.i[2], out.i[1] };
+#else
+  return { static_cast<int32_t>(x * inv_resolution) - std::signbit(x),
+           static_cast<int32_t>(y * inv_resolution) - std::signbit(y),
+           static_cast<int32_t>(z * inv_resolution) - std::signbit(z) };
+#endif
+}
+
 template <typename DataT, int INNER_BITS, int LEAF_BITS>
 inline CoordT VoxelGrid<DataT, INNER_BITS, LEAF_BITS>::posToCoord(double x,
                                                                   double y,
@@ -205,5 +231,3 @@ VoxelGrid<DataT, INNER_BITS, LEAF_BITS>::forEachCell(VisitorFunction func)
 }
 
 }  // namespace Treexy
-
-#endif

test/test.cpp

@@ -0,0 +1,32 @@
+#include "treexy/treexy.hpp"
+#include "treexy/serialization.hpp"
+#include <fstream>
+
+int main()
+{
+  const double VOXEL_RESOLUTION = 0.1;
+
+  Treexy::VoxelGrid<int> grid(VOXEL_RESOLUTION);
+  auto accessor = grid.createAccessor();
+
+  for (double x = 0; x < 0.4; x += VOXEL_RESOLUTION)
+  {
+    for (double y = 0; y < 0.4; y += VOXEL_RESOLUTION)
+    {
+      for (double z = 0; z < 0.4; z += VOXEL_RESOLUTION)
+      {
+        accessor.setValue(grid.posToCoord(x, y, z), 42);
+      }
+    }
+  }
+
+  std::ofstream ofile("box.txy", std::ios::binary);
+  Treexy::Serialize(ofile, grid);
+  ofile.close();
+
+  std::ifstream ifile("box.txy", std::ios::binary);
+  auto new_grid = Treexy::Deserialize<int>(ifile);
+  ifile.close();
+
+  return 0;
+}