Using dynamic number of labels

kimera_semantics/include/kimera_semantics/color.h

@@ -53,6 +53,8 @@ class SemanticLabel2Color {
   // Make these public if someone wants to access them directly.
   ColorToSemanticLabelMap color_to_semantic_label_;
   SemanticLabelToColorMap semantic_label_to_color_map_;
+
+  size_t number_of_colored_labels_;
 };
 
 // Color map from semantic labels to colors.
@@ -66,8 +68,10 @@ inline SemanticLabelToColorMap getRandomSemanticLabelToColorMap() {
   }
   // Make first colours easily distinguishable.
   CHECK_GE(semantic_label_color_map_.size(), 8);
-  CHECK_GE(semantic_label_color_map_.size(), kTotalNumberOfLabels);
+  //We won't use this check, color_map size is numberic limit max anyways
+  //CHECK_GE(semantic_label_color_map_.size(), max_possible_label);
   // TODO(Toni): Check it Matches with default value for SemanticVoxel!
+  //The semantic Labels written here are only meaningful for the Tesse simulation
   semantic_label_color_map_.at(0) = HashableColor::Gray();    // Label unknown
   semantic_label_color_map_.at(1) = HashableColor::Green();   // Label Ceiling
   semantic_label_color_map_.at(2) = HashableColor::Blue();    // Label Chair

kimera_semantics/include/kimera_semantics/common.h

@@ -18,17 +18,16 @@ typedef uint8_t SemanticLabel;
 typedef vxb::AlignedVector<SemanticLabel> SemanticLabels;
 // The size of this array determines how many semantic labels SemanticVoxblox
 // supports.
-// TODO(Toni): parametrize this, although that means it becomes unknown at
-// compile time...
-static constexpr size_t kTotalNumberOfLabels = 21;
+
+//Dynamic size Semantic Probabilities by David R.
 typedef vxb::FloatingPoint SemanticProbability;
-typedef Eigen::Matrix<SemanticProbability, kTotalNumberOfLabels, 1>
+typedef Eigen::Matrix<SemanticProbability, Eigen::Dynamic, 1>
     SemanticProbabilities;
 // A `#Labels X #Labels` Eigen matrix where each `j` column represents the
 // probability of observing label `j` when current label is `i`, where `i`
 // is the row index of the matrix.
 typedef Eigen::
-    Matrix<SemanticProbability, kTotalNumberOfLabels, kTotalNumberOfLabels>
+    Matrix<SemanticProbability, Eigen::Dynamic, Eigen::Dynamic>
         SemanticLikelihoodFunction;
 
 typedef vxb::LongIndexHashMapType<vxb::AlignedVector<size_t>>::type VoxelMap;

kimera_semantics/include/kimera_semantics/semantic_integrator_base.h

