Merge branch 'master' into feature/split_controllers

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "voxblox"]
 	path = voxblox
-	url = git@github.com:ethz-asl/voxblox.git
+	url = https://github.com/ethz-asl/voxblox.git

README.md

@@ -10,25 +10,50 @@
 
 ## Getting started
 - [Installing on Ubuntu](https://github.com/ethz-asl/voxblox_gsm/wiki/Installation-on-Ubuntu)
+- [Sample Datasets](https://github.com/ethz-asl/voxblox-plusplus/wiki/Sample-Datasets)
 - [Basic usage](https://github.com/ethz-asl/voxblox-plusplus/wiki/Basic-usage)
 
-More information can be found in the [wiki pages](https://github.com/ethz-asl/voxblox-plusplus/wiki).
+More information and sample datasets can be found in the [wiki pages](https://github.com/ethz-asl/voxblox-plusplus/wiki).
 
 ## Citing
-If you use **Voxblox++** in your research, please cite the following publication:
+The **Voxblox++** framework is described in the following publication:
 
-- Margarita Grinvald, Fadri Furrer, Tonci Novkovic, Jen Jen Chung, Cesar Cadena, Roland Siegwart, Juan Nieto, **Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery**, _IEEE Robotics and Automation Letters_, 2019. [[PDF](https://arxiv.org/abs/1903.00268)] [[Video](https://www.youtube.com/watch?v=Jvl42VJmYxg)]
+- Margarita Grinvald, Fadri Furrer, Tonci Novkovic, Jen Jen Chung, Cesar Cadena, Roland Siegwart, and Juan Nieto, **Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery**, in _IEEE Robotics and Automation Letters_, July 2019. [[PDF](https://arxiv.org/abs/1903.00268)] [[Video](https://www.youtube.com/watch?v=Jvl42VJmYxg)]
 
 
 ```bibtex
 @article{grinvald2019volumetric,
-  title={{Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery}},
-  author={Grinvald, Margarita and Furrer, Fadri and Novkovic, Tonci and Chung, Jen Jen and Cadena, Cesar and Siegwart, Roland and Nieto, Juan},
+  author={M. {Grinvald} and F. {Furrer} and T. {Novkovic} and J. J. {Chung} and C. {Cadena} and R. {Siegwart} and J. {Nieto}},
   journal={IEEE Robotics and Automation Letters},
+  title={{Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery}},
   year={2019},
-  note={Under review}
+  volume={4},
+  number={3},
+  pages={3037-3044},
+  doi={10.1109/LRA.2019.2923960},
+  ISSN={2377-3766},
+  month={July},
 }
 ```
 
+The original geometry-only framework was introduced in the following publication:
+
+- Fadri Furrer, Tonci Novkovic, Marius Fehr, Abel Gawel, Margarita Grinvald, Torsten Sattler, Roland Siegwart, and Juan Nieto, **Incremental Object Database: Building 3D Models from Multiple Partial Observations**, in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_, October 2018. [[PDF](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8594391)] [[Video](https://www.youtube.com/watch?v=9_xg92qqw70)]
+
+```bibtex
+@inproceedings{8594391,
+  author={F. {Furrer} and T. {Novkovic} and M. {Fehr} and A. {Gawel} and M. {Grinvald} and T. {Sattler} and R. {Siegwart} and J. {Nieto}},
+  booktitle={2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+  title={{Incremental Object Database: Building 3D Models from Multiple Partial Observations}},
+  year={2018},
+  pages={6835-6842},
+  doi={10.1109/IROS.2018.8594391},
+  ISSN={2153-0866},
+  month={Oct},
+}
+```
+
+If you use **Voxblox++** in your research, please cite accordingly.
+
 ## License
 The code is available under the [BSD-3-Clause license](https://github.com/ethz-asl/voxblox-plusplus/blob/master/LICENSE).

global_segment_map/include/global_segment_map/meshing/instance_color_map.h

@@ -1,6 +1,8 @@
 #ifndef GLOBAL_SEGMENT_MAP_MESHING_INSTANCE_COLOR_MAP_H_
 #define GLOBAL_SEGMENT_MAP_MESHING_INSTANCE_COLOR_MAP_H_
 
+#include <shared_mutex>
+
 #include "global_segment_map/common.h"
 
 namespace voxblox {
@@ -11,6 +13,7 @@ class InstanceColorMap {
 
  protected:
   std::map<InstanceLabel, Color> color_map_;
+  std::shared_timed_mutex color_map_mutex_;
 };
 }  // namespace voxblox
 

global_segment_map/include/global_segment_map/meshing/label_color_map.h

@@ -1,6 +1,8 @@
 #ifndef GLOBAL_SEGMENT_MAP_MESHING_LABEL_COLOR_MAP_H_
 #define GLOBAL_SEGMENT_MAP_MESHING_LABEL_COLOR_MAP_H_
 
+#include <shared_mutex>
+
 #include "global_segment_map/common.h"
 
 namespace voxblox {
@@ -11,6 +13,7 @@ class LabelColorMap {
 
  protected:
   std::map<Label, Color> color_map_;
+  std::shared_timed_mutex color_map_mutex_;
 };
 }  // namespace voxblox
 

global_segment_map/include/global_segment_map/meshing/label_tsdf_mesh_integrator.h

@@ -5,7 +5,6 @@
 #include <cmath>
 #include <list>
 #include <map>
-#include <vector>
 
 #include <glog/logging.h>
 #include <voxblox/core/color.h>
@@ -101,6 +100,7 @@ class MeshLabelIntegrator : public MeshIntegrator<TsdfVoxel> {
   // construction time.
   bool remesh_ = false;
   std::map<Label, InstanceLabel> label_instance_map_;
+  std::shared_timed_mutex label_instance_map_mutex_;
 
   LabelColorMap label_color_map_;
   SemanticColorMap semantic_color_map_;

global_segment_map/include/global_segment_map/utils/visualizer.h

@@ -11,14 +11,14 @@ namespace voxblox {
 class Visualizer {
  public:
   Visualizer(const std::vector<std::shared_ptr<MeshLayer>>& mesh_layers,
-             bool* updated_mesh, std::mutex* updated_mesh_mutex_ptr);
+             bool* mesh_layer_updated, std::mutex* mesh_layer_mutex_ptr);
 
   void visualizeMesh();
 
   std::vector<std::shared_ptr<MeshLayer>> mesh_layers_;
 
-  std::mutex* updated_mesh_mutex_ptr_;
-  bool* updated_mesh_;
+  std::mutex* mesh_layer_mutex_ptr_;
+  bool* mesh_layer_updated_;
 
   size_t frame_count_;
 };

global_segment_map/src/label_tsdf_integrator.cc

@@ -447,8 +447,6 @@ LabelVoxel* LabelTsdfIntegrator::allocateStorageAndGetLabelVoxelPtr(
     }
   }
 
-  (*last_block)->updated() = true;
-
   const VoxelIndex local_voxel_idx =
       getLocalFromGlobalVoxelIndex(global_voxel_idx, voxels_per_side_);
 
@@ -709,11 +707,12 @@ void LabelTsdfIntegrator::swapLabels(const Label& old_label,
   label_layer_->getAllAllocatedBlocks(&all_label_blocks);
 
   for (const BlockIndex& block_index : all_label_blocks) {
-    Block<LabelVoxel>::Ptr block =
+    Block<TsdfVoxel>::Ptr tsdf_block = layer_->getBlockPtrByIndex(block_index);
+    Block<LabelVoxel>::Ptr label_block =
         label_layer_->getBlockPtrByIndex(block_index);
-    size_t vps = block->voxels_per_side();
-    for (int i = 0u; i < vps * vps * vps; i++) {
-      LabelVoxel& voxel = block->getVoxelByLinearIndex(i);
+    size_t vps = label_block->voxels_per_side();
+    for (size_t i = 0u; i < vps * vps * vps; i++) {
+      LabelVoxel& voxel = label_block->getVoxelByLinearIndex(i);
       Label previous_label = voxel.label;
 
       LabelConfidence old_label_confidence = 0u;
@@ -741,7 +740,9 @@ void LabelTsdfIntegrator::swapLabels(const Label& old_label,
         changeLabelCount(updated_label, 1);
 
         changeLabelCount(previous_label, -1);
-        block->updated() = true;
+        if (!tsdf_block->updated()) {
+          label_block->updated() = true;
+        }
       }
     }
   }

global_segment_map/src/meshing/instance_color_map.cc

@@ -7,8 +7,11 @@ namespace voxblox {
 void InstanceColorMap::getColor(const InstanceLabel& instance_label,
                                 Color* color) {
   CHECK_NOTNULL(color);
-
-  auto instance_color_map_it = color_map_.find(instance_label);
+  std::map<InstanceLabel, Color>::iterator instance_color_map_it;
+  {
+    std::shared_lock<std::shared_timed_mutex> readerLock(color_map_mutex_);
+    instance_color_map_it = color_map_.find(instance_label);
+  }
 
   if (instance_color_map_it != color_map_.end()) {
     *color = instance_color_map_it->second;
@@ -21,7 +24,7 @@ void InstanceColorMap::getColor(const InstanceLabel& instance_label,
     } else {
       *color = randomColor();
     }
-
+    std::lock_guard<std::shared_timed_mutex> writerLock(color_map_mutex_);
     color_map_.insert(std::pair<InstanceLabel, Color>(instance_label, *color));
   }
 }

global_segment_map/src/meshing/label_color_map.cc

@@ -8,13 +8,18 @@ void LabelColorMap::getColor(const Label& label, Color* color) {
   CHECK_NOTNULL(color);
   CHECK_NE(label, 0u);
 
-  auto label_color_map_it = color_map_.find(label);
+  std::map<Label, Color>::iterator label_color_map_it;
+  {
+    std::shared_lock<std::shared_timed_mutex> readerLock(color_map_mutex_);
+    label_color_map_it = color_map_.find(label);
+  }
 
   if (label_color_map_it != color_map_.end()) {
     *color = label_color_map_it->second;
   } else {
     *color = randomColor();
 
+    std::lock_guard<std::shared_timed_mutex> writerLock(color_map_mutex_);
     color_map_.insert(std::pair<Label, Color>(label, *color));
   }
 }

global_segment_map/src/meshing/label_tsdf_mesh_integrator.cc

@@ -71,16 +71,17 @@ bool MeshLabelIntegrator::generateMesh(bool only_mesh_updated_blocks,
          "label layers!";
 
   BlockIndexList all_tsdf_blocks;
-  // BlockIndexList all_label_blocks;
+
   if (only_mesh_updated_blocks) {
+    BlockIndexList all_label_blocks;
     sdf_layer_const_->getAllUpdatedBlocks(&all_tsdf_blocks);
-    // TODO(grinvalm) unique union of block indices here.
-    // label_layer_mutable_ptr_->getAllUpdatedBlocks(&all_label_blocks);
-    if (all_tsdf_blocks.size() == 0u) {
+    label_layer_mutable_ptr_->getAllUpdatedBlocks(&all_label_blocks);
+    if (all_tsdf_blocks.size() == 0u && all_label_blocks.size() == 0u) {
       return false;
     }
-    // all_tsdf_blocks.insert(all_tsdf_blocks.end(), all_label_blocks.begin(),
-    //                        all_label_blocks.end());
+
+    all_tsdf_blocks.insert(all_tsdf_blocks.end(), all_label_blocks.begin(),
+                           all_label_blocks.end());
   } else {
     sdf_layer_const_->getAllAllocatedBlocks(&all_tsdf_blocks);
   }
@@ -120,11 +121,6 @@ void MeshLabelIntegrator::generateMeshBlocksFunction(
   size_t list_idx;
   while (index_getter->getNextIndex(&list_idx)) {
     const BlockIndex& block_idx = all_tsdf_blocks[list_idx];
-    typename Block<TsdfVoxel>::Ptr tsdf_block =
-        sdf_layer_mutable_->getBlockPtrByIndex(block_idx);
-    typename Block<LabelVoxel>::Ptr label_block =
-        label_layer_mutable_ptr_->getBlockPtrByIndex(block_idx);
-
     updateMeshForBlock(block_idx);
     if (clear_updated_flag) {
       typename Block<TsdfVoxel>::Ptr tsdf_block =
@@ -133,9 +129,7 @@ void MeshLabelIntegrator::generateMeshBlocksFunction(
           label_layer_mutable_ptr_->getBlockPtrByIndex(block_idx);
 
       tsdf_block->updated() = false;
-      // TODO(margaritaG): enable when generateMesh() takes union of TSDF and
-      // label updated voxels.
-      // label_block->updated() = false;
+      label_block->updated() = false;
     }
   }
 }
@@ -147,13 +141,20 @@ InstanceLabel MeshLabelIntegrator::getInstanceLabel(const Label& label) {
   InstanceLabel instance_label =
       semantic_instance_label_fusion_ptr_->getInstanceLabel(
           label, kFramesCountThresholdFactor);
+  std::map<Label, InstanceLabel>::iterator prev_instance_it;
+  {
+    std::shared_lock<std::shared_timed_mutex> readerLock(
+        label_instance_map_mutex_);
+    prev_instance_it = label_instance_map_.find(label);
+  }
 
-  auto prev_instance_it = label_instance_map_.find(label);
   if (prev_instance_it != label_instance_map_.end()) {
     if (prev_instance_it->second != instance_label) {
       *remesh_ptr_ = true;
     }
   }
+  std::lock_guard<std::shared_timed_mutex> writerLock(
+      label_instance_map_mutex_);
   label_instance_map_[label] = instance_label;
   return instance_label;
 }
@@ -178,7 +179,6 @@ void MeshLabelIntegrator::updateMeshForBlock(const BlockIndex& block_index) {
   // } else if (!(tsdf_block && label_block)) {
   //   LOG(FATAL) << "Block allocation differs between the two layers.";
   // }
-
   extractBlockMesh(tsdf_block, mesh_block);
   // Update colors if needed.
   if (config_.use_color) {

global_segment_map/src/utils/visualizer.cc

@@ -4,10 +4,10 @@ namespace voxblox {
 
 Visualizer::Visualizer(
     const std::vector<std::shared_ptr<MeshLayer>>& mesh_layers,
-    bool* updated_mesh, std::mutex* updated_mesh_mutex_ptr)
+    bool* mesh_layer_updated, std::mutex* mesh_layer_mutex_ptr)
     : mesh_layers_(mesh_layers),
-      updated_mesh_(CHECK_NOTNULL(updated_mesh)),
-      updated_mesh_mutex_ptr_(CHECK_NOTNULL(updated_mesh_mutex_ptr)),
+      mesh_layer_updated_(CHECK_NOTNULL(mesh_layer_updated)),
+      mesh_layer_mutex_ptr_(CHECK_NOTNULL(mesh_layer_mutex_ptr)),
       frame_count_(0u) {}
 
 // TODO(grinvalm): make it more efficient by only updating the
@@ -58,15 +58,15 @@ void Visualizer::visualizeMesh() {
     meshes.clear();
     meshes.resize(n_visualizers);
 
-    if (updated_mesh_mutex_ptr_->try_lock()) {
-      if (*updated_mesh_) {
+    if (mesh_layer_mutex_ptr_->try_lock()) {
+      if (*mesh_layer_updated_) {
         for (int index = 0; index < n_visualizers; index++) {
           mesh_layers_[index]->getMesh(&meshes[index]);
         }
         refresh = true;
-        *updated_mesh_ = false;
+        *mesh_layer_updated_ = false;
       }
-      updated_mesh_mutex_ptr_->unlock();
+      mesh_layer_mutex_ptr_->unlock();
     }
 
     if (refresh) {

global_segment_map_node/cfg/default.yaml

@@ -29,6 +29,7 @@ meshing:
   publish_scene_mesh: true
   mesh_filename: "mesh.ply"
   compute_and_publish_bbox: false
+  min_weight: 3
 
 mesh_config:
   min_weight: 3

global_segment_map_node/cfg/scenenet.yaml

@@ -17,6 +17,5 @@ pairwise_confidence_merging:
 meshing:
   visualize: true
   update_mesh_every_n_sec: 1.0
-  publish_segment_mesh: false
   publish_scene_mesh: false
   mesh_filename: "gsm_scenenet_mesh.ply"

global_segment_map_node/include/voxblox_gsm/controller.h

@@ -144,8 +144,9 @@ class Controller {
 
   std::thread viz_thread_;
   Visualizer* visualizer_;
-  std::mutex updated_mesh_mutex_;
-  bool updated_mesh_;
+  std::mutex label_tsdf_layers_mutex_;
+  std::mutex mesh_layer_mutex_;
+  bool mesh_layer_updated_;
   bool need_full_remesh_;
 };
 

global_segment_map_node/launch/asl_office_floor_dataset.launch

@@ -3,7 +3,7 @@
   <arg name="bag_file" default="/path/to/data.bag"/>
   <arg name="visualize" default="true" />
 
-  <node name="mask_rcnn" pkg="mask_rcnn_ros" type="mask_rcnn_node.py" output="screen">
+  <node name="mask_rcnn" pkg="mask_rcnn_ros" type="mask_rcnn_node.py" output="log">
     <remap from="~input" to="/camera/rgb/image_raw" />
     <param name="~visualization" value="$(arg visualize)" />
   </node>

global_segment_map_node/launch/scenenn_dataset.launch

@@ -3,7 +3,7 @@
   <arg name="bag_file" default="/path/to/data.bag"/>
   <arg name="visualize" default="true" />
 
-  <node name="mask_rcnn" pkg="mask_rcnn_ros" type="mask_rcnn_node.py" output="screen">
+  <node name="mask_rcnn" pkg="mask_rcnn_ros" type="mask_rcnn_node.py" output="log">
       <remap from="~input" to="/camera/rgb/image_raw" />
       <param name="~visualization" value="$(arg visualize)" />
   </node>