Merge pull request #12 from ethz-asl/feature/file_io

Save and load maps from files

.pre-commit-config.yaml

@@ -67,7 +67,7 @@ repos:
                 --library=googletest, --library=tooling/cppcheck/gazebo,
                 "--enable=warning,performance,portability",
                 "--suppress=constStatement",
-                "--suppress=syntaxError:*test/src/*/test_*.cc" ]
+                "--suppress=syntaxError:*test/*/test_*.cc" ]
 
   - repo: https://github.com/cheshirekow/cmake-format-precommit
     rev: v0.6.13

CMakeLists.txt

@@ -4,8 +4,14 @@ project(wavemap)
 # NOTE: This CMakeLists file is not used by ROS and catkin, it only exists to
 #       make it easy to load and edit all packages simultaneously in IDEs.
 
+# Include the catkin workspace's include dir, for msg and proto definitions
+execute_process(COMMAND catkin locate --devel
+    OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE CATKIN_WS_DEVEL_PATH)
+include_directories(${CATKIN_WS_DEVEL_PATH}/include)
+
 # Libraries
 add_subdirectory(libraries/wavemap)
+add_subdirectory(libraries/wavemap_io)
 
 # ROS interfaces and tooling
 add_subdirectory(ros/wavemap_msgs)

libraries/wavemap/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.0 (2023-08-09)
+------------------
+
 1.0.0 (2023-08-08)
 ------------------
 * First public release

libraries/wavemap/include/wavemap/config/type_selector.h

@@ -53,6 +53,10 @@ struct TypeSelector {
                          const DerivedTypeSelectorT& rhs) {
     return lhs.id_ == rhs.id_;
   }
+  friend bool operator!=(const DerivedTypeSelectorT& lhs,
+                         const DerivedTypeSelectorT& rhs) {
+    return !(lhs.id_ == rhs.id_);
+  }
 
   // Method to check if the type ID is currently valid
   bool isValid() const { return isValidTypeId(id_); }

libraries/wavemap/include/wavemap/data_structure/volumetric/wavelet_octree.h

@@ -65,6 +65,7 @@ class WaveletOctree : public VolumetricDataStructureBase {
   Index3D getMaxIndex() const override;
   Index3D getMinPossibleIndex() const;
   Index3D getMaxPossibleIndex() const;
+  IndexElement getTreeHeight() const { return config_.tree_height; }
 
   FloatingPoint getCellValue(const Index3D& index) const override;
   FloatingPoint getCellValue(const OctreeIndex& index) const;

libraries/wavemap/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>Base library for wavemap.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

libraries/wavemap_io/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package wavemap_io
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.1.0 (2023-08-09)
+------------------
+* Implement a small serializer to convert maps to/from byte streams and files
+* Contributors: Victor Reijgwart
+
+1.0.0 (2023-08-08)
+------------------

libraries/wavemap_io/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(wavemap_io)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+# TODO(victorr): Switch to regular catkin
+
+# Compiler definitions and options
+add_wavemap_compile_definitions_and_options()
+
+# Libraries
+# cmake-lint: disable=C0301
+cs_add_library(${PROJECT_NAME}
+    src/file_conversions.cc
+    src/stream_conversions.cc)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Tests
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(
+      test_${PROJECT_NAME}
+      test/src/test_file_conversions.cc)
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} gtest_main)
+endif ()
+
+# Export
+cs_install()
+cs_export()

libraries/wavemap_io/include/CPPLINT.cfg

@@ -0,0 +1 @@
+root=.

libraries/wavemap_io/include/wavemap_io/file_conversions.h

@@ -0,0 +1,17 @@
+#ifndef WAVEMAP_IO_FILE_CONVERSIONS_H_
+#define WAVEMAP_IO_FILE_CONVERSIONS_H_
+
+#include <string>
+
+#include <wavemap/data_structure/volumetric/volumetric_data_structure_base.h>
+
+#include "wavemap_io/stream_conversions.h"
+
+namespace wavemap::io {
+bool mapToFile(const VolumetricDataStructureBase& map,
+               const std::string& file_path);
+bool fileToMap(const std::string& file_path,
+               VolumetricDataStructureBase::Ptr& map);
+}  // namespace wavemap::io
+
+#endif  // WAVEMAP_IO_FILE_CONVERSIONS_H_

libraries/wavemap_io/include/wavemap_io/impl/streamable_types_impl.h