@@ -76,6 +76,10 @@ class SemanticIntegratorBase {
     // probability of non-match = 1 - measurement_probability_.
     SemanticProbability semantic_measurement_probability_ = 0.9f;
 
+    //The total number of labels in the segmenation node
+    //Used by the confidence matrices
+    size_t total_number_of_labels_ = 21;
+
     /// How to color the semantic mesh.
     ColorMode color_mode = ColorMode::kSemantic;
 
@@ -127,6 +131,7 @@ class SemanticIntegratorBase {
   // later by calling updateLayerWithStoredBlocks()
   SemanticVoxel* allocateStorageAndGetSemanticVoxelPtr(
       const vxb::GlobalIndex& global_voxel_idx,
+      size_t total_number_of_labels,
       vxb::Block<SemanticVoxel>::Ptr* last_semantic_block,
       vxb::BlockIndex* last_block_idx);
 

kimera_semantics/include/kimera_semantics/semantic_voxel.h

@@ -11,19 +11,21 @@
 
 namespace kimera {
 
-struct SemanticVoxel {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  // Initialize voxel to unknown label.
-  SemanticLabel semantic_label = 0u;
-  // Initialize voxel to uniform probability.
-  // Use log odds! So uniform ditribution of 1/kTotalNumberOfLabels,
-  // should be std::log(1/kTotalNumberOfLabels)
-  SemanticProbabilities semantic_priors =
-      // SemanticProbabilities::Constant(std::log(1 / kTotalNumberOfLabels));
-      SemanticProbabilities::Constant(-0.60205999132);
-  // Initialize voxel with gray color
-  // Make sure that all color maps agree on semantic label 0u -> gray
-  HashableColor color = HashableColor::Gray();
-};
+    struct SemanticVoxel {
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        // Initialize voxel to unknown label.
+        SemanticLabel semantic_label = 0u;
+        // Initialize voxel to uniform probability.
+        // Use log odds! So uniform ditribution of 1/total_number_of_labels_,
+        // should be std::log(1/total_number_of_labels_)
+        size_t total_number_of_layers_;
 
-}  // namespace kimera
+        // SemanticProbabilities::Constant(std::log(1 / total_number_of_labels_));
+        SemanticProbabilities semantic_priors;
+
+        // Initialize voxel with gray color
+        // Make sure that all color maps agree on semantic label 0u -> gray
+        HashableColor color = HashableColor::Gray();
+    };
+
+}  // namespace kimera

kimera_semantics/src/color.cpp

@@ -62,6 +62,7 @@ SemanticLabel2Color::SemanticLabel2Color(const std::string& filename)
   // TODO(Toni): remove
   // Assign color 255,255,255 to unknown object 0u
   color_to_semantic_label_[HashableColor::White()] = 0u;
+    number_of_colored_labels_ = row_number;
 }
 
 SemanticLabel SemanticLabel2Color::getSemanticLabelFromColor(

kimera_semantics/src/semantic_integrator_base.cpp

@@ -47,6 +47,7 @@
 
 #include <voxblox/integrator/tsdf_integrator.h>
 #include <voxblox/utils/timing.h>
+#include <iostream>
 
 #include "kimera_semantics/color.h"
 #include "kimera_semantics/common.h"
@@ -105,20 +106,23 @@ void SemanticIntegratorBase::setSemanticProbabilities() {
       << "Your probabilities do not make sense... The likelihood of a "
          "label, knowing that we have measured that label, should not be"
          "smaller than the likelihood of seeing another label!";
-  semantic_log_likelihood_ =
-      semantic_log_likelihood_.Constant(log_non_match_probability_);
+  semantic_log_likelihood_.resize(semantic_config_.total_number_of_labels_, semantic_config_.total_number_of_labels_);
+  semantic_log_likelihood_.setConstant(log_non_match_probability_);
   semantic_log_likelihood_.diagonal() =
-      semantic_log_likelihood_.diagonal().Constant(log_match_probability_);
+      semantic_log_likelihood_.diagonal().setConstant(log_match_probability_);
+
   // TODO(Toni): sanity checks, set as DCHECK_EQ.
   CHECK_NEAR(semantic_log_likelihood_.diagonal().sum(),
-             kTotalNumberOfLabels * log_match_probability_,
-             100 * vxb::kFloatEpsilon);
+             semantic_config_.total_number_of_labels_ * log_match_probability_,
+             10e-2);
+
+
   CHECK_NEAR(
       semantic_log_likelihood_.sum(),
-      kTotalNumberOfLabels * log_match_probability_ +
-          std::pow(kTotalNumberOfLabels, 2) * log_non_match_probability_ -
-          kTotalNumberOfLabels * log_non_match_probability_,
-      1000 * vxb::kFloatEpsilon);
+      semantic_config_.total_number_of_labels_ * log_match_probability_ +
+      std::pow(semantic_config_.total_number_of_labels_, 2) * log_non_match_probability_ -
+      semantic_config_.total_number_of_labels_ * log_non_match_probability_,
+      10e-2);
 }
 
 // TODO(Toni): Complete this function!!
