Use SemanticLabel2Color: make maps .at safe

kimera_semantics/CMakeLists.txt

@@ -9,9 +9,9 @@ cs_add_library(${PROJECT_NAME}
   src/color.cpp
   src/csv_iterator.cpp
   src/semantic_integrator_base.cpp
-  src/semantic_tsdf_integrator_merged.cpp
-  src/semantic_tsdf_integrator_fast.cpp
   src/semantic_tsdf_integrator_factory.cpp
+  src/semantic_tsdf_integrator_fast.cpp
+  src/semantic_tsdf_integrator_merged.cpp
 )
 
 cs_install()

kimera_semantics/include/kimera_semantics/color.h

@@ -36,6 +36,11 @@ typedef std::unordered_map<HashableColor, SemanticLabel, ColorHasher>
 typedef std::unordered_map<SemanticLabel, HashableColor>
     SemanticLabelToColorMap;
 
+
+// TODO(Toni): this is just to hack our way through the fact that our images
+// are not label ids but just colors :( This is not used if the pointclouds
+// you integrate have associated label ids. It is just for the case where
+// you use its colors as ids.
 class SemanticLabel2Color {
  public:
   SemanticLabel2Color(const std::string& filename);

kimera_semantics/include/kimera_semantics/semantic_integrator_base.h

@@ -79,14 +79,8 @@ class SemanticIntegratorBase {
     /// How to color the semantic mesh.
     ColorMode color_mode = ColorMode::kSemantic;
 
-    SemanticLabelToColorMap semantic_label_color_map_ =
-        getRandomSemanticLabelToColorMap();
-
-    // TODO(Toni): this is just to hack our way through the fact that our images
-    // are not label ids but just colors :( This is not used if the pointclouds
-    // you integrate have associated label ids. It is just for the case where
-    // you use its colors as ids.
-    ColorToSemanticLabelMap color_to_semantic_label_map_;
+
+    std::shared_ptr<SemanticLabel2Color> semantic_label_to_color_ = nullptr;
   };
 
   SemanticIntegratorBase(const SemanticConfig& semantic_config,

kimera_semantics/src/color.cpp

@@ -66,12 +66,29 @@ SemanticLabel2Color::SemanticLabel2Color(const std::string& filename)
 
 SemanticLabel SemanticLabel2Color::getSemanticLabelFromColor(
     const HashableColor& color) const {
-  return color_to_semantic_label_.at(color);
+  const auto& it = color_to_semantic_label_.find(color);
+  if (it != color_to_semantic_label_.end()) {
+    return it->second;
+  } else {
+    LOG(ERROR) << "Caught an unknown color: \n"
+               << "RGB: " << std::to_string(color.r) << ' '
+               <<  std::to_string(color.g) << ' '
+               <<  std::to_string(color.b) << ' '
+               <<  std::to_string(color.a);
+    return 0u; // Assign unknown label for now...
+  }
 }
 
 HashableColor SemanticLabel2Color::getColorFromSemanticLabel(
     const SemanticLabel& semantic_label) const {
-  return semantic_label_to_color_map_.at(semantic_label);
+  const auto& it = semantic_label_to_color_map_.find(semantic_label);
+  if (it != semantic_label_to_color_map_.end()) {
+    return it->second;
+  } else {
+    LOG(ERROR) << "Caught an unknown semantic label: \n"
+               << "Label: " << std::to_string(semantic_label);
+    return HashableColor(); // Assign unknown color for now...
+  }
 }
 
 }  // namespace kimera

kimera_semantics/src/semantic_integrator_base.cpp

@@ -366,7 +366,8 @@ void SemanticIntegratorBase::updateSemanticVoxelColor(
   // thread-safe, and adding mutexes for coloring seems silly.
   // Precompute, for all possible SemanticLabels (255) a color.
   *semantic_voxel_color =
-      semantic_config_.semantic_label_color_map_.at(semantic_label);
+      semantic_config_.semantic_label_to_color_->getColorFromSemanticLabel(
+          semantic_label);
 }
 
 }  // Namespace kimera

kimera_semantics/src/semantic_tsdf_integrator_fast.cpp