@@ -0,0 +1,129 @@
+#ifndef WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_
+#define WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_
+
+namespace wavemap::io::streamable {
+void Index3D::write(std::ostream& ostream) const {
+  ostream.write(reinterpret_cast<const char*>(&x), sizeof(x));
+  ostream.write(reinterpret_cast<const char*>(&y), sizeof(y));
+  ostream.write(reinterpret_cast<const char*>(&z), sizeof(z));
+}
+
+Index3D Index3D::read(std::istream& istream) {
+  Index3D instance;
+  istream.read(reinterpret_cast<char*>(&instance.x), sizeof(x));
+  istream.read(reinterpret_cast<char*>(&instance.y), sizeof(y));
+  istream.read(reinterpret_cast<char*>(&instance.z), sizeof(z));
+  return instance;
+}
+
+void WaveletOctreeNode::write(std::ostream& ostream) const {
+  for (Float coefficient : detail_coefficients) {
+    ostream.write(reinterpret_cast<const char*>(&coefficient),
+                  sizeof(coefficient));
+  }
+  ostream.write(reinterpret_cast<const char*>(&allocated_children_bitset),
+                sizeof(allocated_children_bitset));
+}
+
+WaveletOctreeNode WaveletOctreeNode::read(std::istream& istream) {
+  WaveletOctreeNode instance;
+  for (Float& coefficient : instance.detail_coefficients) {
+    istream.read(reinterpret_cast<char*>(&coefficient), sizeof(coefficient));
+  }
+  istream.read(reinterpret_cast<char*>(&instance.allocated_children_bitset),
+               sizeof(allocated_children_bitset));
+  return instance;
+}
+
+void WaveletOctreeHeader::write(std::ostream& ostream) const {
+  ostream.write(reinterpret_cast<const char*>(&min_cell_width),
+                sizeof(min_cell_width));
+  ostream.write(reinterpret_cast<const char*>(&min_log_odds),
+                sizeof(min_log_odds));
+  ostream.write(reinterpret_cast<const char*>(&max_log_odds),
+                sizeof(max_log_odds));
+  ostream.write(reinterpret_cast<const char*>(&tree_height),
+                sizeof(tree_height));
+  ostream.write(reinterpret_cast<const char*>(&root_node_scale_coefficient),
+                sizeof(root_node_scale_coefficient));
+}
+
+WaveletOctreeHeader WaveletOctreeHeader::read(std::istream& istream) {
+  WaveletOctreeHeader instance;
+  istream.read(reinterpret_cast<char*>(&instance.min_cell_width),
+               sizeof(min_cell_width));
+  istream.read(reinterpret_cast<char*>(&instance.min_log_odds),
+               sizeof(min_log_odds));
+  istream.read(reinterpret_cast<char*>(&instance.max_log_odds),
+               sizeof(max_log_odds));
+  istream.read(reinterpret_cast<char*>(&instance.tree_height),
+               sizeof(tree_height));
+  istream.read(reinterpret_cast<char*>(&instance.root_node_scale_coefficient),
+               sizeof(root_node_scale_coefficient));
+  return instance;
+}
+
+void HashedWaveletOctreeBlockHeader::write(std::ostream& ostream) const {
+  ostream.write(reinterpret_cast<const char*>(&root_node_offset),
+                sizeof(root_node_offset));
+  ostream.write(reinterpret_cast<const char*>(&root_node_scale_coefficient),
+                sizeof(root_node_scale_coefficient));
+}
+
+HashedWaveletOctreeBlockHeader HashedWaveletOctreeBlockHeader::read(
+    std::istream& istream) {
+  HashedWaveletOctreeBlockHeader instance;
+  istream.read(reinterpret_cast<char*>(&instance.root_node_offset),
+               sizeof(root_node_offset));
+  istream.read(reinterpret_cast<char*>(&instance.root_node_scale_coefficient),
+               sizeof(root_node_scale_coefficient));
+  return instance;
+}
+
+void HashedWaveletOctreeHeader::write(std::ostream& ostream) const {
+  ostream.write(reinterpret_cast<const char*>(&min_cell_width),
+                sizeof(min_cell_width));
+  ostream.write(reinterpret_cast<const char*>(&min_log_odds),
+                sizeof(min_log_odds));
+  ostream.write(reinterpret_cast<const char*>(&max_log_odds),
+                sizeof(max_log_odds));
+  ostream.write(reinterpret_cast<const char*>(&tree_height),
+                sizeof(tree_height));
+  ostream.write(reinterpret_cast<const char*>(&num_blocks), sizeof(num_blocks));
+}
+
+HashedWaveletOctreeHeader HashedWaveletOctreeHeader::read(
+    std::istream& istream) {
+  HashedWaveletOctreeHeader instance;
+  istream.read(reinterpret_cast<char*>(&instance.min_cell_width),
+               sizeof(min_cell_width));
+  istream.read(reinterpret_cast<char*>(&instance.min_log_odds),
+               sizeof(min_log_odds));
+  istream.read(reinterpret_cast<char*>(&instance.max_log_odds),
+               sizeof(max_log_odds));
+  istream.read(reinterpret_cast<char*>(&instance.tree_height),
+               sizeof(tree_height));
+  istream.read(reinterpret_cast<char*>(&instance.num_blocks),
+               sizeof(num_blocks));
+  return instance;
+}
+
+void StorageFormat::write(std::ostream& ostream) const {
+  ostream.write(reinterpret_cast<const char*>(&id_), sizeof(id_));
+}
+
+StorageFormat StorageFormat::read(std::istream& istream) {
+  StorageFormat instance;
+  istream.read(reinterpret_cast<char*>(&instance.id_), sizeof(id_));
+  return instance;
+}
+
+StorageFormat StorageFormat::peek(std::istream& istream) {
+  const auto pos = istream.tellg();
+  auto instance = read(istream);
+  istream.seekg(pos);
+  return instance;
+}
+}  // namespace wavemap::io::streamable
+
+#endif  // WAVEMAP_IO_IMPL_STREAMABLE_TYPES_IMPL_H_