@@ -155,14 +159,17 @@ void SemanticIntegratorBase::updateSemanticVoxel(
   updateSemanticVoxelProbabilities(measurement_frequencies,
                                    &semantic_voxel->semantic_priors);
 
+
   // Get MLE semantic label.
   calculateMaximumLikelihoodLabel(semantic_voxel->semantic_priors,
                                   &semantic_voxel->semantic_label);
 
+
   // Colorize according to current MLE semantic label.
   updateSemanticVoxelColor(semantic_voxel->semantic_label,
                            &semantic_voxel->color);
 
+
   // Actually change the color of the TSDF voxel so we do not need to change a
   // single line for the meshing with respect to Voxblox.
   switch (semantic_config_.color_mode) {
@@ -185,7 +192,8 @@ void SemanticIntegratorBase::updateSemanticVoxel(
   }
   ////////////////////////////////////////////////////////////////////////////
   //}
-}
+
+  }
 
 // Will return a pointer to a voxel located at global_voxel_idx in the tsdf
 // layer. Thread safe.
@@ -198,6 +206,7 @@ void SemanticIntegratorBase::updateSemanticVoxel(
 // updateLayerWithStoredBlocks()
 SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr(
     const vxb::GlobalIndex& global_voxel_idx,
+    size_t total_number_of_labels,
     vxb::Block<SemanticVoxel>::Ptr* last_block,
     vxb::BlockIndex* last_block_idx) {
   DCHECK(last_block != nullptr);
@@ -235,14 +244,24 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr(
                                    << block_idx.transpose();
 
       *last_block = insert_status.first->second;
+
+      SemanticProbabilities sem_probs;
+      sem_probs.resize(total_number_of_labels,1);
+      sem_probs.setConstant(std::log(1.0 /float(total_number_of_labels)));
+
+      for (size_t voxel_index = 0; voxel_index<std::pow(semantic_voxels_per_side_,3); voxel_index++){
+          (*last_block)->getVoxelByLinearIndex(voxel_index).total_number_of_layers_ = total_number_of_labels;
+          (*last_block)->getVoxelByLinearIndex(voxel_index).semantic_priors = sem_probs;
+      }
+
     }
   }
 
   // Only used if someone calls the getAllUpdatedBlocks I believe.
   (*last_block)->updated() = true;
 
   const vxb::VoxelIndex local_voxel_idx = vxb::getLocalFromGlobalVoxelIndex(
-      global_voxel_idx, semantic_voxels_per_side_);
+          global_voxel_idx, semantic_voxels_per_side_);
 
   return &((*last_block)->getVoxelByVoxelIndex(local_voxel_idx));
 }
