Apply truncation

voxblox/include/voxblox/core/esdf_map.h

@@ -51,6 +51,10 @@ class EsdfMap {
   virtual ~EsdfMap() {}
 
   Layer<EsdfVoxel>* getEsdfLayerPtr() { return esdf_layer_.get(); }
+  const Layer<EsdfVoxel>* getEsdfLayerConstPtr() const {
+    return esdf_layer_.get();
+  }
+
   const Layer<EsdfVoxel>& getEsdfLayer() const { return *esdf_layer_; }
 
   FloatingPoint block_size() const { return block_size_; }

voxblox/include/voxblox/integrator/projective_tsdf_integrator_inl.h

@@ -248,12 +248,17 @@ void ProjectiveTsdfIntegrator<interpolation_scheme>::updateTsdfVoxel(
     return;
   }
 
+  const float new_sdf = (tsdf_voxel->distance * tsdf_voxel->weight +
+                         std::min(config_.default_truncation_distance, sdf) *
+                         observation_weight) /
+                        new_voxel_weight;
   // 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->distance =
+            (new_sdf > 0.0) ? std::min(config_.default_truncation_distance, new_sdf)
+                            : std::max(-config_.default_truncation_distance, new_sdf);
   tsdf_voxel->weight = new_voxel_weight;
+
+
 }
 
 template <InterpolationScheme interpolation_scheme>

voxblox/include/voxblox/io/layer_io.h

@@ -35,7 +35,7 @@ template <typename VoxelType>
 bool LoadBlocksFromStream(
     const size_t num_blocks,
     typename Layer<VoxelType>::BlockMergingStrategy strategy,
-    std::fstream* proto_file_ptr, Layer<VoxelType>* layer_ptr,
+    std::istream* proto_file_ptr, Layer<VoxelType>* layer_ptr,
     uint64_t* tmp_byte_offset_ptr);
 
 /**

voxblox/include/voxblox/io/layer_io_inl.h

@@ -36,8 +36,8 @@ bool LoadBlocksFromFile(
   do {
     // Get number of messages
     uint32_t num_protos;
-    if (!utils::readProtoMsgCountToStream(&proto_file, &num_protos,
-                                          &tmp_byte_offset)) {
+    if (!utils::readProtoMsgCountFromStream(&proto_file, &num_protos,
+                                            &tmp_byte_offset)) {
       LOG(ERROR) << "Could not read number of messages.";
       return false;
     }
@@ -104,7 +104,7 @@ template <typename VoxelType>
 bool LoadBlocksFromStream(
     const size_t num_blocks,
     typename Layer<VoxelType>::BlockMergingStrategy strategy,
-    std::fstream* proto_file_ptr, Layer<VoxelType>* layer_ptr,
+    std::istream* proto_file_ptr, Layer<VoxelType>* layer_ptr,
     uint64_t* tmp_byte_offset_ptr) {
   CHECK_NOTNULL(proto_file_ptr);
   CHECK_NOTNULL(layer_ptr);
@@ -150,8 +150,8 @@ bool LoadLayer(const std::string& file_path, const bool multiple_layer_support,
   do {
     // Get number of messages
     uint32_t num_protos;
-    if (!utils::readProtoMsgCountToStream(&proto_file, &num_protos,
-                                          &tmp_byte_offset)) {
+    if (!utils::readProtoMsgCountFromStream(&proto_file, &num_protos,
+                                            &tmp_byte_offset)) {
       LOG(ERROR) << "Could not read number of messages.";
       return false;
     }

voxblox/include/voxblox/mesh/marching_cubes.h

@@ -36,8 +36,8 @@ class MarchingCubes {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  static int kTriangleTable[256][16];
-  static int kEdgeIndexPairs[12][2];
+  static const int kTriangleTable[256][16];
+  static const int kEdgeIndexPairs[12][2];
 
   MarchingCubes() {}
   virtual ~MarchingCubes() {}

voxblox/include/voxblox/utils/color_maps.h

@@ -91,6 +91,72 @@ class IronbowColorMap : public ColorMap {
   float increment_;
 };
 
+class IdColorMap {
+ public:
+  IdColorMap() {}
+  virtual ~IdColorMap() {}
+
+  virtual Color colorLookup(size_t value) const = 0;
+};
+/**
+ * Map unique IDs from [0, Inf[ to unique colors that have constant (irrational)
+ * spacing and high spread on the color wheel.
+ * An important advantage of this color map is that the colors are independent
+ * of the max ID value, e.g. previous colors don't change when adding new IDs.
+ */
+class IrrationalIdColorMap : IdColorMap {
+ public:
+  IrrationalIdColorMap() : irrational_base_(3.f * M_PI) {}
+
+  void setIrrationalBase(float value) { irrational_base_ = value; }
+
+  virtual Color colorLookup(const size_t value) const {
+    const float normalized_color = std::fmod(value / irrational_base_, 1.f);
+    return rainbowColorMap(normalized_color);
+  }
+
+ private:
+  float irrational_base_;
+};
+/**
+ * Map unique IDs from [0, Inf[ to unique colors
+ * with piecewise constant spacing and higher spread on the color wheel.
+ * An important advantage of this color map is that the colors are independent
+ * of the max ID value, e.g. previous colors don't change when adding new IDs.
+ */
+class ExponentialOffsetIdColorMap : IdColorMap {
+ public:
+  ExponentialOffsetIdColorMap() : items_per_revolution_(10u) {}
+
+  void setItemsPerRevolution(uint value) { items_per_revolution_ = value; }
+
+  virtual Color colorLookup(const size_t value) const {
+    const size_t revolution = value / items_per_revolution_;
+    const float progress_along_revolution =
+        std::fmod(value / items_per_revolution_, 1.f);
+    // NOTE: std::modf could be used to simultaneously get the integer and
+    //       fractional parts, but the above code is assumed to be more readable
+
+    // Calculate the offset if appropriate
+    float offset = 0;
+    if (items_per_revolution_ < value + 1u) {
+      const size_t current_episode = std::floor(std::log2(revolution));
+      const size_t episode_start = std::exp2(current_episode);
+      const size_t episode_num_subdivisions = episode_start;
+      const size_t current_subdivision = revolution - episode_start;
+      const float subdivision_step_size =
+          1 / (items_per_revolution_ * 2 * episode_num_subdivisions);
+      offset = (2 * current_subdivision + 1) * subdivision_step_size;
+    }
+
+    const float normalized_color = progress_along_revolution + offset;
+    return rainbowColorMap(normalized_color);
+  }
+
+ private:
+  float items_per_revolution_;
+};
+
 }  // namespace voxblox
 
 #endif  // VOXBLOX_UTILS_COLOR_MAPS_H_

