Develop

kimera_semantics/CMakeLists.txt

@@ -1,6 +1,10 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.1)
 project(kimera_semantics)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
 find_package(catkin_simple REQUIRED)
 
 catkin_simple()

kimera_semantics/include/kimera_semantics/color.h

@@ -36,7 +36,6 @@ 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
@@ -53,32 +52,13 @@ 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 getNumLabels() const;
 };
 
 // Color map from semantic labels to colors.
 // Precompute distinct but all possible colors.
 // This is to avoid thread-saving updateSemanticColor...
-inline SemanticLabelToColorMap getRandomSemanticLabelToColorMap() {
-  SemanticLabelToColorMap semantic_label_color_map_;
-  SemanticLabel max_possible_label = std::numeric_limits<SemanticLabel>::max();
-  for (SemanticLabel i = 0; i < max_possible_label; i++) {
-    semantic_label_color_map_[i] = vxb::randomColor();
-  }
-  // Make first colours easily distinguishable.
-  CHECK_GE(semantic_label_color_map_.size(), 8u);
-  CHECK_GE(semantic_label_color_map_.size(), kTotalNumberOfLabels);
-  // TODO(Toni): Check it Matches with default value for SemanticVoxel!
-  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
-  semantic_label_color_map_.at(3) = HashableColor::Purple();  // Label Floor
-  semantic_label_color_map_.at(4) =
-      HashableColor::Pink();  // Label Objects/Furniture/Chair
-  semantic_label_color_map_.at(5) = HashableColor::Teal();    // Label Sofa
-  semantic_label_color_map_.at(6) = HashableColor::Orange();  // Label Table
-  semantic_label_color_map_.at(7) =
-      HashableColor::Yellow();  // Label Wall/Window/TV/board/Picture
-  return semantic_label_color_map_;
-}
+SemanticLabelToColorMap getRandomSemanticLabelToColorMap(size_t num_labels);
 
 }  // namespace kimera

kimera_semantics/include/kimera_semantics/common.h

@@ -14,34 +14,24 @@ namespace kimera {
 
 namespace vxb = voxblox;
 
-typedef uint8_t SemanticLabel;
-typedef vxb::AlignedVector<SemanticLabel> SemanticLabels;
 // Consider id 0 to be the `unknown' label, for which we don't update the
 // log-likelihood for that measurement.
 static constexpr uint8_t kUnknownSemanticLabelId = 0u;
-// 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;
+
+typedef uint8_t SemanticLabel;
+typedef vxb::AlignedVector<SemanticLabel> SemanticLabels;
+
 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>
-        SemanticLikelihoodFunction;
+typedef Eigen::Matrix<SemanticProbability, Eigen::Dynamic, Eigen::Dynamic>
+    SemanticLikelihoodFunction;
 
 typedef vxb::LongIndexHashMapType<vxb::AlignedVector<size_t>>::type VoxelMap;
 typedef VoxelMap::value_type VoxelMapElement;
 
-// Add compatibility for c++11's lack of make_unique.
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args) {
-  return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-
-
 }  // namespace kimera

kimera_semantics/include/kimera_semantics/semantic_integrator_base.h