@@ -278,11 +297,11 @@ void SemanticIntegratorBase::updateSemanticVoxelProbabilities(
     const SemanticProbabilities& measurement_frequencies,
     SemanticProbabilities* semantic_prior_probability) const {
   DCHECK(semantic_prior_probability != nullptr);
-  DCHECK_EQ(semantic_prior_probability->size(), kTotalNumberOfLabels);
+  DCHECK_EQ(semantic_prior_probability->size(), semantic_config_.total_number_of_labels_);
   DCHECK_LE((*semantic_prior_probability)[0], 0.0);
   DCHECK(std::isfinite((*semantic_prior_probability)[0]));
   DCHECK(!semantic_prior_probability->hasNaN());
-  DCHECK_EQ(measurement_frequencies.size(), kTotalNumberOfLabels);
+  DCHECK_EQ(measurement_frequencies.size(), semantic_config_.total_number_of_labels_);
   DCHECK_GE(measurement_frequencies.sum(), 1.0)
       << "We should at least have one measurement when calling this "
          "function.";
@@ -325,9 +344,9 @@ void SemanticIntegratorBase::normalizeProbabilities(
   if (normalization_factor != 0.0) {
     unnormalized_probs->normalize();
   } else {
-    CHECK_EQ(unnormalized_probs->size(), kTotalNumberOfLabels);
+    CHECK_EQ(unnormalized_probs->size(), semantic_config_.total_number_of_labels_);
     static const SemanticProbability kUniformLogProbability =
-        std::log(1 / kTotalNumberOfLabels);
+        std::log(1.0 / semantic_config_.total_number_of_labels_);
     LOG(WARNING) << "Normalization Factor is " << normalization_factor
                  << ", all values are 0. Normalizing to log(1/n) = "
                  << kUniformLogProbability;

kimera_semantics/src/semantic_tsdf_integrator_fast.cpp

@@ -41,6 +41,7 @@
 #include <utility>
 
 #include <voxblox/utils/timing.h>
+#include <iostream>
 
 #include "kimera_semantics/color.h"
 
@@ -128,9 +129,12 @@ void FastSemanticTsdfIntegrator::integrateSemanticFunction(
       updateTsdfVoxel(origin, point_G, global_voxel_idx, color, weight, voxel);
 
       SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr(
-          global_voxel_idx, &semantic_block, &semantic_block_idx);
-      SemanticProbabilities semantic_label_frequencies =
-          SemanticProbabilities::Zero();
+              global_voxel_idx, semantic_config_.total_number_of_labels_, &semantic_block, &semantic_block_idx);
+
+      SemanticProbabilities semantic_label_frequencies;
+      semantic_label_frequencies.resize(semantic_config_.total_number_of_labels_, 1);
+      semantic_label_frequencies.setZero();
+
       CHECK_LT(semantic_label, semantic_label_frequencies.size());
       semantic_label_frequencies[semantic_label] += 1.0f;
       updateSemanticVoxel(global_voxel_idx,

kimera_semantics/src/semantic_tsdf_integrator_merged.cpp

@@ -251,8 +251,9 @@ void MergedSemanticTsdfIntegrator::integrateVoxel(
   vxb::FloatingPoint merged_weight = 0.0f;
   // Calculate semantic labels frequencies to encode likelihood function.
   // Prefill with 0 frequency.
-  SemanticProbabilities semantic_label_frequencies =
-      SemanticProbabilities::Zero();
+  SemanticProbabilities semantic_label_frequencies;
+  semantic_label_frequencies.resize(semantic_config_.total_number_of_labels_, 1);
+  semantic_label_frequencies.setZero();
 
   // Loop over all point indices inside current voxel.
   // Generate merged values to propagate to other voxels:
@@ -319,7 +320,7 @@ void MergedSemanticTsdfIntegrator::integrateVoxel(
                     merged_weight, voxel);
 
     SemanticVoxel* semantic_voxel =
-        allocateStorageAndGetSemanticVoxelPtr(global_voxel_idx, &semantic_block, &semantic_block_idx);
+        allocateStorageAndGetSemanticVoxelPtr(global_voxel_idx, semantic_config_.total_number_of_labels_, &semantic_block, &semantic_block_idx);
     updateSemanticVoxel(global_voxel_idx,
                         semantic_label_frequencies,
                         &mutexes_,

kimera_semantics_ros/launch/kimera_semantics.launch

@@ -125,6 +125,7 @@
 
     <param name="semantic_label_2_color_csv_filepath"
     value="$(find kimera_semantics_ros)/cfg/tesse_multiscene_office1_segmentation_mapping.csv"/>
+    <param name="total_number_of_labels" value="21"/>
 
     <param name="publish_pointclouds"     value="false"/>
     <param name="update_mesh_every_n_sec" value="0.1" />

kimera_semantics_ros/src/ros_params.cpp

@@ -48,6 +48,13 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) {
   semantic_config.semantic_measurement_probability_ =
       static_cast<SemanticProbability>(semantic_measurement_probability);
 
+  // Get the total number of labels of the prediction
+  int total_number_of_labels = static_cast<int>(semantic_config.total_number_of_labels_);
+  nh_private.param("total_number_of_labels",
+                   total_number_of_labels,
+                   total_number_of_labels);
+  semantic_config.total_number_of_labels_ = static_cast<size_t>(total_number_of_labels);
+
   // Get semantic color mode
   std::string color_mode = "color";
   nh_private.param("semantic_color_mode", color_mode, color_mode);