libraries/wavemap_io/include/wavemap_io/stream_conversions.h

@@ -0,0 +1,28 @@
+#ifndef WAVEMAP_IO_STREAM_CONVERSIONS_H_
+#define WAVEMAP_IO_STREAM_CONVERSIONS_H_
+
+#include <istream>
+#include <ostream>
+
+#include <wavemap/common.h>
+#include <wavemap/data_structure/volumetric/cell_types/haar_coefficients.h>
+#include <wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree.h>
+#include <wavemap/data_structure/volumetric/hashed_wavelet_octree.h>
+#include <wavemap/data_structure/volumetric/wavelet_octree.h>
+
+#include "wavemap_io/streamable_types.h"
+
+namespace wavemap::io {
+bool mapToStream(const VolumetricDataStructureBase& map, std::ostream& ostream);
+bool streamToMap(std::istream& istream, VolumetricDataStructureBase::Ptr& map);
+
+void mapToStream(const WaveletOctree& map, std::ostream& ostream);
+bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map);
+
+void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream);
+bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map);
+
+void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream);
+}  // namespace wavemap::io
+
+#endif  // WAVEMAP_IO_STREAM_CONVERSIONS_H_

libraries/wavemap_io/include/wavemap_io/streamable_types.h

@@ -0,0 +1,87 @@
+#ifndef WAVEMAP_IO_STREAMABLE_TYPES_H_
+#define WAVEMAP_IO_STREAMABLE_TYPES_H_
+
+#include <istream>
+#include <ostream>
+
+namespace wavemap::io::streamable {
+// NOTE: This file defines the serialization format for all types that might be
+//       used in wavemap map files. The idea is that these are used as common
+//       building blocks (e.g. 3D indices are always (de)serialized using
+//       serializable::Index3D) and modified as little as possible to avoid
+//       breaking backward compatibility with previously saved maps.
+
+using UInt8 = uint8_t;
+using UInt64 = uint64_t;
+using Int32 = int32_t;
+using Float = float;
+
+struct Index3D {
+  Int32 x{};
+  Int32 y{};
+  Int32 z{};
+
+  inline void write(std::ostream& ostream) const;
+  inline static Index3D read(std::istream& istream);
+};
+
+struct WaveletOctreeNode {
+  std::array<Float, 7> detail_coefficients{};
+  UInt8 allocated_children_bitset{};
+
+  inline void write(std::ostream& ostream) const;
+  inline static WaveletOctreeNode read(std::istream& istream);
+};
+
+struct WaveletOctreeHeader {
+  Float min_cell_width{};
+  Float min_log_odds{};
+  Float max_log_odds{};
+
+  Int32 tree_height{};
+  Float root_node_scale_coefficient{};
+
+  inline void write(std::ostream& ostream) const;
+  inline static WaveletOctreeHeader read(std::istream& istream);
+};
+
+struct HashedWaveletOctreeBlockHeader {
+  Index3D root_node_offset{};
+  Float root_node_scale_coefficient{};
+
+  inline void write(std::ostream& ostream) const;
+  inline static HashedWaveletOctreeBlockHeader read(std::istream& istream);
+};
+
+struct HashedWaveletOctreeHeader {
+  Float min_cell_width{};
+  Float min_log_odds{};
+  Float max_log_odds{};
+
+  Int32 tree_height{};
+  UInt64 num_blocks{};
+
+  inline void write(std::ostream& ostream) const;
+  inline static HashedWaveletOctreeHeader read(std::istream& istream);
+};
+
+struct StorageFormat : TypeSelector<StorageFormat> {
+  using TypeSelector<StorageFormat>::TypeSelector;
+
+  enum Id : TypeId {
+    kWaveletOctree,
+    kHashedWaveletOctree,
+  };
+
+  static constexpr std::array names = {"wavelet_octree",
+                                       "hashed_wavelet_octree"};
+
+  inline void write(std::ostream& ostream) const;
+  inline static StorageFormat read(std::istream& istream);
+  inline static StorageFormat peek(std::istream& istream);
+};
+}  // namespace wavemap::io::streamable
+
+#include "wavemap_io/impl/streamable_types_impl.h"
+
+#endif  // WAVEMAP_IO_STREAMABLE_TYPES_H_

libraries/wavemap_io/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>wavemap_io</name>
+  <version>1.1.0</version>
+  <description>(De)serialization of wavemap types to files.</description>
+
+  <maintainer email="[email protected]">Victor Reijgwart</maintainer>
+  <license>BSD</license>
+  <url>https://github.com/victorreijgwart/wavemap</url>
+
+  <author email="[email protected]">Victor Reijgwart</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>wavemap</depend>
+
+  <test_depend>gtest</test_depend>
+</package>