@@ -150,8 +150,10 @@ void FastSemanticTsdfIntegrator::integratePointCloud(
   // TODO(Toni): parallelize with openmp
   for (size_t i = 0; i < colors.size(); i++) {
     const vxb::Color& color = colors[i];
-    semantic_labels[i] = semantic_config_.color_to_semantic_label_map_.at(
-        HashableColor(color.r, color.g, color.b, 255u));
+    CHECK(semantic_config_.semantic_label_to_color_);
+    semantic_labels[i] =
+        semantic_config_.semantic_label_to_color_->getSemanticLabelFromColor(
+            HashableColor(color.r, color.g, color.b, 255u));
   }
 
   vxb::timing::Timer integrate_timer("integrate/fast");

kimera_semantics/src/semantic_tsdf_integrator_merged.cpp

@@ -81,21 +81,10 @@ void MergedSemanticTsdfIntegrator::integratePointCloud(
     // TODO(Toni): Pointcloud recolor sets `a` field to 0. Making the
     // map lookup fail.
     const vxb::Color& color = colors[i];
-    hash_colors.at(i) = HashableColor(color.r, color.g, color.b, 255u);
-    // const auto& it =
-    // semantic_config_.color_to_semantic_label_map_.find(color_a); if (it !=
-    // semantic_config_.color_to_semantic_label_map_.end()) {
+    CHECK(semantic_config_.semantic_label_to_color_);
     semantic_labels[i] =
-        semantic_config_.color_to_semantic_label_map_.at(
-          hash_colors.at(i));
-    //} else {
-    //  LOG(ERROR) << "Caught an unknown color: \n"
-    //             << "RGB: " << std::to_string(color_a.r) << ' '
-    //             <<  std::to_string(color_a.g) << ' '
-    //              <<  std::to_string(color_a.b) << ' '
-    //              <<  std::to_string(color_a.a);
-    //  semantic_labels[i] = 0u; // Assign unknown label for now...
-    //}
+        semantic_config_.semantic_label_to_color_->getSemanticLabelFromColor(
+            HashableColor(color.r, color.g, color.b, 255u));
   }
 
   vxb::timing::Timer integrate_pcl_semantic_tsdf_timer(

kimera_semantics_ros/include/kimera_semantics_ros/ros_params.h

@@ -28,6 +28,13 @@ getSemanticTsdfIntegratorTypeFromRosParam(const ros::NodeHandle& nh_private) {
   return semantic_tsdf_integrator_type;
 }
 
+inline std::string getSemanticLabelToColorCsvFilepathFromRosParam(
+    const ros::NodeHandle& nh) {
+  std::string path = "semantics2labels.csv";
+  nh.param("semantic_label_2_color_csv_filepath", path, path);
+  return path;
+}
+
 inline SemanticIntegratorBase::SemanticConfig
 getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) {
   SemanticIntegratorBase::SemanticConfig semantic_config;
@@ -54,6 +61,11 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) {
     LOG(FATAL) << "Unknown semantic color mode: " << color_mode;
   }
 
+  // Get semantic map
+  semantic_config.semantic_label_to_color_ =
+      kimera::make_unique<SemanticLabel2Color>(
+      getSemanticLabelToColorCsvFilepathFromRosParam(nh_private);
+
   return semantic_config;
 }
 

kimera_semantics_ros/include/kimera_semantics_ros/semantic_tsdf_server.h

@@ -58,17 +58,11 @@ class SemanticTsdfServer : public vxb::TsdfServer {
   virtual ~SemanticTsdfServer() = default;
 
  protected:
-  static std::string getSemanticLabelToColorCsvFilepathFromRosParam(
-      const ros::NodeHandle& nh);
-
   // Configs.
   SemanticIntegratorBase::SemanticConfig semantic_config_;
 
   // Layers.
   std::unique_ptr<vxb::Layer<SemanticVoxel>> semantic_layer_;
-
-  // Map from semantic label to actual color.
-  const SemanticLabel2Color semantic_label_to_color_;
 };
 
 }  // Namespace kimera

kimera_semantics_ros/launch/kimera_semantics.launch

@@ -15,6 +15,7 @@
   <node name="player" pkg="rosbag" type="play" output="screen"
   args="-r 0.5 --clock $(arg bag_file)"  if="$(arg play_bag)"/>
 
+  <!-- Generate pointcloud with semantic labels -->
   <arg name="publish_point_clouds" default="true"/>
   <arg name="run_stereo_dense"     default="false"/>
   <group if="$(arg publish_point_clouds)">
@@ -27,8 +28,8 @@
           -no-bond" unless="$(arg run_stereo_dense)">
       <!-- Input -->
       <remap from="rgb/camera_info"             to="/tesse/left_cam/camera_info"/>
