From af3a951e4557a071d1e9ab1bb3eefd11f7888188 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Mon, 28 Jun 2021 22:17:54 +0200
Subject: [PATCH] Avoid numerical errors (when deintegrating)

---
 voxblox/CMakeLists.txt                        |  2 +-
 .../projective_tsdf_integrator_inl.h          | 22 ++++++++++---
 voxblox_ros/CMakeLists.txt                    |  2 +-
 voxblox_ros/include/voxblox_ros/ptcloud_vis.h | 32 +++++++++++++++++++
 voxblox_rviz_plugin/CMakeLists.txt            |  2 +-
 5 files changed, 52 insertions(+), 8 deletions(-)

diff --git a/voxblox/CMakeLists.txt b/voxblox/CMakeLists.txt
index 3a0c1ef3e..e7fa923e9 100644
--- a/voxblox/CMakeLists.txt
+++ b/voxblox/CMakeLists.txt
@@ -5,7 +5,7 @@ find_package(catkin_simple REQUIRED )
 catkin_simple(ALL_DEPS_REQUIRED)
 
 set(CMAKE_MACOSX_RPATH 0)
-add_definitions(-std=c++11 -Wall -Wextra)
+add_definitions(-std=c++14 -Wall -Wextra)
 
 ############
 # PROTOBUF #
diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index 828ea68ca..55a99ba20 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -282,11 +282,23 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
     return;
   }
 
-  // Store the updated voxel weight and distance
-  tsdf_voxel->distance = (tsdf_voxel->distance * tsdf_voxel->weight +
-                          std::min(config_.default_truncation_distance, sdf) *
-                              observation_weight) /
-                         new_voxel_weight;
+  // Store the new voxel distance
+  const FloatingPoint new_voxel_distance =
+      (tsdf_voxel->distance * tsdf_voxel->weight +
+       std::min(config_.default_truncation_distance, sdf) *
+           observation_weight) /
+      new_voxel_weight;
+  // NOTE: The integration and deintegration steps are theoretically reversible,
+  //       but non-linearities in the implementation (e.g. weight clamped to
+  //       [0, max_weight], or even dropped distant blocks), and numerical
+  //       errors (e.g. limited precision during the new_voxel_distance
+  //       computation) can make make it grow beyond the truncation distance.
+  //       We therefore explicitly clamp the distance.
+  tsdf_voxel->distance = std::min(
+      std::max(new_voxel_distance, -config_.default_truncation_distance),
+      config_.default_truncation_distance);
+
+  // Store the new weight
   tsdf_voxel->weight = std::min(new_voxel_weight, config_.max_weight);
 }
 
diff --git a/voxblox_ros/CMakeLists.txt b/voxblox_ros/CMakeLists.txt
index 860c92c40..a666d7f2a 100644
--- a/voxblox_ros/CMakeLists.txt
+++ b/voxblox_ros/CMakeLists.txt
@@ -4,7 +4,7 @@ project(voxblox_ros)
 find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
 
-add_definitions(-std=c++11 -Wall -Wextra)
+add_definitions(-std=c++14 -Wall -Wextra)
 
 #############
 # LIBRARIES #
diff --git a/voxblox_ros/include/voxblox_ros/ptcloud_vis.h b/voxblox_ros/include/voxblox_ros/ptcloud_vis.h
index bf1af6516..a8bb5693a 100644
--- a/voxblox_ros/include/voxblox_ros/ptcloud_vis.h
+++ b/voxblox_ros/include/voxblox_ros/ptcloud_vis.h
@@ -240,6 +240,19 @@ inline bool visualizeDistanceIntensityTsdfVoxelsSlice(
   return false;
 }
 
+inline bool visualizeWeightIntensityTsdfVoxelsSlice(
+    const TsdfVoxel& voxel, const Point& coord, unsigned int free_plane_index,
+    FloatingPoint free_plane_val, FloatingPoint voxel_size, double* intensity) {
+  CHECK_NOTNULL(intensity);
+
+  if (std::abs(coord(free_plane_index) - free_plane_val) <=
+      (voxel_size / 2.0 + kFloatEpsilon)) {
+      *intensity = voxel.weight;
+      return true;
+  }
+  return false;
+}
+
 inline bool visualizeDistanceIntensityEsdfVoxels(const EsdfVoxel& voxel,
                                                  const Point& /*coord*/,
                                                  double* intensity) {
@@ -409,6 +422,25 @@ inline void createDistancePointcloudFromTsdfLayerSlice(
       pointcloud);
 }
 
+inline void createWeightPointcloudFromTsdfLayerSlice(
+    const Layer<TsdfVoxel>& layer, unsigned int free_plane_index,
+    FloatingPoint free_plane_val, pcl::PointCloud<pcl::PointXYZI>* pointcloud) {
+  CHECK_NOTNULL(pointcloud);
+  // Make sure that the free_plane_val doesn't fall evenly between 2 slices.
+  // Prefer to push it up.
+  // Using std::remainder rather than std::fmod as the latter has huge crippling
+  // issues with floating-point division.
+  if (std::remainder(free_plane_val, layer.voxel_size()) < kFloatEpsilon) {
+    free_plane_val += layer.voxel_size() / 2.0;
+  }
+
+  createColorPointcloudFromLayer<TsdfVoxel>(
+      layer,
+      std::bind(&visualizeWeightIntensityTsdfVoxelsSlice, ph::_1, ph::_2,
+                free_plane_index, free_plane_val, layer.voxel_size(), ph::_3),
+      pointcloud);
+}
+
 inline void createDistancePointcloudFromEsdfLayerSlice(
     const Layer<EsdfVoxel>& layer, unsigned int free_plane_index,
     FloatingPoint free_plane_val, pcl::PointCloud<pcl::PointXYZI>* pointcloud) {
diff --git a/voxblox_rviz_plugin/CMakeLists.txt b/voxblox_rviz_plugin/CMakeLists.txt
index 4b9eac81b..b543389ec 100644
--- a/voxblox_rviz_plugin/CMakeLists.txt
+++ b/voxblox_rviz_plugin/CMakeLists.txt
@@ -5,7 +5,7 @@ find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
 
 set(CMAKE_MACOSX_RPATH 0)
-add_definitions(-std=c++11 -Wall)
+add_definitions(-std=c++14 -Wall)
 
 ## This setting causes Qt's "MOC" generation to happen automatically.
 set(CMAKE_AUTOMOC ON)