diff --git a/kimera_semantics/include/kimera_semantics/color.h b/kimera_semantics/include/kimera_semantics/color.h index 5f8c4f1..f2de7c1 100644 --- a/kimera_semantics/include/kimera_semantics/color.h +++ b/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 diff --git a/kimera_semantics/include/kimera_semantics/common.h b/kimera_semantics/include/kimera_semantics/common.h index 0a7c461..5dabc3b 100644 --- a/kimera_semantics/include/kimera_semantics/common.h +++ b/kimera_semantics/include/kimera_semantics/common.h @@ -18,17 +18,16 @@ typedef uint8_t SemanticLabel; typedef vxb::AlignedVector 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 +typedef Eigen::Matrix 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 + Matrix SemanticLikelihoodFunction; typedef vxb::LongIndexHashMapType>::type VoxelMap; diff --git a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h index d203238..9da8535 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h +++ b/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::Ptr* last_semantic_block, vxb::BlockIndex* last_block_idx); diff --git a/kimera_semantics/include/kimera_semantics/semantic_voxel.h b/kimera_semantics/include/kimera_semantics/semantic_voxel.h index 7454539..d55e27b 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_voxel.h +++ b/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 \ No newline at end of file diff --git a/kimera_semantics/src/color.cpp b/kimera_semantics/src/color.cpp index 04069eb..3a16382 100644 --- a/kimera_semantics/src/color.cpp +++ b/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( diff --git a/kimera_semantics/src/semantic_integrator_base.cpp b/kimera_semantics/src/semantic_integrator_base.cpp index f579656..e47f041 100644 --- a/kimera_semantics/src/semantic_integrator_base.cpp +++ b/kimera_semantics/src/semantic_integrator_base.cpp @@ -47,6 +47,7 @@ #include #include +#include #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::Ptr* last_block, vxb::BlockIndex* last_block_idx) { DCHECK(last_block != nullptr); @@ -235,6 +244,16 @@ 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_indexgetVoxelByLinearIndex(voxel_index).total_number_of_layers_ = total_number_of_labels; + (*last_block)->getVoxelByLinearIndex(voxel_index).semantic_priors = sem_probs; + } + } } @@ -242,7 +261,7 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( (*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; diff --git a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp index a27e994..040fda9 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp @@ -41,6 +41,7 @@ #include #include +#include #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, diff --git a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp index db1bc56..c1f1ee0 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp +++ b/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_, diff --git a/kimera_semantics_ros/launch/kimera_semantics.launch b/kimera_semantics_ros/launch/kimera_semantics.launch index abd7003..5958aaa 100644 --- a/kimera_semantics_ros/launch/kimera_semantics.launch +++ b/kimera_semantics_ros/launch/kimera_semantics.launch @@ -125,6 +125,7 @@ + diff --git a/kimera_semantics_ros/src/ros_params.cpp b/kimera_semantics_ros/src/ros_params.cpp index 133640b..51285e5 100644 --- a/kimera_semantics_ros/src/ros_params.cpp +++ b/kimera_semantics_ros/src/ros_params.cpp @@ -48,6 +48,13 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) { semantic_config.semantic_measurement_probability_ = static_cast(semantic_measurement_probability); + // Get the total number of labels of the prediction + int total_number_of_labels = static_cast(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(total_number_of_labels); + // Get semantic color mode std::string color_mode = "color"; nh_private.param("semantic_color_mode", color_mode, color_mode);