From f455bd9d841aa1d26fd125a9f97b47d978c8f869 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Tue, 12 May 2020 19:28:14 +0200
Subject: [PATCH 1/8] Projective integrator draft

---
 .../integrator/projective_tsdf_integrator.h   |  66 +++
 .../projective_tsdf_integrator_inl.h          | 382 ++++++++++++++++++
 voxblox_ros/include/voxblox_ros/ros_params.h  |   5 +
 voxblox_ros/launch/arche_dataset.launch       |  59 +++
 voxblox_ros/src/tsdf_server.cc                |   5 +
 5 files changed, 517 insertions(+)
 create mode 100644 voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
 create mode 100644 voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
 create mode 100644 voxblox_ros/launch/arche_dataset.launch

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
new file mode 100644
index 000000000..1b71fbbc7
--- /dev/null
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
@@ -0,0 +1,66 @@
+#ifndef VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_H_
+#define VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_H_
+
+#include <vector>
+
+#include "voxblox/integrator/tsdf_integrator.h"
+
+namespace voxblox {
+enum class InterpolationScheme {
+  kNearestNeighbor,
+  kMinNeighbor,
+  kBilinear,
+  kAdaptive
+};
+template <InterpolationScheme interpolation_scheme>
+class ProjectiveTsdfIntegrator : public voxblox::TsdfIntegratorBase {
+ public:
+  ProjectiveTsdfIntegrator(const Config& config,
+                           voxblox::Layer<voxblox::TsdfVoxel>* layer);
+
+  void integratePointCloud(const Transformation& T_G_C,
+                           const Pointcloud& points_C, const Colors& colors,
+                           const bool freespace_points = false) override;
+  void integratePointCloud(const Transformation& T_G_C,
+                           const Pointcloud& points_C, const Colors& colors,
+                           const bool freespace_points = false,
+                           const bool deintegrate = false);
+
+ private:
+  static constexpr int kWidth = 1024;
+  static constexpr int kHeight = 64;
+  static constexpr double altitude_angle_max = 16.611;
+  static constexpr double azimuth_angle_offset = 3.164;
+
+  Eigen::MatrixXf range_image_;
+
+  // Calculate the number of voxels per block
+  const size_t num_voxels_per_block_;
+
+  void parsePointcloud(const Transformation& T_G_C, const Pointcloud& points_C,
+                       Eigen::MatrixXf* range_image,
+                       voxblox::IndexSet* touched_block_indices);
+
+  void updateTsdfBlocks(const Transformation& T_G_C,
+                        const Eigen::MatrixXf& range_image,
+                        const voxblox::IndexSet& touched_block_indices,
+                        const bool deintegrate = false);
+  inline void updateTsdfVoxel(const Transformation& T_G_C,
+                              const Eigen::MatrixXf& range_image,
+                              const Point& t_C_voxel, TsdfVoxel* tsdf_voxel,
+                              const bool deintegrate = false);
+
+  template <typename T>
+  Point imageToBearing(const T h, const T w);
+
+  template <typename T>
+  bool bearingToImage(const Point& b_C_normalized, T* h, T* w);
+
+  inline float interpolate(const Eigen::MatrixXf& range_image, const float h,
+                           const float w);
+};
+}  // namespace voxblox
+
+#include "voxblox/integrator/projective_tsdf_integrator_inl.h"
+
+#endif  // VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_H_
diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
new file mode 100644
index 000000000..bbfe7d783
--- /dev/null
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -0,0 +1,382 @@
+#ifndef VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
+#define VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
+
+#include <algorithm>
+#include <iostream>
+#include <list>
+#include <vector>
+
+#include "voxblox/core/block.h"
+
+namespace voxblox {
+template <InterpolationScheme interpolation_scheme>
+ProjectiveTsdfIntegrator<interpolation_scheme>::ProjectiveTsdfIntegrator(
+    const voxblox::TsdfIntegratorBase::Config &config,
+    voxblox::Layer<voxblox::TsdfVoxel> *layer)
+    : TsdfIntegratorBase(config, layer),
+      num_voxels_per_block_(layer_->voxels_per_side() *
+                            layer_->voxels_per_side() *
+                            layer_->voxels_per_side()),
+      range_image_(kHeight, kWidth) {}
+
+template <InterpolationScheme interpolation_scheme>
+void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
+    const Transformation &T_G_C, const Pointcloud &points_C,
+    const Colors &colors, const bool freespace_points) {
+  integratePointCloud(T_G_C, points_C, colors, freespace_points, false);
+}
+
+template <InterpolationScheme interpolation_scheme>
+void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
+    const Transformation &T_G_C, const Pointcloud &points_C,
+    const Colors &colors, const bool freespace_points, const bool deintegrate) {
+  // Construct the range image and select the blocks it affects
+  voxblox::IndexSet touched_block_indices;
+  timing::Timer range_image_and_block_selector_timer(
+      "range_image_and_block_selector_timer");
+  parsePointcloud(T_G_C, points_C, &range_image_, &touched_block_indices);
+  range_image_and_block_selector_timer.Stop();
+
+  // Process all blocks
+  timing::Timer integration_timer("integration_timer");
+  if (config_.integrator_threads == 1) {
+    updateTsdfBlocks(T_G_C, range_image_, touched_block_indices, deintegrate);
+  } else {
+    std::vector<voxblox::IndexSet> block_index_subsets(
+        config_.integrator_threads);
+    size_t idx = 0;
+    for (const auto &block_index : touched_block_indices) {
+      block_index_subsets[idx % config_.integrator_threads].emplace(
+          block_index);
+      idx++;
+    }
+    std::list<std::thread> integration_threads;
+    for (size_t i = 0; i < config_.integrator_threads; ++i) {
+      integration_threads.emplace_back(
+          &ProjectiveTsdfIntegrator::updateTsdfBlocks, this, T_G_C,
+          range_image_, block_index_subsets[i], deintegrate);
+    }
+    for (std::thread &integration_thread : integration_threads) {
+      integration_thread.join();
+    }
+  }
+  integration_timer.Stop();
+}
+
+template <InterpolationScheme interpolation_scheme>
+void ProjectiveTsdfIntegrator<interpolation_scheme>::parsePointcloud(
+    const Transformation &T_G_C, const Pointcloud &points_C,
+    Eigen::MatrixXf *range_image, voxblox::IndexSet *touched_block_indices) {
+  CHECK_NOTNULL(range_image);
+  CHECK_NOTNULL(touched_block_indices);
+  range_image->setZero();
+  voxblox::Point t_G_C_scaled = T_G_C.getPosition() * layer_->block_size_inv();
+  for (const voxblox::Point &point_C : points_C) {
+    // Compute the point's bearing vector
+    float distance = point_C.norm();
+    if (std::abs(distance) < kFloatEpsilon) {
+      // Avoid divisions by zero
+      continue;
+    }
+    Point point_C_bearing = point_C / distance;
+
+    // Get the bearing vector's coordinates in the range image
+    // NOTE: In this first implementation we only update the range image at its
+    //       nearest neighbor coordinate
+    int h, w;
+    if (!bearingToImage(point_C_bearing, &h, &w)) {
+      continue;
+    }
+
+    // Set the range image distance
+    if (range_image->operator()(h, w) < kFloatEpsilon) {
+      range_image->operator()(h, w) = distance;
+    } else {
+      // If the range pixel has already been updated by another ray,
+      // we resolve the conflict by taking the minimum distance
+      range_image->operator()(h, w) =
+          std::min(range_image->operator()(h, w), distance);
+    }
+
+    // Mark the blocks hit by this ray
+    if (config_.min_ray_length_m <= distance &&
+        distance <= config_.max_ray_length_m) {
+      Point point_G_scaled =
+          T_G_C *
+          (point_C_bearing * (distance + config_.default_truncation_distance)) *
+          layer_->block_size_inv();
+      voxblox::GlobalIndex block_index;
+      voxblox::RayCaster ray_caster(point_G_scaled, t_G_C_scaled);
+      while (ray_caster.nextRayIndex(&block_index)) {
+        touched_block_indices->insert(block_index.cast<IndexElement>());
+      }
+    }
+  }
+
+  // Allocate all the blocks
+  // NOTE: The layer's BlockHashMap is not thread safe, so doing it here before
+  //       we start the multi-threading makes life easier
+  for (const BlockIndex &block_index : *touched_block_indices) {
+    layer_->allocateBlockPtrByIndex(block_index);
+  }
+}
+
+template <InterpolationScheme interpolation_scheme>
+void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfBlocks(
+    const Transformation &T_G_C, const Eigen::MatrixXf &range_image,
+    const voxblox::IndexSet &touched_block_indices, const bool deintegrate) {
+  for (const voxblox::BlockIndex &block_index : touched_block_indices) {
+    voxblox::Block<TsdfVoxel>::Ptr block_ptr =
+        layer_->getBlockPtrByIndex(block_index);
+    block_ptr->updated().set();
+
+    for (size_t linear_index = 0u; linear_index < num_voxels_per_block_;
+         ++linear_index) {
+      TsdfVoxel *tsdf_voxel = &block_ptr->getVoxelByLinearIndex(linear_index);
+      const Point t_C_voxel =
+          T_G_C.inverse() *
+          block_ptr->computeCoordinatesFromLinearIndex(linear_index);
+      updateTsdfVoxel(T_G_C, range_image, t_C_voxel, tsdf_voxel, deintegrate);
+    }
+  }
+}
+
+template <InterpolationScheme interpolation_scheme>
+void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
+    const Transformation &T_G_C, const Eigen::MatrixXf &range_image,
+    const Point &t_C_voxel, TsdfVoxel *tsdf_voxel, const bool deintegrate) {
+  const float distance_to_voxel = t_C_voxel.norm();
+  if (distance_to_voxel < config_.min_ray_length_m ||
+      distance_to_voxel > config_.max_ray_length_m) {
+    return;
+  }
+
+  float h, w;
+  if (!bearingToImage(t_C_voxel / distance_to_voxel, &h, &w)) {
+    return;
+  }
+
+  float observation_weight;
+  if (deintegrate) {
+    observation_weight = -1.0f;
+  } else {
+    observation_weight = 1.0f;
+  }
+  const float new_voxel_weight = tsdf_voxel->weight + observation_weight;
+
+  if (new_voxel_weight < kFloatEpsilon) {
+    tsdf_voxel->distance = 0.0f;
+    tsdf_voxel->weight = 0.0f;
+    return;
+  }
+
+  const float distance_to_surface = interpolate(range_image, h, w);
+  const float sdf = distance_to_surface - distance_to_voxel;
+  if (sdf < -config_.default_truncation_distance) {
+    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;
+  tsdf_voxel->weight = new_voxel_weight;
+}
+
+template <InterpolationScheme interpolation_scheme>
+template <typename T>
+Point ProjectiveTsdfIntegrator<interpolation_scheme>::imageToBearing(
+    const T h, const T w) {
+  double altitude_angle =
+      M_PI / 180.0 *
+      (altitude_angle_max - h / 63.0 * (2.0 * altitude_angle_max));
+  double azimuth_angle =
+      2.0 * M_PI *
+      (static_cast<double>(w) / kWidth + azimuth_angle_offset / 360.0);
+
+  Point bearing;
+  bearing.x() = std::cos(altitude_angle) * std::cos(azimuth_angle);
+  bearing.y() = -std::cos(altitude_angle) * std::sin(azimuth_angle);
+  bearing.z() = std::sin(altitude_angle);
+
+  return bearing;
+}
+
+template <InterpolationScheme interpolation_scheme>
+template <typename T>
+bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
+    const Point &b_C_normalized, T *h, T *w) {
+  CHECK_NOTNULL(h);
+  CHECK_NOTNULL(w);
+
+  double altitude_angle = std::asin(b_C_normalized.z());
+  *h = static_cast<T>((altitude_angle_max - 180.0 / M_PI * altitude_angle) *
+                      63.0 / (2.0 * altitude_angle_max));
+  if (*h < 0 || kHeight - 1 < *h) {
+    return false;
+  }
+
+  double azimuth_angle;
+  if (b_C_normalized.x() > 0) {
+    if (b_C_normalized.y() > 0) {
+      azimuth_angle =
+          2.0 * M_PI + std::atan(-b_C_normalized.y() / b_C_normalized.x());
+    } else {
+      azimuth_angle = std::atan(-b_C_normalized.y() / b_C_normalized.x());
+    }
+  } else {
+    azimuth_angle = M_PI + std::atan(-b_C_normalized.y() / b_C_normalized.x());
+  }
+  *w = static_cast<T>(
+      kWidth * (azimuth_angle / (2.0 * M_PI) - azimuth_angle_offset / 360.0));
+  if (*w < 0) {
+    *w += kWidth;
+  }
+
+  return (0 < *w && *w < kWidth - 1);
+}
+
+template <>
+inline float
+ProjectiveTsdfIntegrator<InterpolationScheme::kNearestNeighbor>::interpolate(
+    const Eigen::MatrixXf &range_image, const float h, const float w) {
+  return range_image(std::round(h), std::round(w));
+}
+
+template <>
+inline float
+ProjectiveTsdfIntegrator<InterpolationScheme::kMinNeighbor>::interpolate(
+    const Eigen::MatrixXf &range_image, const float h, const float w) {
+  // TODO(victorr): Implement better edge handling
+  if (h >= kHeight || w >= kWidth) {
+    return range_image(std::floor(h), std::floor(w));
+  }
+
+  const float min_neighbor =
+      range_image.block<2, 2>(std::floor(h), std::floor(w)).minCoeff();
+
+  if (min_neighbor < config_.min_ray_length_m) {
+    return range_image(std::round(h), std::round(w));
+  } else {
+    return min_neighbor;
+  }
+}
+
+template <>
+inline float
+ProjectiveTsdfIntegrator<InterpolationScheme::kBilinear>::interpolate(
+    const Eigen::MatrixXf &range_image, const float h, const float w) {
+  // TODO(victorr): Implement better edge handling
+  // Check if we're on the edge, to avoid out-of-bounds matrix access
+  if (h >= kHeight || w >= kWidth) {
+    return range_image(std::floor(h), std::floor(w));
+  }
+
+  // Check if all of the neighbors are valid values
+  // NOTE: This for example removes LiDAR points that had no return,
+  //       which are encoded as zeros
+  if ((range_image.block<2, 2>(std::floor(h), std::floor(w)).array() <
+       config_.min_ray_length_m)
+          .any()) {
+    return range_image(std::round(h), std::round(w));
+  }
+
+  // TODO(victorr): Write this as a matrix
+  float h_int, w_int;
+  const float h_decimals = std::modf(h, &h_int);
+  const float w_decimals = std::modf(w, &w_int);
+  const float left = (1 - h_decimals) * range_image(h_int, w_int) +
+                     h_decimals * range_image(h_int + 1, w_int);
+  const float right = (1 - h_decimals) * range_image(h_int, w_int + 1) +
+                      h_decimals * range_image(h_int + 1, w_int + 1);
+  return (1 - w_decimals) * left + w_decimals * right;
+}
+
+template <>
+inline float
+ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>::interpolate(
+    const Eigen::MatrixXf &range_image, const float h, const float w) {
+  // Check if we're on the edge, to avoid out-of-bounds matrix access
+  if (h >= kHeight || w >= kWidth) {
+    return range_image(std::floor(h), std::floor(w));
+  }
+
+  // Get the bilinear interpolation coefficients
+  float h_int, w_int;
+  const float h_decimals = std::modf(h, &h_int);
+  const float w_decimals = std::modf(w, &w_int);
+
+  // Define easy to read names for all four range image pixels
+  const float top_left = range_image(h_int, w_int);
+  const float top_right = range_image(h_int, w_int + 1);
+  const float bottom_left = range_image(h_int + 1, w_int);
+  const float bottom_right = range_image(h_int + 1, w_int + 1);
+
+  // Left column
+  float bilinear_left, min_left;
+  if (top_left < config_.min_ray_length_m) {
+    min_left = bottom_left;
+    bilinear_left = bottom_left;
+  } else if (bottom_left < config_.min_ray_length_m) {
+    min_left = top_left;
+    bilinear_left = top_left;
+  } else {
+    min_left = std::min(top_left, bottom_left);
+    bilinear_left = (1 - h_decimals) * top_left + h_decimals * bottom_left;
+  }
+
+  // Right column
+  float bilinear_right, min_right;
+  if (top_right < config_.min_ray_length_m) {
+    min_right = bottom_right;
+    bilinear_right = bottom_right;
+  } else if (bottom_right < config_.min_ray_length_m) {
+    min_right = top_right;
+    bilinear_right = top_right;
+  } else {
+    min_right = std::min(bottom_right, top_right);
+    bilinear_right = (1 - h_decimals) * top_right + h_decimals * bottom_right;
+  }
+
+  // Combine left and right bilinear values
+  float bilinear_distance;
+  if (bilinear_left < config_.min_ray_length_m) {
+    if (bilinear_right < config_.min_ray_length_m) {
+      bilinear_distance = 0.0f;
+    } else {
+      bilinear_distance = bilinear_right;
+    }
+  } else if (bilinear_right < config_.min_ray_length_m) {
+    bilinear_distance = bilinear_left;
+  } else {
+    bilinear_distance =
+        (1 - w_decimals) * bilinear_left + w_decimals * bilinear_right;
+  }
+
+  // Combine left and right min values
+  float min_distance;
+  if (min_left < config_.min_ray_length_m) {
+    if (min_right < config_.min_ray_length_m) {
+      min_distance = 0.0f;
+    } else {
+      min_distance = min_right;
+    }
+  } else if (min_right < config_.min_ray_length_m) {
+    min_distance = min_left;
+  } else {
+    min_distance = std::min(min_left, min_right);
+  }
+
+  // Use the bilinear distance unless the min distance is much smaller,
+  // in which case we go for the safe option
+  if (min_distance + 0.5f * config_.default_truncation_distance <
+      bilinear_distance) {
+    return min_distance;
+  } else {
+    return bilinear_distance;
+  }
+}
+}  // namespace voxblox
+
+#endif  // VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
diff --git a/voxblox_ros/include/voxblox_ros/ros_params.h b/voxblox_ros/include/voxblox_ros/ros_params.h
index 856a8aa61..8a2e2b9bd 100644
--- a/voxblox_ros/include/voxblox_ros/ros_params.h
+++ b/voxblox_ros/include/voxblox_ros/ros_params.h
@@ -68,6 +68,8 @@ inline TsdfIntegratorBase::Config getTsdfIntegratorConfigFromRosParam(
 
   double truncation_distance = integrator_config.default_truncation_distance;
   double max_weight = integrator_config.max_weight;
+  int integrator_threads =
+      static_cast<int>(integrator_config.integrator_threads);
   nh_private.param("voxel_carving_enabled",
                    integrator_config.voxel_carving_enabled,
                    integrator_config.voxel_carving_enabled);
@@ -107,10 +109,13 @@ inline TsdfIntegratorBase::Config getTsdfIntegratorConfigFromRosParam(
   nh_private.param("integration_order_mode",
                    integrator_config.integration_order_mode,
                    integrator_config.integration_order_mode);
+  nh_private.param("integrator_threads", integrator_threads,
+                   integrator_threads);
 
   integrator_config.default_truncation_distance =
       static_cast<float>(truncation_distance);
   integrator_config.max_weight = static_cast<float>(max_weight);
+  integrator_config.integrator_threads = integrator_threads;
 
   return integrator_config;
 }
diff --git a/voxblox_ros/launch/arche_dataset.launch b/voxblox_ros/launch/arche_dataset.launch
new file mode 100644
index 000000000..d87e29e2a
--- /dev/null
+++ b/voxblox_ros/launch/arche_dataset.launch
@@ -0,0 +1,59 @@
+<launch>
+    <param name="use_sim_time" value="true"/>
+
+    <node if="1" name="player" pkg="rosbag" type="play" output="screen"
+          args="-r 1.0 -s 5 -q --clock /home/victor/data/voxgraph_release/arche_flight1_2ms_indoor-outdoor-figure-8.bag"/>
+
+    <node pkg="tf" type="static_transform_publisher" name="odom_to_mission" args="0 0 0 0 0 0 1 odom mission 100"/>
+    <node pkg="tf" type="static_transform_publisher" name="os1_stuff" args="0 0 0 0 0 1 0 os1_sensor os1_lidar 100"/>
+
+    <node name="voxblox_projective" pkg="voxblox_ros" type="tsdf_server"
+          output="screen" args="-alsologtostderr" clear_params="true">
+        <remap from="pointcloud" to="lidar_undistortion/pointcloud_corrected"/>
+        <rosparam>
+            verbose: false
+            method: "projective"
+            color_mode: "normals"
+            world_frame: "mission"
+            tsdf_voxel_size: 0.20
+            truncation_distance: 0.60
+            pointcloud_queue_size: 1
+            max_ray_length_m: 12.0 # 14 or 16m would be better, but for that we need better odometry. Otherwise the surface get too damaged.
+            min_ray_length_m: 1.0
+            max_consecutive_ray_collisions: 1
+            use_const_weight: true
+<!--            slice_level: 1.0-->
+<!--            publish_pointclouds_on_update: true-->
+<!--            publish_slices: true-->
+            update_mesh_every_n_sec: 1.0
+            integrator_threads: 1
+        </rosparam>
+    </node>
+
+    <node name="voxblox_fast" pkg="voxblox_ros" type="tsdf_server"
+          output="screen" args="-alsologtostderr" clear_params="true">
+        <remap from="pointcloud" to="lidar_undistortion/pointcloud_corrected"/>
+        <rosparam>
+            verbose: false
+            method: "fast"
+            color_mode: "normals"
+            world_frame: "mission"
+            tsdf_voxel_size: 0.15
+            truncation_distance: 0.60
+            pointcloud_queue_size: 1
+            max_ray_length_m: 12.0 # 14 or 16m would be better, but for that we need better odometry. Otherwise the surface get too damaged.
+            min_ray_length_m: 1.0
+            max_consecutive_ray_collisions: 1
+            use_const_weight: true
+            <!--            slice_level: 1.0-->
+            <!--            publish_pointclouds_on_update: true-->
+            <!--            publish_slices: true-->
+            update_mesh_every_n_sec: 1.0
+<!--            integrator_threads: 1-->
+        </rosparam>
+    </node>
+
+    <include file="$(find lidar_undistortion)/launch/os1_undistortion.launch"/>
+
+    <node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen"/>
+</launch>
diff --git a/voxblox_ros/src/tsdf_server.cc b/voxblox_ros/src/tsdf_server.cc
index 5d566e35d..1c6f675eb 100644
--- a/voxblox_ros/src/tsdf_server.cc
+++ b/voxblox_ros/src/tsdf_server.cc
@@ -2,6 +2,7 @@
 
 #include <minkindr_conversions/kindr_msg.h>
 #include <minkindr_conversions/kindr_tf.h>
+#include <voxblox/integrator/projective_tsdf_integrator.h>
 
 #include "voxblox_ros/conversions.h"
 #include "voxblox_ros/ros_params.h"
@@ -100,6 +101,10 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,
   } else if (method.compare("fast") == 0) {
     tsdf_integrator_.reset(new FastTsdfIntegrator(
         integrator_config, tsdf_map_->getTsdfLayerPtr()));
+  } else if (method.compare("projective") == 0) {
+    tsdf_integrator_.reset(
+        new ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>(
+            integrator_config, tsdf_map_->getTsdfLayerPtr()));
   } else {
     tsdf_integrator_.reset(new SimpleTsdfIntegrator(
         integrator_config, tsdf_map_->getTsdfLayerPtr()));

From 88addb81003fd8973378259ab01a795b67b20850 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Tue, 12 May 2020 20:03:25 +0200
Subject: [PATCH 2/8] Support all TSDF integrator params in projective
 integrator

---
 .../projective_tsdf_integrator_inl.h          | 67 +++++++++++++++----
 1 file changed, 55 insertions(+), 12 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index bbfe7d783..03979a02c 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -145,37 +145,80 @@ template <InterpolationScheme interpolation_scheme>
 void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
     const Transformation &T_G_C, const Eigen::MatrixXf &range_image,
     const Point &t_C_voxel, TsdfVoxel *tsdf_voxel, const bool deintegrate) {
+  // Skip voxels that are too far or too close
   const float distance_to_voxel = t_C_voxel.norm();
   if (distance_to_voxel < config_.min_ray_length_m ||
       distance_to_voxel > config_.max_ray_length_m) {
     return;
   }
 
+  // Project the current voxel into the range image
   float h, w;
   if (!bearingToImage(t_C_voxel / distance_to_voxel, &h, &w)) {
     return;
   }
 
+  // Compute the signed distance
+  const float distance_to_surface = interpolate(range_image, h, w);
+  const float sdf = distance_to_surface - distance_to_voxel;
+
+  // Skip voxels that fall outside the TSDF truncation distance
+  if (sdf < -config_.default_truncation_distance) {
+    return;
+  }
+  if (!config_.voxel_carving_enabled &&
+      sdf > config_.default_truncation_distance) {
+    return;
+  }
+
+  // Compute the weight of the measurement
   float observation_weight;
-  if (deintegrate) {
-    observation_weight = -1.0f;
-  } else {
-    observation_weight = 1.0f;
+  {
+    // Set the sign of the measurement weight
+    if (deintegrate) {
+      observation_weight = -1.0f;
+    } else {
+      observation_weight = 1.0f;
+    }
+
+    // Scale the weight by the inverse square depth if appropriate
+    if (!config_.use_const_weight) {
+      const FloatingPoint dist_z = std::abs(t_C_voxel.z());
+      if (dist_z > kEpsilon) {
+        observation_weight /= (dist_z * dist_z);
+      } else {
+        return;
+      }
+    }
+
+    // Apply weight drop-off if appropriate
+    const FloatingPoint dropoff_epsilon = voxel_size_;
+    if (config_.use_weight_dropoff && sdf < -dropoff_epsilon) {
+      observation_weight =
+          observation_weight * (config_.default_truncation_distance + sdf) /
+          (config_.default_truncation_distance - dropoff_epsilon);
+      observation_weight = std::max(observation_weight, 0.0f);
+    }
+
+    // Apply sparsity compensation if appropriate
+    if (config_.use_sparsity_compensation_factor) {
+      if (std::abs(sdf) < config_.default_truncation_distance) {
+        observation_weight *= config_.sparsity_compensation_factor;
+      }
+    }
   }
-  const float new_voxel_weight = tsdf_voxel->weight + observation_weight;
 
-  if (new_voxel_weight < kFloatEpsilon) {
+  // Truncate the new total voxel weight according to the max weight
+  const float new_voxel_weight =
+      std::min(tsdf_voxel->weight + observation_weight, config_.max_weight);
+
+  // Make sure voxels go back to zero when deintegrating
+  if (deintegrate && new_voxel_weight < 1.0) {
     tsdf_voxel->distance = 0.0f;
     tsdf_voxel->weight = 0.0f;
     return;
   }
 
-  const float distance_to_surface = interpolate(range_image, h, w);
-  const float sdf = distance_to_surface - distance_to_voxel;
-  if (sdf < -config_.default_truncation_distance) {
-    return;
-  }
-
   // Store the updated voxel weight and distance
   tsdf_voxel->distance = (tsdf_voxel->distance * tsdf_voxel->weight +
                           std::min(config_.default_truncation_distance, sdf) *

From 548b39f9f127b50b99adfe1e4c8e5e78adf3dff4 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Wed, 13 May 2020 01:02:39 +0200
Subject: [PATCH 3/8] Scale update weight with point density and make carving
 optional

---
 .../integrator/projective_tsdf_integrator.h   |  1 +
 .../projective_tsdf_integrator_inl.h          | 47 +++++++++++++------
 2 files changed, 33 insertions(+), 15 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
index 1b71fbbc7..05dbc449a 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
@@ -31,6 +31,7 @@ class ProjectiveTsdfIntegrator : public voxblox::TsdfIntegratorBase {
   static constexpr int kHeight = 64;
   static constexpr double altitude_angle_max = 16.611;
   static constexpr double azimuth_angle_offset = 3.164;
+  const double ray_intersections_per_distance_squared_;
 
   Eigen::MatrixXf range_image_;
 
diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index 03979a02c..c1128d5ba 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -17,7 +17,15 @@ ProjectiveTsdfIntegrator<interpolation_scheme>::ProjectiveTsdfIntegrator(
       num_voxels_per_block_(layer_->voxels_per_side() *
                             layer_->voxels_per_side() *
                             layer_->voxels_per_side()),
-      range_image_(kHeight, kWidth) {}
+      range_image_(kHeight, kWidth),
+      ray_intersections_per_distance_squared_(
+          voxel_size_ * kWidth / (2 * M_PI)  // horizontal point density
+          * voxel_size_ * kHeight /
+          (2 * altitude_angle_max * M_PI / 180.0))  // vertical point density
+{
+  CHECK(config_.use_const_weight) << "Scaling the weight by the inverse square "
+                                     "depth is (not yet) supported.";
+}
 
 template <InterpolationScheme interpolation_scheme>
 void ProjectiveTsdfIntegrator<interpolation_scheme>::integratePointCloud(
@@ -105,8 +113,17 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::parsePointcloud(
           T_G_C *
           (point_C_bearing * (distance + config_.default_truncation_distance)) *
           layer_->block_size_inv();
+      Point t_G_C_truncated_scaled;
+      if (config_.voxel_carving_enabled) {
+        t_G_C_truncated_scaled = t_G_C_scaled;
+      } else {
+        t_G_C_truncated_scaled =
+            point_G_scaled - T_G_C * point_C_bearing *
+                                 config_.default_truncation_distance *
+                                 layer_->block_size_inv();
+      }
       voxblox::GlobalIndex block_index;
-      voxblox::RayCaster ray_caster(point_G_scaled, t_G_C_scaled);
+      voxblox::RayCaster ray_caster(point_G_scaled, t_G_C_truncated_scaled);
       while (ray_caster.nextRayIndex(&block_index)) {
         touched_block_indices->insert(block_index.cast<IndexElement>());
       }
@@ -162,6 +179,13 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
   const float distance_to_surface = interpolate(range_image, h, w);
   const float sdf = distance_to_surface - distance_to_voxel;
 
+  // Approximate how many rays would have updated the voxel
+  // NOTE: We do this to reflect that we are more certain about
+  //       the updates applied to nearer voxels
+  const float num_rays_intersecting_voxel =
+      ray_intersections_per_distance_squared_ /
+      (distance_to_voxel * distance_to_voxel);
+
   // Skip voxels that fall outside the TSDF truncation distance
   if (sdf < -config_.default_truncation_distance) {
     return;
@@ -176,20 +200,13 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
   {
     // Set the sign of the measurement weight
     if (deintegrate) {
-      observation_weight = -1.0f;
+      observation_weight = -num_rays_intersecting_voxel;
     } else {
-      observation_weight = 1.0f;
+      observation_weight = num_rays_intersecting_voxel;
     }
 
-    // Scale the weight by the inverse square depth if appropriate
-    if (!config_.use_const_weight) {
-      const FloatingPoint dist_z = std::abs(t_C_voxel.z());
-      if (dist_z > kEpsilon) {
-        observation_weight /= (dist_z * dist_z);
-      } else {
-        return;
-      }
-    }
+    // NOTE: Scaling the weight by the inverse square depth is not yet
+    //       supported, since this problem is ill-defined for LiDAR
 
     // Apply weight drop-off if appropriate
     const FloatingPoint dropoff_epsilon = voxel_size_;
@@ -233,7 +250,7 @@ Point ProjectiveTsdfIntegrator<interpolation_scheme>::imageToBearing(
     const T h, const T w) {
   double altitude_angle =
       M_PI / 180.0 *
-      (altitude_angle_max - h / 63.0 * (2.0 * altitude_angle_max));
+      (altitude_angle_max - h / (kHeight - 1.0) * (2.0 * altitude_angle_max));
   double azimuth_angle =
       2.0 * M_PI *
       (static_cast<double>(w) / kWidth + azimuth_angle_offset / 360.0);
@@ -255,7 +272,7 @@ bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
 
   double altitude_angle = std::asin(b_C_normalized.z());
   *h = static_cast<T>((altitude_angle_max - 180.0 / M_PI * altitude_angle) *
-                      63.0 / (2.0 * altitude_angle_max));
+                      (kHeight - 1.0) / (2.0 * altitude_angle_max));
   if (*h < 0 || kHeight - 1 < *h) {
     return false;
   }

From b3f8aeeb9404a819a5166ea76fd8cc7ebb993f2e Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Wed, 13 May 2020 02:01:51 +0200
Subject: [PATCH 4/8] Consistently use TsdfIntegratorFactory

---
 .../voxblox/integrator/tsdf_integrator.h      |  6 ++++--
 voxblox/src/integrator/tsdf_integrator.cc     |  6 ++++++
 voxblox_ros/include/voxblox_ros/tsdf_server.h |  2 +-
 voxblox_ros/src/tsdf_server.cc                | 19 ++-----------------
 4 files changed, 13 insertions(+), 20 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/tsdf_integrator.h b/voxblox/include/voxblox/integrator/tsdf_integrator.h
index 9dcf81760..3e224ef8c 100644
--- a/voxblox/include/voxblox/integrator/tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/tsdf_integrator.h
@@ -31,14 +31,16 @@ enum class TsdfIntegratorType : int {
   kSimple = 1,
   kMerged = 2,
   kFast = 3,
+  kProjective = 4,
 };
 
-static constexpr size_t kNumTsdfIntegratorTypes = 3u;
+static constexpr size_t kNumTsdfIntegratorTypes = 4u;
 
 const std::array<std::string, kNumTsdfIntegratorTypes>
     kTsdfIntegratorTypeNames = {{/*kSimple*/ "simple",
                                  /*kMerged*/ "merged",
-                                 /*kFast*/ "fast"}};
+                                 /*kFast*/ "fast",
+                                 /*kProjective*/ "projective"}};
 
 /**
  * Base class to the simple, merged and fast TSDF integrators. The integrator
diff --git a/voxblox/src/integrator/tsdf_integrator.cc b/voxblox/src/integrator/tsdf_integrator.cc
index 40bf72249..11213b23a 100644
--- a/voxblox/src/integrator/tsdf_integrator.cc
+++ b/voxblox/src/integrator/tsdf_integrator.cc
@@ -1,4 +1,5 @@
 #include "voxblox/integrator/tsdf_integrator.h"
+#include "voxblox/integrator/projective_tsdf_integrator.h"
 
 #include <iostream>
 #include <list>
@@ -37,6 +38,11 @@ TsdfIntegratorBase::Ptr TsdfIntegratorFactory::create(
     case TsdfIntegratorType::kFast:
       return TsdfIntegratorBase::Ptr(new FastTsdfIntegrator(config, layer));
       break;
+    case TsdfIntegratorType::kProjective:
+      return TsdfIntegratorBase::Ptr(
+          new ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>(config,
+                                                                       layer));
+      break;
     default:
       LOG(FATAL) << "Unknown TSDF integrator type: "
                  << static_cast<int>(integrator_type);
diff --git a/voxblox_ros/include/voxblox_ros/tsdf_server.h b/voxblox_ros/include/voxblox_ros/tsdf_server.h
index 606cf8a61..736bd6e38 100644
--- a/voxblox_ros/include/voxblox_ros/tsdf_server.h
+++ b/voxblox_ros/include/voxblox_ros/tsdf_server.h
@@ -230,7 +230,7 @@ class TsdfServer {
 
   // Maps and integrators.
   std::shared_ptr<TsdfMap> tsdf_map_;
-  std::unique_ptr<TsdfIntegratorBase> tsdf_integrator_;
+  TsdfIntegratorBase::Ptr tsdf_integrator_;
 
   /// ICP matcher
   std::shared_ptr<ICP> icp_;
diff --git a/voxblox_ros/src/tsdf_server.cc b/voxblox_ros/src/tsdf_server.cc
index 1c6f675eb..ad3ea2dd8 100644
--- a/voxblox_ros/src/tsdf_server.cc
+++ b/voxblox_ros/src/tsdf_server.cc
@@ -92,23 +92,8 @@ TsdfServer::TsdfServer(const ros::NodeHandle& nh,
 
   std::string method("merged");
   nh_private_.param("method", method, method);
-  if (method.compare("simple") == 0) {
-    tsdf_integrator_.reset(new SimpleTsdfIntegrator(
-        integrator_config, tsdf_map_->getTsdfLayerPtr()));
-  } else if (method.compare("merged") == 0) {
-    tsdf_integrator_.reset(new MergedTsdfIntegrator(
-        integrator_config, tsdf_map_->getTsdfLayerPtr()));
-  } else if (method.compare("fast") == 0) {
-    tsdf_integrator_.reset(new FastTsdfIntegrator(
-        integrator_config, tsdf_map_->getTsdfLayerPtr()));
-  } else if (method.compare("projective") == 0) {
-    tsdf_integrator_.reset(
-        new ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>(
-            integrator_config, tsdf_map_->getTsdfLayerPtr()));
-  } else {
-    tsdf_integrator_.reset(new SimpleTsdfIntegrator(
-        integrator_config, tsdf_map_->getTsdfLayerPtr()));
-  }
+  tsdf_integrator_ = TsdfIntegratorFactory::create(
+      method, integrator_config, tsdf_map_->getTsdfLayerPtr());
 
   mesh_layer_.reset(new MeshLayer(tsdf_map_->block_size()));
 

From d5bfac9f5f338603d1bd7e262a1b78b791db83b9 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Thu, 14 May 2020 13:59:58 +0200
Subject: [PATCH 5/8] Make projective integrator sensor model params
 configurable through ROS

---
 .../integrator/projective_tsdf_integrator.h   | 10 ++--
 .../projective_tsdf_integrator_inl.h          | 50 ++++++++++++-------
 .../voxblox/integrator/tsdf_integrator.h      |  5 ++
 voxblox_ros/include/voxblox_ros/ros_params.h  | 12 +++++
 voxblox_ros/launch/arche_dataset.launch       |  4 ++
 5 files changed, 60 insertions(+), 21 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
index 05dbc449a..70e6912f0 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
@@ -27,10 +27,12 @@ class ProjectiveTsdfIntegrator : public voxblox::TsdfIntegratorBase {
                            const bool deintegrate = false);
 
  private:
-  static constexpr int kWidth = 1024;
-  static constexpr int kHeight = 64;
-  static constexpr double altitude_angle_max = 16.611;
-  static constexpr double azimuth_angle_offset = 3.164;
+  // Sensor model
+  const int horizontal_resolution_;
+  const int vertical_resolution_;
+  // TODO(victorr): Use radians internally instead
+  const double altitude_angle_max_deg_;
+  const double azimuth_angle_offset_deg_;
   const double ray_intersections_per_distance_squared_;
 
   Eigen::MatrixXf range_image_;
diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index c1128d5ba..8701adda6 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -14,15 +14,28 @@ ProjectiveTsdfIntegrator<interpolation_scheme>::ProjectiveTsdfIntegrator(
     const voxblox::TsdfIntegratorBase::Config &config,
     voxblox::Layer<voxblox::TsdfVoxel> *layer)
     : TsdfIntegratorBase(config, layer),
+      horizontal_resolution_(config.sensor_horizontal_resolution),
+      vertical_resolution_(config.sensor_vertical_resolution),
+      altitude_angle_max_deg_(config.sensor_altitude_angle_max_degrees),
+      azimuth_angle_offset_deg_(config.sensor_azimuth_angle_offset_degrees),
+      range_image_(config.sensor_vertical_resolution,
+                   config.sensor_horizontal_resolution),
       num_voxels_per_block_(layer_->voxels_per_side() *
                             layer_->voxels_per_side() *
                             layer_->voxels_per_side()),
-      range_image_(kHeight, kWidth),
       ray_intersections_per_distance_squared_(
-          voxel_size_ * kWidth / (2 * M_PI)  // horizontal point density
-          * voxel_size_ * kHeight /
-          (2 * altitude_angle_max * M_PI / 180.0))  // vertical point density
+          voxel_size_ * horizontal_resolution_ /
+          (2 * M_PI)  // horizontal point density
+          * voxel_size_ * vertical_resolution_ /
+          (2 * altitude_angle_max_deg_ * M_PI /
+           180.0))  // vertical point density
 {
+  CHECK_GT(horizontal_resolution_, 0)
+      << "The horizontal sensor resolution must be a positive integer";
+  CHECK_GT(vertical_resolution_, 0)
+      << "The vertical sensor resolution must be a positive integer";
+  CHECK_GT(altitude_angle_max_deg_, 0)
+      << "The maximum altitude angle of the sensor must be a positive float";
   CHECK(config_.use_const_weight) << "Scaling the weight by the inverse square "
                                      "depth is (not yet) supported.";
 }
@@ -250,10 +263,11 @@ Point ProjectiveTsdfIntegrator<interpolation_scheme>::imageToBearing(
     const T h, const T w) {
   double altitude_angle =
       M_PI / 180.0 *
-      (altitude_angle_max - h / (kHeight - 1.0) * (2.0 * altitude_angle_max));
-  double azimuth_angle =
-      2.0 * M_PI *
-      (static_cast<double>(w) / kWidth + azimuth_angle_offset / 360.0);
+      (altitude_angle_max_deg_ -
+       h / (vertical_resolution_ - 1.0) * (2.0 * altitude_angle_max_deg_));
+  double azimuth_angle = 2.0 * M_PI *
+                         (static_cast<double>(w) / horizontal_resolution_ +
+                          azimuth_angle_offset_deg_ / 360.0);
 
   Point bearing;
   bearing.x() = std::cos(altitude_angle) * std::cos(azimuth_angle);
@@ -271,9 +285,10 @@ bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
   CHECK_NOTNULL(w);
 
   double altitude_angle = std::asin(b_C_normalized.z());
-  *h = static_cast<T>((altitude_angle_max - 180.0 / M_PI * altitude_angle) *
-                      (kHeight - 1.0) / (2.0 * altitude_angle_max));
-  if (*h < 0 || kHeight - 1 < *h) {
+  *h = static_cast<T>(
+      (altitude_angle_max_deg_ - 180.0 / M_PI * altitude_angle) *
+      (vertical_resolution_ - 1.0) / (2.0 * altitude_angle_max_deg_));
+  if (*h < 0 || vertical_resolution_ - 1 < *h) {
     return false;
   }
 
@@ -289,12 +304,13 @@ bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
     azimuth_angle = M_PI + std::atan(-b_C_normalized.y() / b_C_normalized.x());
   }
   *w = static_cast<T>(
-      kWidth * (azimuth_angle / (2.0 * M_PI) - azimuth_angle_offset / 360.0));
+      horizontal_resolution_ *
+      (azimuth_angle / (2.0 * M_PI) - azimuth_angle_offset_deg_ / 360.0));
   if (*w < 0) {
-    *w += kWidth;
+    *w += horizontal_resolution_;
   }
 
-  return (0 < *w && *w < kWidth - 1);
+  return (0 < *w && *w < horizontal_resolution_ - 1);
 }
 
 template <>
@@ -309,7 +325,7 @@ inline float
 ProjectiveTsdfIntegrator<InterpolationScheme::kMinNeighbor>::interpolate(
     const Eigen::MatrixXf &range_image, const float h, const float w) {
   // TODO(victorr): Implement better edge handling
-  if (h >= kHeight || w >= kWidth) {
+  if (h >= vertical_resolution_ || w >= horizontal_resolution_) {
     return range_image(std::floor(h), std::floor(w));
   }
 
@@ -329,7 +345,7 @@ ProjectiveTsdfIntegrator<InterpolationScheme::kBilinear>::interpolate(
     const Eigen::MatrixXf &range_image, const float h, const float w) {
   // TODO(victorr): Implement better edge handling
   // Check if we're on the edge, to avoid out-of-bounds matrix access
-  if (h >= kHeight || w >= kWidth) {
+  if (h >= vertical_resolution_ || w >= horizontal_resolution_) {
     return range_image(std::floor(h), std::floor(w));
   }
 
@@ -358,7 +374,7 @@ inline float
 ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>::interpolate(
     const Eigen::MatrixXf &range_image, const float h, const float w) {
   // Check if we're on the edge, to avoid out-of-bounds matrix access
-  if (h >= kHeight || w >= kWidth) {
+  if (h >= vertical_resolution_ || w >= horizontal_resolution_) {
     return range_image(std::floor(h), std::floor(w));
   }
 
diff --git a/voxblox/include/voxblox/integrator/tsdf_integrator.h b/voxblox/include/voxblox/integrator/tsdf_integrator.h
index 3e224ef8c..b5847d590 100644
--- a/voxblox/include/voxblox/integrator/tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/tsdf_integrator.h
@@ -86,6 +86,11 @@ class TsdfIntegratorBase {
     int clear_checks_every_n_frames = 1;
     /// fast integrator specific
     float max_integration_time_s = std::numeric_limits<float>::max();
+    // projective integrator specific
+    int sensor_horizontal_resolution = 0;
+    int sensor_vertical_resolution = 0;
+    double sensor_altitude_angle_max_degrees = 0.0;
+    double sensor_azimuth_angle_offset_degrees = 0.0;
 
     std::string print() const;
   };
diff --git a/voxblox_ros/include/voxblox_ros/ros_params.h b/voxblox_ros/include/voxblox_ros/ros_params.h
index 8a2e2b9bd..2646c9df6 100644
--- a/voxblox_ros/include/voxblox_ros/ros_params.h
+++ b/voxblox_ros/include/voxblox_ros/ros_params.h
@@ -111,6 +111,18 @@ inline TsdfIntegratorBase::Config getTsdfIntegratorConfigFromRosParam(
                    integrator_config.integration_order_mode);
   nh_private.param("integrator_threads", integrator_threads,
                    integrator_threads);
+  nh_private.param("sensor_horizontal_resolution",
+                   integrator_config.sensor_horizontal_resolution,
+                   integrator_config.sensor_horizontal_resolution);
+  nh_private.param("sensor_vertical_resolution",
+                   integrator_config.sensor_vertical_resolution,
+                   integrator_config.sensor_vertical_resolution);
+  nh_private.param("sensor_altitude_angle_max_degrees",
+                   integrator_config.sensor_altitude_angle_max_degrees,
+                   integrator_config.sensor_altitude_angle_max_degrees);
+  nh_private.param("sensor_azimuth_angle_offset_degrees",
+                   integrator_config.sensor_azimuth_angle_offset_degrees,
+                   integrator_config.sensor_azimuth_angle_offset_degrees);
 
   integrator_config.default_truncation_distance =
       static_cast<float>(truncation_distance);
diff --git a/voxblox_ros/launch/arche_dataset.launch b/voxblox_ros/launch/arche_dataset.launch
index d87e29e2a..5f8ed56f2 100644
--- a/voxblox_ros/launch/arche_dataset.launch
+++ b/voxblox_ros/launch/arche_dataset.launch
@@ -13,6 +13,10 @@
         <rosparam>
             verbose: false
             method: "projective"
+            sensor_horizontal_resolution: 1024
+            sensor_vertical_resolution: 64
+            sensor_altitude_angle_max_degrees: 16.611
+            sensor_azimuth_angle_offset_degrees: 3.164
             color_mode: "normals"
             world_frame: "mission"
             tsdf_voxel_size: 0.20

From c1f32bb655de532fa18c39d64439408b26dbc6d0 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor_reijgwart@hotmail.com>
Date: Thu, 14 May 2020 18:27:03 +0200
Subject: [PATCH 6/8] Clear up spherical sensor model param names

---
 .../integrator/projective_tsdf_integrator.h   |  4 +--
 .../projective_tsdf_integrator_inl.h          | 29 +++++++------------
 .../voxblox/integrator/tsdf_integrator.h      |  3 +-
 voxblox_ros/include/voxblox_ros/ros_params.h  |  9 ++----
 voxblox_ros/launch/arche_dataset.launch       |  3 +-
 5 files changed, 17 insertions(+), 31 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
index 70e6912f0..b0e6e5ce1 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator.h
@@ -30,9 +30,7 @@ class ProjectiveTsdfIntegrator : public voxblox::TsdfIntegratorBase {
   // Sensor model
   const int horizontal_resolution_;
   const int vertical_resolution_;
-  // TODO(victorr): Use radians internally instead
-  const double altitude_angle_max_deg_;
-  const double azimuth_angle_offset_deg_;
+  const double vertical_fov_rad_;
   const double ray_intersections_per_distance_squared_;
 
   Eigen::MatrixXf range_image_;
diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index 8701adda6..35e2ca574 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -16,8 +16,8 @@ ProjectiveTsdfIntegrator<interpolation_scheme>::ProjectiveTsdfIntegrator(
     : TsdfIntegratorBase(config, layer),
       horizontal_resolution_(config.sensor_horizontal_resolution),
       vertical_resolution_(config.sensor_vertical_resolution),
-      altitude_angle_max_deg_(config.sensor_altitude_angle_max_degrees),
-      azimuth_angle_offset_deg_(config.sensor_azimuth_angle_offset_degrees),
+      vertical_fov_rad_(config.sensor_vertical_field_of_view_degrees * M_PI /
+                        180.0),
       range_image_(config.sensor_vertical_resolution,
                    config.sensor_horizontal_resolution),
       num_voxels_per_block_(layer_->voxels_per_side() *
@@ -27,15 +27,14 @@ ProjectiveTsdfIntegrator<interpolation_scheme>::ProjectiveTsdfIntegrator(
           voxel_size_ * horizontal_resolution_ /
           (2 * M_PI)  // horizontal point density
           * voxel_size_ * vertical_resolution_ /
-          (2 * altitude_angle_max_deg_ * M_PI /
-           180.0))  // vertical point density
+          vertical_fov_rad_)  // vertical point density
 {
   CHECK_GT(horizontal_resolution_, 0)
       << "The horizontal sensor resolution must be a positive integer";
   CHECK_GT(vertical_resolution_, 0)
       << "The vertical sensor resolution must be a positive integer";
-  CHECK_GT(altitude_angle_max_deg_, 0)
-      << "The maximum altitude angle of the sensor must be a positive float";
+  CHECK_GT(vertical_fov_rad_, 0)
+      << "The vertical field of view of the sensor must be a positive float";
   CHECK(config_.use_const_weight) << "Scaling the weight by the inverse square "
                                      "depth is (not yet) supported.";
 }
@@ -262,12 +261,9 @@ template <typename T>
 Point ProjectiveTsdfIntegrator<interpolation_scheme>::imageToBearing(
     const T h, const T w) {
   double altitude_angle =
-      M_PI / 180.0 *
-      (altitude_angle_max_deg_ -
-       h / (vertical_resolution_ - 1.0) * (2.0 * altitude_angle_max_deg_));
-  double azimuth_angle = 2.0 * M_PI *
-                         (static_cast<double>(w) / horizontal_resolution_ +
-                          azimuth_angle_offset_deg_ / 360.0);
+      vertical_fov_rad_ * (1.0 / 2.0 - h / (vertical_resolution_ - 1.0));
+  double azimuth_angle =
+      2.0 * M_PI * static_cast<double>(w) / horizontal_resolution_;
 
   Point bearing;
   bearing.x() = std::cos(altitude_angle) * std::cos(azimuth_angle);
@@ -285,9 +281,8 @@ bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
   CHECK_NOTNULL(w);
 
   double altitude_angle = std::asin(b_C_normalized.z());
-  *h = static_cast<T>(
-      (altitude_angle_max_deg_ - 180.0 / M_PI * altitude_angle) *
-      (vertical_resolution_ - 1.0) / (2.0 * altitude_angle_max_deg_));
+  *h = static_cast<T>((vertical_resolution_ - 1.0) *
+                      (1.0 / 2.0 - altitude_angle / vertical_fov_rad_));
   if (*h < 0 || vertical_resolution_ - 1 < *h) {
     return false;
   }
@@ -303,9 +298,7 @@ bool ProjectiveTsdfIntegrator<interpolation_scheme>::bearingToImage(
   } else {
     azimuth_angle = M_PI + std::atan(-b_C_normalized.y() / b_C_normalized.x());
   }
-  *w = static_cast<T>(
-      horizontal_resolution_ *
-      (azimuth_angle / (2.0 * M_PI) - azimuth_angle_offset_deg_ / 360.0));
+  *w = static_cast<T>(horizontal_resolution_ * azimuth_angle / (2.0 * M_PI));
   if (*w < 0) {
     *w += horizontal_resolution_;
   }
diff --git a/voxblox/include/voxblox/integrator/tsdf_integrator.h b/voxblox/include/voxblox/integrator/tsdf_integrator.h
index b5847d590..f684002e5 100644
--- a/voxblox/include/voxblox/integrator/tsdf_integrator.h
+++ b/voxblox/include/voxblox/integrator/tsdf_integrator.h
@@ -89,8 +89,7 @@ class TsdfIntegratorBase {
     // projective integrator specific
     int sensor_horizontal_resolution = 0;
     int sensor_vertical_resolution = 0;
-    double sensor_altitude_angle_max_degrees = 0.0;
-    double sensor_azimuth_angle_offset_degrees = 0.0;
+    double sensor_vertical_field_of_view_degrees = 0.0;
 
     std::string print() const;
   };
diff --git a/voxblox_ros/include/voxblox_ros/ros_params.h b/voxblox_ros/include/voxblox_ros/ros_params.h
index 2646c9df6..ee7c417d5 100644
--- a/voxblox_ros/include/voxblox_ros/ros_params.h
+++ b/voxblox_ros/include/voxblox_ros/ros_params.h
@@ -117,12 +117,9 @@ inline TsdfIntegratorBase::Config getTsdfIntegratorConfigFromRosParam(
   nh_private.param("sensor_vertical_resolution",
                    integrator_config.sensor_vertical_resolution,
                    integrator_config.sensor_vertical_resolution);
-  nh_private.param("sensor_altitude_angle_max_degrees",
-                   integrator_config.sensor_altitude_angle_max_degrees,
-                   integrator_config.sensor_altitude_angle_max_degrees);
-  nh_private.param("sensor_azimuth_angle_offset_degrees",
-                   integrator_config.sensor_azimuth_angle_offset_degrees,
-                   integrator_config.sensor_azimuth_angle_offset_degrees);
+  nh_private.param("sensor_vertical_field_of_view_degrees",
+                   integrator_config.sensor_vertical_field_of_view_degrees,
+                   integrator_config.sensor_vertical_field_of_view_degrees);
 
   integrator_config.default_truncation_distance =
       static_cast<float>(truncation_distance);
diff --git a/voxblox_ros/launch/arche_dataset.launch b/voxblox_ros/launch/arche_dataset.launch
index 5f8ed56f2..8967b0f34 100644
--- a/voxblox_ros/launch/arche_dataset.launch
+++ b/voxblox_ros/launch/arche_dataset.launch
@@ -15,8 +15,7 @@
             method: "projective"
             sensor_horizontal_resolution: 1024
             sensor_vertical_resolution: 64
-            sensor_altitude_angle_max_degrees: 16.611
-            sensor_azimuth_angle_offset_degrees: 3.164
+            sensor_vertical_field_of_view_degrees: 33.222
             color_mode: "normals"
             world_frame: "mission"
             tsdf_voxel_size: 0.20

From 580392ee0a2d93590ca2ad6298ed835a8ab96109 Mon Sep 17 00:00:00 2001
From: Lukas Schmid <schmluk@mavt.ethz.ch>
Date: Sat, 22 May 2021 17:40:14 +0200
Subject: [PATCH 7/8] fix projective integrator distance overflow at max weight

---
 .../integrator/projective_tsdf_integrator_inl.h       | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
index 35e2ca574..0ca12eda0 100644
--- a/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
+++ b/voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h
@@ -1,5 +1,5 @@
-#ifndef VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
-#define VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
+#ifndef VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
+#define VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
 
 #include <algorithm>
 #include <iostream>
@@ -238,8 +238,7 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
   }
 
   // Truncate the new total voxel weight according to the max weight
-  const float new_voxel_weight =
-      std::min(tsdf_voxel->weight + observation_weight, config_.max_weight);
+  const float new_voxel_weight = tsdf_voxel->weight + observation_weight;
 
   // Make sure voxels go back to zero when deintegrating
   if (deintegrate && new_voxel_weight < 1.0) {
@@ -253,7 +252,7 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
                           std::min(config_.default_truncation_distance, sdf) *
                               observation_weight) /
                          new_voxel_weight;
-  tsdf_voxel->weight = new_voxel_weight;
+  tsdf_voxel->weight = std::min(new_voxel_weight, config_.max_weight);
 }
 
 template <InterpolationScheme interpolation_scheme>
@@ -448,4 +447,4 @@ ProjectiveTsdfIntegrator<InterpolationScheme::kAdaptive>::interpolate(
 }
 }  // namespace voxblox
 
-#endif  // VOXBLOX_INCLUDE_VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_
+#endif  // VOXBLOX_INTEGRATOR_PROJECTIVE_TSDF_INTEGRATOR_INL_H_

From 1e9a87090488f80fb2c6b0f1eeaf57be466b281d Mon Sep 17 00:00:00 2001
From: samuel-gull <samuel.david.gull@gmail.com>
Date: Thu, 3 Jun 2021 09:16:28 +0200
Subject: [PATCH 8/8] distance scaling for updates from neighbors

---
 voxblox/src/integrator/esdf_integrator.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/voxblox/src/integrator/esdf_integrator.cc b/voxblox/src/integrator/esdf_integrator.cc
index c7f187aae..74277f7af 100644
--- a/voxblox/src/integrator/esdf_integrator.cc
+++ b/voxblox/src/integrator/esdf_integrator.cc
@@ -505,7 +505,7 @@ bool EsdfIntegrator::updateVoxelFromNeighbors(const GlobalIndex& global_index) {
   // Go through the neighbors and see if we can update any of them.
   for (unsigned int idx = 0u; idx < neighbor_indices.cols(); ++idx) {
     const GlobalIndex& neighbor_index = neighbor_indices.col(idx);
-    const FloatingPoint distance = Neighborhood<>::kDistances[idx];
+    const FloatingPoint distance = Neighborhood<>::kDistances[idx] * voxel_size_;
 
     EsdfVoxel* neighbor_voxel =
         esdf_layer_->getVoxelPtrByGlobalIndex(neighbor_index);