Avoid numerical errors (when deintegrating)

ethz-asl · Jun 29, 2021 · af3a951 · af3a951
1 parent f037e07
commit af3a951
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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)