Merge pull request #35 from ethz-asl/fix/failing_dchecks

Address failing DCHECKs for Morton encoding

.github/workflows/ci.yml

@@ -364,14 +364,14 @@ jobs:
       - name: Build regular code
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
-        run: catkin build wavemap_all --no-status --force-color
+        run: catkin build wavemap_all --no-status --force-color --cmake-args -DDCHECK_ALWAYS_ON=ON
 
       - name: Build unit tests
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
         run: |
           echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color --no-deps --catkin-make-args tests
+          catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DDCHECK_ALWAYS_ON=ON --catkin-make-args tests
           echo "::remove-matcher owner=problem-matcher-gcc::"
 
       - name: Run unit tests

libraries/wavemap/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+* Address failing DCHECKs for Morton conversions of negative indices
+* Contributors: Victor Reijgwart
+
 1.5.2 (2023-09-19)
 ------------------
 * Add missing install rules for wavemap

libraries/wavemap/include/wavemap/utils/morton_encoding.h

@@ -8,11 +8,17 @@
 namespace wavemap::morton {
 template <int dim>
 constexpr int kMaxTreeHeight = 8 * sizeof(MortonIndex) / dim;
+
 template <int dim>
 constexpr IndexElement kMaxSingleCoordinate =
     (dim < 3) ? std::numeric_limits<IndexElement>::max()
               : (1ul << kMaxTreeHeight<dim>)-1ul;
 
+template <int dim>
+constexpr MortonIndex kMaxMortonIndex =
+    (dim < 3) ? std::numeric_limits<MortonIndex>::max()
+              : (1ul << (dim * kMaxTreeHeight<dim>)) - 1ul;
+
 template <int dim>
 struct MortonByteLut {
  private:
@@ -41,14 +47,38 @@ struct MortonByteLut {
   static constexpr std::array<uint_fast8_t, 256> decode{generate_decoder()};
 };
 
-template <int dim>
+/**
+ * Method to convert regular n-dimensional indices into Morton indices.
+ * @tparam dim Dimension of the index.
+ * @tparam check_sign Whether to check that each index coefficient is positive.
+ *         Note that negative signs are not preserved when encoding and decoding
+ *         Morton indices. The check can be enabled to throw an error in cases
+ *         where round trip conversions would not yield an identical index.
+ *         Since we often only perform one way conversions and use the Morton
+ *         indices as relative offsets, the check is disabled by default.
+ *         Note that since the check is a DCHECK, it only triggers when the code
+ *         is built with the DCHECK_ALWAYS_ON flag enabled or in debug mode.
+ */
+template <int dim, bool check_sign = false>
 MortonIndex encode(const Index<dim>& index) {
+  // Check if the index coordinates are within the supported range
+  // NOTE: This check is only performed in debug mode, or if DCHECK_ALWAYS_ON is
+  //       set. Otherwise, the loop is empty and optimized out.
+  for (int dim_idx = 0; dim_idx < dim; ++dim_idx) {
+    if constexpr (check_sign) {
+      DCHECK_GE(index[dim_idx], 0);
+    } else {
+      DCHECK_GT(index[dim_idx], -kMaxSingleCoordinate<dim>);
+    }
+    DCHECK_LE(index[dim_idx], kMaxSingleCoordinate<dim>);
+  }
+
+  // Perform the morton encoding, using bitwise expansion if supported by the
+  // target CPU architecture and LUTs otherwise
   uint64_t morton = 0u;
 #ifdef BIT_EXPAND_AVAILABLE
   constexpr auto pattern = bit_manip::repeat_block<uint64_t>(dim, 0b1);
   for (int dim_idx = 0; dim_idx < dim; ++dim_idx) {
-    DCHECK_GE(index[dim_idx], 0);
-    DCHECK_LE(index[dim_idx], kMaxSingleCoordinate<dim>);
     morton |= bit_manip::expand<uint64_t>(index[dim_idx], pattern << dim_idx);
   }
 #else
@@ -67,6 +97,9 @@ MortonIndex encode(const Index<dim>& index) {
   return morton;
 }
 
+// Decode a single coordinate from a Morton index using LUTs
+// NOTE: This method is only used if bitwise compression is not supported by the
+//       target CPU architecture.
 template <int dim>
 IndexElement decode_coordinate(MortonIndex morton, int coordinate_idx) {
   constexpr MortonIndex kByteMask = (1 << 8) - 1;
@@ -86,6 +119,11 @@ IndexElement decode_coordinate(MortonIndex morton, int coordinate_idx) {
 
 template <int dim>
 Index<dim> decode(MortonIndex morton) {
+  // Check if the Morton index is within the supported range
+  DCHECK_LE(morton, kMaxMortonIndex<dim>);
+
+  // Perform the morton decoding, using bitwise compression if supported by the
+  // target CPU architecture and LUTs otherwise
   Index<dim> index;
 #ifdef BIT_COMPRESS_AVAILABLE
   constexpr auto pattern = bit_manip::repeat_block<uint64_t>(dim, 0b1);

libraries/wavemap/package.xml

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

libraries/wavemap_io/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_io
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

libraries/wavemap_io/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_io</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>(De)serialization of wavemap types to files.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

ros/wavemap_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

ros/wavemap_msgs/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_msgs</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>Message definitions for wavemap's ROS interfaces.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

ros/wavemap_ros/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 * Add missing install rules for wavemap

ros/wavemap_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_ros</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>ROS interface for wavemap.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

ros/wavemap_ros_conversions/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_ros_conversions
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

ros/wavemap_ros_conversions/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_ros_conversions</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>Conversions between wavemap and ROS types.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

ros/wavemap_rviz_plugin/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_rviz_plugin
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

ros/wavemap_rviz_plugin/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_rviz_plugin</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>Plugin to interactively visualize maps published in wavemap's
     native format.
   </description>

tooling/packages/catkin_setup/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package catkin_setup
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

tooling/packages/catkin_setup/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>catkin_setup</name>
-    <version>1.5.2</version>
+    <version>1.5.3</version>
     <description>Dummy package to make it easy to setup the workspace and generate the setup.[sh|bash|zsh] scripts in CI.</description>
 
     <maintainer email="[email protected]">Victor Reijgwart</maintainer>

tooling/packages/wavemap_all/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_all
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

tooling/packages/wavemap_all/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_all</name>
-  <version>1.5.2</version>
+  <version>1.5.3</version>
   <description>Metapackage that builds all wavemap packages.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

tooling/packages/wavemap_utils/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package wavemap_utils
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.3 (2023-09-28)
+------------------
+
 1.5.2 (2023-09-19)
 ------------------
 

tooling/packages/wavemap_utils/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>wavemap_utils</name>
-    <version>1.5.2</version>
+    <version>1.5.3</version>
     <description>Small package containing scripts to simplify common wavemap development tasks.</description>
 
     <maintainer email="[email protected]">Victor Reijgwart</maintainer>