voxblox/include/voxblox/utils/protobuf_utils.h

@@ -2,6 +2,7 @@
 #define VOXBLOX_UTILS_PROTOBUF_UTILS_H_
 
 #include <fstream>
+#include <istream>
 
 #include <glog/logging.h>
 #include <google/protobuf/message.h>
@@ -10,13 +11,14 @@
 namespace voxblox {
 
 namespace utils {
-bool readProtoMsgCountToStream(std::fstream* stream_in, uint32_t* message_count,
-                               uint64_t* byte_offset);
+bool readProtoMsgCountFromStream(std::istream* stream_in,
+                                 uint32_t* message_count,
+                                 uint64_t* byte_offset);
 
 bool writeProtoMsgCountToStream(uint32_t message_count,
                                 std::fstream* stream_out);
 
-bool readProtoMsgFromStream(std::fstream* stream_in,
+bool readProtoMsgFromStream(std::istream* stream_in,
                             google::protobuf::Message* message,
                             uint64_t* byte_offset);
 

voxblox/src/mesh/marching_cubes.cc

@@ -26,7 +26,7 @@ namespace voxblox {
 // Lookup table from the 256 possible cube configurations from
 // CalculateVertexConfigurationIndex() to the 0-5 triplets the give the edges
 // where the triangle vertices lie.
-int MarchingCubes::kTriangleTable[256][16] = {
+const int MarchingCubes::kTriangleTable[256][16] = {
     {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
     {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
     {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
@@ -286,8 +286,8 @@ int MarchingCubes::kTriangleTable[256][16] = {
 
 // Lookup table from the 12 cube edge indices to their corresponding corner
 // indices.
-int MarchingCubes::kEdgeIndexPairs[12][2] = {{0, 1}, {1, 2}, {2, 3}, {3, 0},
-                                             {4, 5}, {5, 6}, {6, 7}, {7, 4},
-                                             {0, 4}, {1, 5}, {2, 6}, {3, 7}};
+const int MarchingCubes::kEdgeIndexPairs[12][2] = {
+    {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6},
+    {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}};
 
 }  // namespace voxblox

voxblox/src/utils/protobuf_utils.cc

@@ -7,12 +7,12 @@
 namespace voxblox {
 
 namespace utils {
-bool readProtoMsgCountToStream(std::fstream* stream_in, uint32_t* message_count,
-                               uint64_t* byte_offset) {
+bool readProtoMsgCountFromStream(std::istream* stream_in,
+                                 uint32_t* message_count,
+                                 uint64_t* byte_offset) {
   CHECK_NOTNULL(stream_in);
   CHECK_NOTNULL(message_count);
   CHECK_NOTNULL(byte_offset);
-  CHECK(stream_in->is_open());
   stream_in->clear();
   stream_in->seekg(*byte_offset, std::ios::beg);
   google::protobuf::io::IstreamInputStream raw_in(stream_in);
@@ -37,13 +37,12 @@ bool writeProtoMsgCountToStream(uint32_t message_count,
   return true;
 }
 
-bool readProtoMsgFromStream(std::fstream* stream_in,
+bool readProtoMsgFromStream(std::istream* stream_in,
                             google::protobuf::Message* message,
                             uint64_t* byte_offset) {
   CHECK_NOTNULL(stream_in);
   CHECK_NOTNULL(message);
   CHECK_NOTNULL(byte_offset);
-  CHECK(stream_in->is_open());
 
   stream_in->clear();
   stream_in->seekg(*byte_offset, std::ios::beg);

voxblox_ros/src/esdf_server.cc

@@ -240,7 +240,9 @@ void EsdfServer::esdfMapCallback(const voxblox_msgs::Layer& layer_msg) {
     ROS_ERROR_THROTTLE(10, "Got an invalid ESDF map message!");
   } else {
     ROS_INFO_ONCE("Got an ESDF map from ROS topic!");
-    publishPointclouds();
+    if (publish_pointclouds_) {
+      publishPointclouds();
+    }
   }
 }