Mainly remove assumption that Submap needs to contain a TsdfMap

cblox/include/cblox/core/submap.h

@@ -2,6 +2,7 @@
 #define CBLOX_CORE_SUBMAP_H_
 
 #include <mutex>
+#include <utility>
 
 #include "cblox/core/common.h"
 
@@ -31,6 +32,15 @@ class Submap {
 
   inline SubmapID getID() const { return submap_id_; }
 
+  // Set interval in which submap was actively mapping.
+  inline void startMappingTime(int64_t time) { mapping_interval_.first = time; }
+  inline void stopMappingTime(int64_t time) { mapping_interval_.second = time; }
+
+  // Access mapping interval.
+  inline const std::pair<int64_t, int64_t>& getMappingInterval() const {
+    return mapping_interval_;
+  }
+
   virtual size_t getNumberOfAllocatedBlocks() const = 0;
 
   virtual size_t getMemorySize() const = 0;
@@ -39,6 +49,14 @@ class Submap {
 
   virtual void prepareForPublish() = 0;
 
+  // NOTE(ntonci): This assumes that all derived submap types will have at least
+  // TSDF Layer.
+  virtual voxblox::Layer<TsdfVoxel>* getTsdfLayerPtr() = 0;
+  virtual const voxblox::Layer<TsdfVoxel>& getTsdfLayer() const = 0;
+
+  virtual FloatingPoint block_size() const = 0;
+  virtual FloatingPoint voxel_size() const = 0;
+
   /*
   // Note(ntonci): In order to provide saving/loading functionality to the
   // derived class, the following methods should be implemented (see TsdfSubmap
@@ -58,6 +76,7 @@ class Submap {
  protected:
   const SubmapID submap_id_;
   Transformation T_M_S_;
+  std::pair<int64_t, int64_t> mapping_interval_;
 
  private:
   mutable std::mutex transformation_mutex_;

cblox/include/cblox/core/submap_collection.h

@@ -16,7 +16,7 @@ namespace cblox {
 class SubmapCollectionInterface {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
+
   typedef std::shared_ptr<SubmapCollectionInterface> Ptr;
   typedef std::shared_ptr<const SubmapCollectionInterface> ConstPtr;
 
@@ -31,9 +31,10 @@ class SubmapCollectionInterface {
   virtual bool getSubmapPose(const SubmapID submap_id,
                              Transformation* pose_ptr) const = 0;
 
-  virtual TsdfMap::Ptr getActiveTsdfMapPtr() = 0;
-  virtual const TsdfMap& getActiveTsdfMap() const = 0;
-  virtual TsdfMap::Ptr getTsdfMapPtr(const SubmapID submap_id) = 0;
+  virtual voxblox::Layer<TsdfVoxel>* getActiveTsdfLayerPtr() = 0;
+  virtual voxblox::Layer<TsdfVoxel>* getTsdfLayerPtr(
+      const SubmapID submap_id) = 0;
+  virtual const voxblox::Layer<TsdfVoxel>& getActiveTsdfLayer() const = 0;
 
   virtual bool empty() const = 0;
   virtual size_t size() const = 0;
@@ -102,11 +103,11 @@ class SubmapCollection : public SubmapCollectionInterface {
   const Transformation& getActiveSubmapPose() const;
   SubmapID getActiveSubmapID() const;
 
-  // Access the tsdf_map member of the active submap
-  TsdfMap::Ptr getActiveTsdfMapPtr();
-  const TsdfMap& getActiveTsdfMap() const;
-  // Access the tsdf_map member of any submap
-  virtual TsdfMap::Ptr getTsdfMapPtr(const SubmapID submap_id);
+  // Access the tsdf_layer member of the active submap
+  voxblox::Layer<TsdfVoxel>* getActiveTsdfLayerPtr();
+  const voxblox::Layer<TsdfVoxel>& getActiveTsdfLayer() const;
+  // Access the tsdf_layer member of any submap
+  voxblox::Layer<TsdfVoxel>* getTsdfLayerPtr(const SubmapID submap_id);
 
   // Activate a submap
   // NOTE(alexmillane): Note that creating a new submap automatically activates
@@ -163,6 +164,11 @@ class SubmapCollection : public SubmapCollectionInterface {
   // Gets the combined memory size of the layers in this collection.
   size_t getMemorySize() const;
 
+  inline const std::map<SubmapID, typename SubmapType::Ptr>& getIdToSubmap()
+      const {
+    return id_to_submap_;
+  }
+
   // Loading from file
   static bool LoadFromFile(
       const std::string& file_path,

cblox/include/cblox/core/submap_collection_inl.h

@@ -106,8 +106,8 @@ bool SubmapCollection<SubmapType>::duplicateSubmap(
     // below the new submap appears empty
     // new_tsdf_sub_map->getTsdfMapPtr().reset(new
     // TsdfMap(src_submap_ptr->getTsdfMap().getTsdfLayer()));
-    *(new_tsdf_sub_map->getTsdfMapPtr()) =
-        *(new TsdfMap(src_submap_ptr->getTsdfMap().getTsdfLayer()));
+    *(new_tsdf_sub_map->getTsdfLayerPtr()) =
+        *(new voxblox::Layer<TsdfVoxel>(src_submap_ptr->getTsdfLayer()));
     id_to_submap_.emplace(new_submap_id, new_tsdf_sub_map);
     return true;
   }
@@ -143,25 +143,27 @@ SubmapCollection<SubmapType>::getSubmapConstPtrs() const {
 }
 
 template <typename SubmapType>
-TsdfMap::Ptr SubmapCollection<SubmapType>::getActiveTsdfMapPtr() {
+voxblox::Layer<TsdfVoxel>*
+SubmapCollection<SubmapType>::getActiveTsdfLayerPtr() {
   const auto it = id_to_submap_.find(active_submap_id_);
   CHECK(it != id_to_submap_.end());
-  return (it->second)->getTsdfMapPtr();
+  return (it->second)->getTsdfLayerPtr();
 }
 
 template <typename SubmapType>
-TsdfMap::Ptr SubmapCollection<SubmapType>::getTsdfMapPtr(
+voxblox::Layer<TsdfVoxel>* SubmapCollection<SubmapType>::getTsdfLayerPtr(
     const SubmapID submap_id) {
   const auto it = id_to_submap_.find(submap_id);
   CHECK(it != id_to_submap_.end());
-  return (it->second)->getTsdfMapPtr();
+  return (it->second)->getTsdfLayerPtr();
 }
 
 template <typename SubmapType>
-const TsdfMap& SubmapCollection<SubmapType>::getActiveTsdfMap() const {
+const voxblox::Layer<TsdfVoxel>&
+SubmapCollection<SubmapType>::getActiveTsdfLayer() const {
   const auto it = id_to_submap_.find(active_submap_id_);
   CHECK(it != id_to_submap_.end());
-  return (it->second)->getTsdfMap();
+  return (it->second)->getTsdfLayer();
 }
 
 template <typename SubmapType>
@@ -218,10 +220,9 @@ TsdfMap::Ptr SubmapCollection<SubmapType>::getProjectedMap() const {
   // Looping over the current submaps
   for (const auto& id_submap_pair : id_to_submap_) {
     // Getting the tsdf submap and its pose
-    const TsdfMap& tsdf_map = (id_submap_pair.second)->getTsdfMap();
     const Transformation& T_G_S = (id_submap_pair.second)->getPose();
     // Merging layers the submap into the global layer
-    mergeLayerAintoLayerB(tsdf_map.getTsdfLayer(), T_G_S,
+    mergeLayerAintoLayerB(id_submap_pair.second->getTsdfLayer(), T_G_S,
                           combined_tsdf_layer_ptr);
   }
   // Returning the new map
@@ -430,8 +431,8 @@ void SubmapCollection<SubmapType>::fuseSubmapPair(
     const Transformation& T_G_S2 = submap_ptr_2->getPose();
     const Transformation T_S1_S2 = T_G_S1.inverse() * T_G_S2;
     // Merging the submap layers
-    mergeLayerAintoLayerB(submap_ptr_2->getTsdfMap().getTsdfLayer(), T_S1_S2,
-                          submap_ptr_1->getTsdfMapPtr()->getTsdfLayerPtr());
+    mergeLayerAintoLayerB(submap_ptr_2->getTsdfLayer(), T_S1_S2,
+                          submap_ptr_1->getTsdfLayerPtr());
     // Deleting Submap #2
     const size_t num_erased = id_to_submap_.erase(submap_id_2);
     CHECK_EQ(num_erased, 1);

cblox/include/cblox/core/tsdf_esdf_submap.h

@@ -18,12 +18,12 @@ class TsdfEsdfSubmap : public TsdfSubmap {
 
   struct Config : TsdfSubmap::Config, EsdfMap::Config {
     // default constructor
-    Config() : TsdfSubmap::Config(), EsdfMap::Config(){};
+    Config() : TsdfSubmap::Config(), EsdfMap::Config() {}
     // constructor based on tsdf and esdf config
     Config(const TsdfSubmap::Config& tsdf_map_config,
            const EsdfMap::Config& esdf_map_config)
         : TsdfSubmap::Config(tsdf_map_config),
-          EsdfMap::Config(esdf_map_config){};
+          EsdfMap::Config(esdf_map_config) {}
   };
 
   TsdfEsdfSubmap(const Transformation& T_M_S, const SubmapID submap_id,
@@ -42,9 +42,8 @@ class TsdfEsdfSubmap : public TsdfSubmap {
   TsdfEsdfSubmap(TsdfSubmap::Ptr tsdf_submap,
                  const voxblox::EsdfIntegrator::Config& esdf_integrator_config =
                      voxblox::EsdfIntegrator::Config())
-      : TsdfSubmap(tsdf_submap->getPose(), 
-                   tsdf_submap->getID(),
-                   tsdf_submap->getTsdfMapPtr()),
+      : TsdfSubmap(tsdf_submap->getPose(), tsdf_submap->getID(),
+                   tsdf_submap->getTsdfLayerSharedPtr()),
         esdf_integrator_config_(esdf_integrator_config) {
     CHECK(tsdf_submap);
     // Copying the config
@@ -67,12 +66,26 @@ class TsdfEsdfSubmap : public TsdfSubmap {
   // Generate the ESDF from the TSDF.
   void generateEsdf();
 
-  // Returns the underlying ESDF map pointers
-  EsdfMap::Ptr getEsdfMapPtr() { return esdf_map_; }
-  const EsdfMap& getEsdfMap() const { return *esdf_map_; }
+  // Returns the underlying esdf_layer pointers
+  inline voxblox::Layer<EsdfVoxel>* getEsdfLayerPtr() {
+    return esdf_map_->getEsdfLayerPtr();
+  }
+  inline const voxblox::Layer<EsdfVoxel>& getEsdfLayer() const {
+    return esdf_map_->getEsdfLayer();
+  }
 
   virtual Config getEsdfConfig() const { return config_; }
 
+  virtual size_t getNumberOfAllocatedBlocks() const override {
+    return tsdf_map_->getTsdfLayer().getNumberOfAllocatedBlocks() +
+           esdf_map_->getEsdfLayer().getNumberOfAllocatedBlocks();
+  }
+
+  virtual size_t getMemorySize() const override {
+    return tsdf_map_->getTsdfLayer().getMemorySize() +
+           esdf_map_->getEsdfLayer().getMemorySize();
+  }
+
   virtual void finishSubmap() override;
 
   virtual void prepareForPublish() override;

cblox/include/cblox/core/tsdf_submap.h

@@ -39,6 +39,13 @@ class TsdfSubmap : public Submap {
     config_.tsdf_voxels_per_side =
         tsdf_map_ptr->getTsdfLayer().voxels_per_side();
   }
+  TsdfSubmap(const Transformation& T_M_S, const SubmapID submap_id,
+             voxblox::Layer<TsdfVoxel>::Ptr tsdf_layer_ptr)
+      : Submap(T_M_S, submap_id), tsdf_map_(new TsdfMap(tsdf_layer_ptr)) {
+    CHECK(tsdf_layer_ptr);
+    config_.tsdf_voxel_size = tsdf_layer_ptr->voxel_size();
+    config_.tsdf_voxels_per_side = tsdf_layer_ptr->voxels_per_side();
+  }
 
   virtual ~TsdfSubmap() {
     if (!tsdf_map_.unique()) {
@@ -49,9 +56,17 @@ class TsdfSubmap : public Submap {
     }
   }
 
-  // Returns the underlying TSDF map pointers.
-  inline TsdfMap::Ptr getTsdfMapPtr() { return tsdf_map_; }
-  inline const TsdfMap& getTsdfMap() const { return *tsdf_map_; }
+  // Returns the underlying tsdf_layer pointers.
+  inline voxblox::Layer<TsdfVoxel>* getTsdfLayerPtr() {
+    return tsdf_map_->getTsdfLayerPtr();
+  }
+  inline voxblox::Layer<TsdfVoxel>::Ptr getTsdfLayerSharedPtr() {
+    return std::shared_ptr<voxblox::Layer<TsdfVoxel>>(
+        tsdf_map_->getTsdfLayerPtr());
+  }
+  inline const voxblox::Layer<TsdfVoxel>& getTsdfLayer() const {
+    return tsdf_map_->getTsdfLayer();
+  }
 
   inline FloatingPoint block_size() const { return tsdf_map_->block_size(); }
   inline FloatingPoint voxel_size() const { return tsdf_map_->voxel_size(); }
@@ -61,15 +76,6 @@ class TsdfSubmap : public Submap {
 
   Config getTsdfConfig() const { return config_; }
 
-  // Set interval in which submap was actively mapping.
-  inline void startMappingTime(int64_t time) { mapping_interval_.first = time; }
-  inline void stopMappingTime(int64_t time) { mapping_interval_.second = time; }
-
-  // Access mapping interval.
-  inline const std::pair<int64_t, int64_t>& getMappingInterval() const {
-    return mapping_interval_;
-  }
-
   virtual size_t getNumberOfAllocatedBlocks() const override {
     return tsdf_map_->getTsdfLayer().getNumberOfAllocatedBlocks();
   }
@@ -97,7 +103,6 @@ class TsdfSubmap : public Submap {
  protected:
   Config config_;
   TsdfMap::Ptr tsdf_map_;
-  std::pair<int64_t, int64_t> mapping_interval_;
 };
 
 }  // namespace cblox

cblox/include/cblox/integrator/tsdf_submap_collection_integrator.h

@@ -34,10 +34,10 @@ class TsdfSubmapCollectionIntegrator {
 
  private:
   // Initializes the integrator
-  void initializeIntegrator(const TsdfMap::Ptr& tsdf_map_ptr);
+  void initializeIntegrator(voxblox::Layer<TsdfVoxel>* tsdf_layer_ptr);
 
   // Changes the integration target the latest submap in the collection.
-  void updateIntegratorTarget(const TsdfMap::Ptr& tsdf_map_ptr);
+  void updateIntegratorTarget(voxblox::Layer<TsdfVoxel>* tsdf_layer_ptr);
 
   // Gets the submap relative pose
   // NOTE(alexmilane): T_G_S - Transformation between camera frame (C) and

cblox/include/cblox/mesh/submap_mesher_inl.h

@@ -57,14 +57,11 @@ void SubmapMesher::generateSeparatedMeshLayers(
     CHECK_NOTNULL(sub_map_ptr.get());
     LOG(INFO) << "Generating mesh for submap number #" << mesh_index;
     mesh_index++;
-    // Getting the TSDF data
-    const TsdfMap& tsdf_map = sub_map_ptr->getTsdfMap();
     // Creating a mesh layer to hold the mesh fragment
-    MeshLayer::Ptr mesh_layer_ptr(
-        new MeshLayer(sub_map_ptr->getTsdfMap().block_size()));
+    MeshLayer::Ptr mesh_layer_ptr(new MeshLayer(sub_map_ptr->block_size()));
     // Generating the mesh
     MeshIntegrator<TsdfVoxel> mesh_integrator(
-        mesh_config_, tsdf_map.getTsdfLayer(), mesh_layer_ptr.get());
+        mesh_config_, sub_map_ptr->getTsdfLayer(), mesh_layer_ptr.get());
     constexpr bool only_mesh_updated_blocks = false;
     constexpr bool clear_updated_flag = false;
     mesh_integrator.generateMesh(only_mesh_updated_blocks, clear_updated_flag);
@@ -78,7 +75,7 @@ void SubmapMesher::generateMeshInGlobalFrame(const SubmapType& submap,
                                              MeshLayer* mesh_layer_G_ptr) {
   CHECK_NOTNULL(mesh_layer_G_ptr);
   // Mesh in submap frame
-  MeshLayer mesh_layer_S(submap.getTsdfMap().block_size());
+  MeshLayer mesh_layer_S(submap.block_size());
   generateMeshInSubmapFrame(submap, &mesh_layer_S);
   // To world frame
   const Transformation& T_G_S = submap.getPose();
@@ -89,11 +86,9 @@ template <typename SubmapType>
 void SubmapMesher::generateMeshInSubmapFrame(const SubmapType& submap,
                                              MeshLayer* mesh_layer_S_ptr) {
   CHECK_NOTNULL(mesh_layer_S_ptr);
-  // Getting the TSDF data
-  const TsdfMap& tsdf_map = submap.getTsdfMap();
   // Generating the mesh
-  MeshIntegrator<TsdfVoxel> mesh_integrator(
-      mesh_config_, tsdf_map.getTsdfLayer(), mesh_layer_S_ptr);
+  MeshIntegrator<TsdfVoxel> mesh_integrator(mesh_config_, submap.getTsdfLayer(),
+                                            mesh_layer_S_ptr);
   constexpr bool only_mesh_updated_blocks = false;
   constexpr bool clear_updated_flag = false;
   mesh_integrator.generateMesh(only_mesh_updated_blocks, clear_updated_flag);

cblox/src/core/tsdf_esdf_submap.cpp

@@ -1,4 +1,7 @@
 #include "cblox/core/tsdf_esdf_submap.h"
+
+#include <memory>
+
 #include "cblox/utils/quat_transformation_protobuf_utils.h"
 
 namespace cblox {
@@ -81,8 +84,7 @@ TsdfEsdfSubmap::Ptr TsdfEsdfSubmap::LoadFromStream(
   if (!voxblox::io::LoadBlocksFromStream(
           submap_proto.num_blocks(),
           Layer<TsdfVoxel>::BlockMergingStrategy::kReplace, proto_file_ptr,
-          submap_ptr->getTsdfMapPtr()->getTsdfLayerPtr(),
-          tmp_byte_offset_ptr)) {
+          submap_ptr->getTsdfLayerPtr(), tmp_byte_offset_ptr)) {
     LOG(ERROR) << "Could not load the tsdf blocks from stream.";
     return nullptr;
   }
@@ -96,8 +98,7 @@ TsdfEsdfSubmap::Ptr TsdfEsdfSubmap::LoadFromStream(
   if (!voxblox::io::LoadBlocksFromStream(
           submap_proto.num_esdf_blocks(),
           Layer<EsdfVoxel>::BlockMergingStrategy::kReplace, proto_file_ptr,
-          submap_ptr->getEsdfMapPtr()->getEsdfLayerPtr(),
-          tmp_byte_offset_ptr)) {
+          submap_ptr->getEsdfLayerPtr(), tmp_byte_offset_ptr)) {
     LOG(ERROR) << "Could not load the esdf blocks from stream.";
     return nullptr;
   }

cblox/src/core/tsdf_submap.cpp

@@ -1,5 +1,7 @@
 #include "cblox/core/tsdf_submap.h"
 
+#include <memory>
+
 #include "cblox/utils/quat_transformation_protobuf_utils.h"
 
 namespace cblox {
@@ -84,8 +86,7 @@ TsdfSubmap::Ptr TsdfSubmap::LoadFromStream(const Config& config,
   if (!voxblox::io::LoadBlocksFromStream(
           submap_proto.num_blocks(),
           Layer<TsdfVoxel>::BlockMergingStrategy::kReplace, proto_file_ptr,
-          submap_ptr->getTsdfMapPtr()->getTsdfLayerPtr(),
-          tmp_byte_offset_ptr)) {
+          submap_ptr->getTsdfLayerPtr(), tmp_byte_offset_ptr)) {
     LOG(ERROR) << "Could not load the tsdf blocks from stream.";
     return nullptr;
   }