@@ -60,10 +60,10 @@ enum class ColorMode : int {
 class SemanticIntegratorBase {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  typedef std::shared_ptr<SemanticIntegratorBase> Ptr;
-  typedef vxb::
-      ApproxHashArray<12, std::mutex, vxb::GlobalIndex, vxb::LongIndexHash>
-          Mutexes;
+  using Ptr = std::shared_ptr<SemanticIntegratorBase>;
+  using Mutexes = vxb::
+      ApproxHashArray<12, std::mutex, vxb::GlobalIndex, vxb::LongIndexHash>;
+  using LabelFrequencyMap = std::map<SemanticLabel, size_t>;
 
   struct SemanticConfig {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -111,7 +111,7 @@ class SemanticIntegratorBase {
    * @param semantic_voxel
    */
   void updateSemanticVoxel(const vxb::GlobalIndex& global_voxel_idx,
-                           const SemanticProbabilities& measurement_frequencies,
+                           const LabelFrequencyMap& frequencies,
                            Mutexes* mutexes,
                            vxb::TsdfVoxel* tsdf_voxel,
                            SemanticVoxel* semantic_voxel);
@@ -150,7 +150,7 @@ class SemanticIntegratorBase {
    **/
   // TODO(Toni): Unit Test this function!
   void updateSemanticVoxelProbabilities(
-      const SemanticProbabilities& measurement_frequencies,
+      const LabelFrequencyMap& measurement_frequencies,
       SemanticProbabilities* semantic_prior_probability) const;
 
   // THREAD SAFE
@@ -203,8 +203,12 @@ class SemanticIntegratorBase {
   mutable std::mutex temp_semantic_block_mutex_;
   vxb::Layer<SemanticVoxel>::BlockHashMap temp_semantic_block_map_;
 
+  //! Number of semantic labels
+  const size_t num_labels;
+
   // Log probabilities of matching measurement and prior label,
   // and non-matching.
+  SemanticProbability log_init_probablility;
   SemanticProbability log_match_probability_;
   SemanticProbability log_non_match_probability_;
   // A `#Labels X #Labels` Eigen matrix where each `j` column represents the

kimera_semantics/include/kimera_semantics/semantic_voxel.h

@@ -13,17 +13,14 @@ namespace kimera {
 
 struct SemanticVoxel {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  // Initialize voxel to unknown label.
+  //! Current MLE semantic 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
+  //! Log-likelihood priors of each label
+  SemanticProbabilities semantic_priors;
+  //! Current color of label
   HashableColor color = HashableColor::Gray();
+  //! Whether or not the voxel has been initialized
+  bool empty = true;
 };
 
 }  // namespace kimera

kimera_semantics/src/color.cpp

@@ -74,10 +74,9 @@ SemanticLabel SemanticLabel2Color::getSemanticLabelFromColor(
   } else {
     LOG(ERROR) << "Caught an unknown color: \n"
                << "RGBA: " << std::to_string(color.r) << ' '
-               <<  std::to_string(color.g) << ' '
-               <<  std::to_string(color.b) << ' '
-               <<  std::to_string(color.a);
-    return kUnknownSemanticLabelId; // Assign unknown label for now...
+               << std::to_string(color.g) << ' ' << std::to_string(color.b)
+               << ' ' << std::to_string(color.a);
+    return kUnknownSemanticLabelId;  // Assign unknown label for now...
   }
 }
 
@@ -89,8 +88,40 @@ HashableColor SemanticLabel2Color::getColorFromSemanticLabel(
   } else {
     LOG(ERROR) << "Caught an unknown semantic label: \n"
                << "Label: " << std::to_string(semantic_label);
-    return HashableColor(); // Assign unknown color for now...
+    return HashableColor();  // Assign unknown color for now...
   }
 }
 
+size_t SemanticLabel2Color::getNumLabels() const {
+  // note that we want to avoid overflow when max_label == 255 (we return
+  // max_label + 1), so this is a size_t instead of a uint8_t
+  size_t max_label = 0;
+  for (const auto id_color_pair : semantic_label_to_color_map_) {
+    if (id_color_pair.first > max_label) {
+      max_label = id_color_pair.first;
+    }
+  }
+  return max_label + 1;
+}
+
+SemanticLabelToColorMap getRandomSemanticLabelToColorMap(size_t num_labels) {
+  SemanticLabelToColorMap cmap;
+
+  for (size_t i = 0; i < num_labels; i++) {
+    cmap[i] = vxb::randomColor();
+  }
+
+  // Make first colours easily distinguishable.
+  CHECK_GE(cmap.size(), 8u);
+  cmap.at(0) = HashableColor::Gray();    // Label unknown
+  cmap.at(1) = HashableColor::Green();   // Label Ceiling
+  cmap.at(2) = HashableColor::Blue();    // Label Chair
+  cmap.at(3) = HashableColor::Purple();  // Label Floor
+  cmap.at(4) = HashableColor::Pink();    // Label Objects/Furniture/Chair
+  cmap.at(5) = HashableColor::Teal();    // Label Sofa
+  cmap.at(6) = HashableColor::Orange();  // Label Table
+  cmap.at(7) = HashableColor::Yellow();  // Label Wall/Window/TV/board/Picture
+  return cmap;
+}
+
 }  // namespace kimera

kimera_semantics/src/semantic_integrator_base.cpp

@@ -59,17 +59,7 @@ SemanticIntegratorBase::SemanticIntegratorBase(
     vxb::Layer<SemanticVoxel>* semantic_layer)
     : semantic_config_(semantic_config),
       semantic_layer_(nullptr),
-      temp_semantic_block_mutex_(),
-      temp_semantic_block_map_(),
-      log_match_probability_(),
-      log_non_match_probability_(),
-      semantic_log_likelihood_(),
-      semantic_voxel_size_(),
-      semantic_voxels_per_side_(),
-      semantic_block_size_(),
-      semantic_voxel_size_inv_(),
-      semantic_voxels_per_side_inv_(),
-      semantic_block_size_inv_() {
+      num_labels(semantic_config.semantic_label_to_color_->getNumLabels()) {
   setSemanticLayer(semantic_layer);
   CHECK_NOTNULL(semantic_layer_);
   setSemanticProbabilities();
@@ -95,30 +85,27 @@ void SemanticIntegratorBase::setSemanticProbabilities() {
       semantic_config_.semantic_measurement_probability_;
   SemanticProbability non_match_probability =
       1.0f - semantic_config_.semantic_measurement_probability_;
+
   CHECK_GT(match_probability, 0.0);
   CHECK_GT(non_match_probability, 0.0);
   CHECK_LT(match_probability, 1.0);
   CHECK_LT(non_match_probability, 1.0);
-  log_match_probability_ = std::log(match_probability);
-  log_non_match_probability_ = std::log(non_match_probability);
-  CHECK_GT(log_match_probability_, log_non_match_probability_)
+  CHECK_GT(match_probability, non_match_probability)
       << "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_);
+
+  log_match_probability_ = std::log(match_probability);
+  log_non_match_probability_ = std::log(non_match_probability);
+  log_init_probablility =
+      std::log(1.0 / static_cast<SemanticProbability>(num_labels));
+
+  semantic_log_likelihood_ = semantic_log_likelihood_.Constant(
+      num_labels, num_labels, log_non_match_probability_);
   semantic_log_likelihood_.diagonal() =
-      semantic_log_likelihood_.diagonal().Constant(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);
-  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_log_likelihood_.diagonal().Constant(num_labels,
+                                                   log_match_probability_);
+
   // Set the likelihood row for unknown label to 0, so to avoid integrating
   // our knowledge that the voxel is unknown.
   // Log(0) is -inf.
@@ -135,7 +122,7 @@ SemanticProbability SemanticIntegratorBase::computeMeasurementProbability(
 
 void SemanticIntegratorBase::updateSemanticVoxel(
     const vxb::GlobalIndex& global_voxel_idx,
-    const SemanticProbabilities& measurement_frequencies,
+    const std::map<SemanticLabel, size_t>& measurement_frequencies,
     Mutexes* mutexes,
     vxb::TsdfVoxel* tsdf_voxel,
     SemanticVoxel* semantic_voxel) {
@@ -156,7 +143,15 @@ void SemanticIntegratorBase::updateSemanticVoxel(
   //  if (std::abs(sdf) < config_.default_truncation_distance) {
   // Do the same for semantic updates:
   // Don't do it bcs of copyrights... :'(
+
   ////// Here is the actual logic of Kimera-Semantics:   /////////////////////
+  // Intialize new voxels to uniform probability
+  if (semantic_voxel->empty) {
+    semantic_voxel->semantic_priors =
+        SemanticProbabilities::Constant(num_labels, 1, log_init_probablility);
+    semantic_voxel->empty = false;
+  }
+
   // Calculate new probabilities given the measurement frequencies.
   updateSemanticVoxelProbabilities(measurement_frequencies,
                                    &semantic_voxel->semantic_priors);
@@ -281,36 +276,36 @@ void SemanticIntegratorBase::updateSemanticLayerWithStoredBlocks() {
  **/
 // TODO(Toni): Unit Test this function!
 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_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_GE(measurement_frequencies.sum(), 1.0)
+    const LabelFrequencyMap& obs,
+    SemanticProbabilities* prior) const {
+  DCHECK(prior != nullptr);
+  DCHECK_GT(prior->size(), 0);
+  DCHECK_LE((*prior)[0], 0.0);
+  DCHECK(std::isfinite((*prior)[0]));
+  DCHECK(!prior->hasNaN());
+  DCHECK(!obs.empty())
       << "We should at least have one measurement when calling this "
          "function.";
 
   // Check prior is normalized!
   // static constexpr bool kDebug = true;
   // if (kDebug) {
-  //  CHECK_NEAR(semantic_prior_probability->sum(), 1.0, vxb::kFloatEpsilon);
+  //  CHECK_NEAR(prior->sum(), 1.0, vxb::kFloatEpsilon);
   //}
 
   // A. Pre-multiply each column of
   // likelihood matrix with corresponding measurement frequency.
   // B. Compute posterior probabilities:
   // Post_i = (likelihood * meas freqs)_i + prior_i;
-  *semantic_prior_probability +=
-      semantic_log_likelihood_ * measurement_frequencies;
-  DCHECK(!semantic_prior_probability->hasNaN());
+  for (auto&& [label, count] : obs) {
+    *prior += count * semantic_log_likelihood_.col(label);
+  }
+  DCHECK(!prior->hasNaN());
 
   // Normalize posterior probability.
   // TODO(Toni): no need to normalize all the time unless someone asks
   // for meaningful probabilities?
-  // normalizeProbabilities(semantic_prior_probability);
+  // normalizeProbabilities(prior);
 }
 
 // THREAD SAFE
@@ -331,13 +326,10 @@ void SemanticIntegratorBase::normalizeProbabilities(
   if (normalization_factor != 0.0) {
     unnormalized_probs->normalize();
   } else {
-    CHECK_EQ(unnormalized_probs->size(), kTotalNumberOfLabels);
-    static const SemanticProbability kUniformLogProbability =
-        std::log(1 / kTotalNumberOfLabels);
     LOG(WARNING) << "Normalization Factor is " << normalization_factor
                  << ", all values are 0. Normalizing to log(1/n) = "
-                 << kUniformLogProbability;
-    unnormalized_probs->setConstant(kUniformLogProbability);
+                 << log_init_probablility;
+    unnormalized_probs->setConstant(log_init_probablility);
   }
 
   // TODO(Toni): REMOVE