-      <remap from="rgb/image_rect_color"        to="/tesse/segmentation"/>
-      <remap from="depth_registered/image_rect" to="/tesse/depth"/>
+      <remap from="rgb/image_rect_color"        to="/tesse/segmentation/image_raw"/>
+      <remap from="depth_registered/image_rect" to="/tesse/depth/image_raw"/>
       <!-- Output -->
       <remap from="depth_registered/points"     to="/points"/>
       <!-- Params -->
@@ -42,9 +43,9 @@
       <!-- Input -->
       <remap from="left/image_rect_color"   to="/dev/null/left_cam"/>
       <remap from="right/image_rect_color"  to="/dev/null/right_cam"/>
-      <remap from="left/image_raw"          to="/tesse/left_cam"/>
+      <remap from="left/image_raw"          to="/tesse/left_cam/image_raw"/>
       <remap from="left/camera_info"        to="/tesse/left_cam/camera_info"/>
-      <remap from="right/image_raw"         to="/tesse/right_cam"/>
+      <remap from="right/image_raw"         to="/tesse/right_cam/image_raw"/>
       <remap from="right/camera_info"       to="/tesse/right_cam/camera_info"/>
       <!-- Output -->
       <remap from="/stereo_gray/points2"    to="/stereo_gray/points2"/>
@@ -57,7 +58,7 @@
       pkg="pointcloud_recolor" output="screen"
       args="-alsologtostderr" required="true" if="$(arg run_stereo_dense)">
       <!-- Input -->
-      <remap from="image_in"               to="/tesse/segmentation"/>
+      <remap from="image_in"               to="/tesse/segmentation/image_raw"/>
       <remap from="pointcloud_in"          to="/stereo_gray/points2"/>
       <!-- Output -->
       <remap from="pointcloud_recolor_out" to="/points"/>

kimera_semantics_ros/package.xml

@@ -14,6 +14,7 @@
 
   <depend>cv_bridge</depend>
   <depend>gflags_catkin</depend>
+  <depend>glog_catkin</depend>
   <depend>minkindr_conversions</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>

kimera_semantics_ros/src/semantic_tsdf_server.cpp

@@ -36,10 +36,13 @@
 
 #include "kimera_semantics_ros/semantic_tsdf_server.h"
 
+#include <glog/logging.h>
+
 #include <voxblox_ros/ros_params.h>
-#include "kimera_semantics_ros/ros_params.h"
 
-#include "kimera_semantics/semantic_tsdf_integrator_factory.h"
+#include <kimera_semantics/semantic_tsdf_integrator_factory.h>
+
+#include "kimera_semantics_ros/ros_params.h"
 
 namespace kimera {
 
@@ -60,17 +63,10 @@ SemanticTsdfServer::SemanticTsdfServer(
     const vxb::MeshIntegratorConfig& mesh_config)
     : vxb::TsdfServer(nh, nh_private, config, integrator_config, mesh_config),
       semantic_config_(getSemanticTsdfIntegratorConfigFromRosParam(nh_private)),
-      semantic_layer_(nullptr),
-      semantic_label_to_color_(
-          getSemanticLabelToColorCsvFilepathFromRosParam(nh_private)) {
+      semantic_layer_(nullptr) {
   /// Semantic layer
   semantic_layer_.reset(new vxb::Layer<SemanticVoxel>(
       config.tsdf_voxel_size, config.tsdf_voxels_per_side));
-  /// Semantic configuration
-  semantic_config_.semantic_label_color_map_ =
-      semantic_label_to_color_.semantic_label_to_color_map_;
-  semantic_config_.color_to_semantic_label_map_ =
-      semantic_label_to_color_.color_to_semantic_label_;
   /// Replace the TSDF integrator by the SemanticTsdfIntegrator
   tsdf_integrator_ =
       SemanticTsdfIntegratorFactory::create(
@@ -82,11 +78,4 @@ SemanticTsdfServer::SemanticTsdfServer(
   CHECK(tsdf_integrator_);
 }
 
-std::string SemanticTsdfServer::getSemanticLabelToColorCsvFilepathFromRosParam(
-    const ros::NodeHandle& nh) {
-  std::string path = "semantics2labels.csv";
-  nh.param("semantic_label_2_color_csv_filepath", path, path);
-  return path;
-}
-
 }  // Namespace kimera