From 18bc3af60f0d6c6a3d00dd248cfb1d81eaa7ad07 Mon Sep 17 00:00:00 2001 From: RozDavid Date: Wed, 3 Feb 2021 14:31:01 +0100 Subject: [PATCH 1/5] Dynamic number of layers from ROS param server --- .gitignore | 1 + .../include/kimera_semantics/common.h | 9 ++--- .../semantic_integrator_base.h | 5 +++ .../include/kimera_semantics/semantic_voxel.h | 32 ++++++++-------- .../src/semantic_integrator_base.cpp | 37 +++++++++++++------ .../src/semantic_tsdf_integrator_fast.cpp | 2 +- .../src/semantic_tsdf_integrator_merged.cpp | 2 +- kimera_semantics_ros/src/ros_params.cpp | 6 +++ 8 files changed, 60 insertions(+), 34 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3f5dcf4 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +cmake-build-debug/ 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..d774d88 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_layers = 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..7d30719 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_layers, + // should be std::log(1/total_number_of_layers) + size_t total_number_of_layers; -} // namespace kimera + // SemanticProbabilities::Constant(std::log(1 / total_number_of_layers)); + 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/semantic_integrator_base.cpp b/kimera_semantics/src/semantic_integrator_base.cpp index f579656..8541df2 100644 --- a/kimera_semantics/src/semantic_integrator_base.cpp +++ b/kimera_semantics/src/semantic_integrator_base.cpp @@ -105,19 +105,22 @@ 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_.resize(semantic_config_.total_number_of_layers, semantic_config_.total_number_of_layers); semantic_log_likelihood_ = - semantic_log_likelihood_.Constant(log_non_match_probability_); + 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_, + semantic_config_.total_number_of_layers * 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_, + semantic_config_.total_number_of_layers * log_match_probability_ + + std::pow(semantic_config_.total_number_of_layers, 2) * log_non_match_probability_ - + semantic_config_.total_number_of_layers * log_non_match_probability_, 1000 * vxb::kFloatEpsilon); } @@ -198,14 +201,20 @@ 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); DCHECK(last_block_idx != nullptr); + SemanticVoxel semanticVoxel; + const vxb::BlockIndex& block_idx = vxb::getBlockIndexFromGlobalVoxelIndex( global_voxel_idx, semantic_voxels_per_side_inv_); + const vxb::VoxelIndex local_voxel_idx = vxb::getLocalFromGlobalVoxelIndex( + global_voxel_idx, semantic_voxels_per_side_); + if ((block_idx != *last_block_idx) || (*last_block == nullptr)) { *last_block = semantic_layer_->getBlockPtrByIndex(block_idx); *last_block_idx = block_idx; @@ -222,6 +231,7 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( temp_semantic_block_map_.find(block_idx); if (it != temp_semantic_block_map_.end()) { *last_block = it->second; + semanticVoxel = (*last_block)->getVoxelByVoxelIndex(local_voxel_idx); } else { auto insert_status = temp_semantic_block_map_.emplace( block_idx, @@ -235,16 +245,19 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( << block_idx.transpose(); *last_block = insert_status.first->second; + semanticVoxel = (*last_block)->getVoxelByVoxelIndex(local_voxel_idx); + semanticVoxel.total_number_of_layers = total_number_of_labels; + + SemanticProbabilities sem_probs; + sem_probs.resize(total_number_of_labels,1); + sem_probs.setConstant(std::log(1.0/float(total_number_of_labels))); } } // 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_); - - return &((*last_block)->getVoxelByVoxelIndex(local_voxel_idx)); + return &(semanticVoxel); } // NOT THREAD SAFE @@ -278,11 +291,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_layers); 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_layers); DCHECK_GE(measurement_frequencies.sum(), 1.0) << "We should at least have one measurement when calling this " "function."; diff --git a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp index a27e994..6602129 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp @@ -128,7 +128,7 @@ 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); + global_voxel_idx, semantic_config_.total_number_of_layers, &semantic_block, &semantic_block_idx); SemanticProbabilities semantic_label_frequencies = SemanticProbabilities::Zero(); CHECK_LT(semantic_label, semantic_label_frequencies.size()); diff --git a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp index db1bc56..7a83917 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp @@ -319,7 +319,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_layers, &semantic_block, &semantic_block_idx); updateSemanticVoxel(global_voxel_idx, semantic_label_frequencies, &mutexes_, diff --git a/kimera_semantics_ros/src/ros_params.cpp b/kimera_semantics_ros/src/ros_params.cpp index 133640b..0c46d08 100644 --- a/kimera_semantics_ros/src/ros_params.cpp +++ b/kimera_semantics_ros/src/ros_params.cpp @@ -48,6 +48,12 @@ 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_layers = semantic_config.total_number_of_layers; + nh_private.param("total_number_of_layers", + total_number_of_layers, + total_number_of_layers); + // Get semantic color mode std::string color_mode = "color"; nh_private.param("semantic_color_mode", color_mode, color_mode); From 90b2954b33835e228f8f2e7ceda923dc096d6e67 Mon Sep 17 00:00:00 2001 From: RozDavid Date: Sat, 6 Feb 2021 22:50:25 +0100 Subject: [PATCH 2/5] Dynamic Labels working --- .../include/kimera_semantics/color.h | 6 ++- .../semantic_integrator_base.h | 2 +- .../include/kimera_semantics/semantic_voxel.h | 6 +-- kimera_semantics/src/color.cpp | 1 + .../src/semantic_integrator_base.cpp | 46 +++++++++++-------- .../src/semantic_tsdf_integrator_fast.cpp | 10 ++-- .../src/semantic_tsdf_integrator_merged.cpp | 7 +-- kimera_semantics_ros/src/ros_params.cpp | 9 ++-- 8 files changed, 52 insertions(+), 35 deletions(-) diff --git a/kimera_semantics/include/kimera_semantics/color.h b/kimera_semantics/include/kimera_semantics/color.h index 5f8c4f1..e3cedd9 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/semantic_integrator_base.h b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h index d774d88..9cd0ade 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h +++ b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h @@ -78,7 +78,7 @@ class SemanticIntegratorBase { //The total number of labels in the segmenation node //Used by the confidence matrices - size_t total_number_of_layers = 21; + size_t total_number_of_labels = 21; /// How to color the semantic mesh. ColorMode color_mode = ColorMode::kSemantic; diff --git a/kimera_semantics/include/kimera_semantics/semantic_voxel.h b/kimera_semantics/include/kimera_semantics/semantic_voxel.h index 7d30719..30825af 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_voxel.h +++ b/kimera_semantics/include/kimera_semantics/semantic_voxel.h @@ -16,11 +16,11 @@ namespace kimera { // 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_layers, - // should be std::log(1/total_number_of_layers) + // 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; - // SemanticProbabilities::Constant(std::log(1 / total_number_of_layers)); + // SemanticProbabilities::Constant(std::log(1 / total_number_of_labels)); SemanticProbabilities semantic_priors; // Initialize voxel with gray color diff --git a/kimera_semantics/src/color.cpp b/kimera_semantics/src/color.cpp index 04069eb..2ca145f 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 8541df2..a3f6e45 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,23 +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_.resize(semantic_config_.total_number_of_layers, semantic_config_.total_number_of_layers); - semantic_log_likelihood_ = - semantic_log_likelihood_.setConstant(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().setConstant(log_match_probability_); + // TODO(Toni): sanity checks, set as DCHECK_EQ. CHECK_NEAR(semantic_log_likelihood_.diagonal().sum(), - semantic_config_.total_number_of_layers * log_match_probability_, - 100 * vxb::kFloatEpsilon); + semantic_config_.total_number_of_labels * log_match_probability_, + 10e-2); CHECK_NEAR( semantic_log_likelihood_.sum(), - semantic_config_.total_number_of_layers * log_match_probability_ + - std::pow(semantic_config_.total_number_of_layers, 2) * log_non_match_probability_ - - semantic_config_.total_number_of_layers * 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!! @@ -158,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) { @@ -188,7 +192,8 @@ void SemanticIntegratorBase::updateSemanticVoxel( } //////////////////////////////////////////////////////////////////////////// //} -} + + } // Will return a pointer to a voxel located at global_voxel_idx in the tsdf // layer. Thread safe. @@ -207,8 +212,6 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( DCHECK(last_block != nullptr); DCHECK(last_block_idx != nullptr); - SemanticVoxel semanticVoxel; - const vxb::BlockIndex& block_idx = vxb::getBlockIndexFromGlobalVoxelIndex( global_voxel_idx, semantic_voxels_per_side_inv_); @@ -231,7 +234,6 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( temp_semantic_block_map_.find(block_idx); if (it != temp_semantic_block_map_.end()) { *last_block = it->second; - semanticVoxel = (*last_block)->getVoxelByVoxelIndex(local_voxel_idx); } else { auto insert_status = temp_semantic_block_map_.emplace( block_idx, @@ -245,19 +247,23 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( << block_idx.transpose(); *last_block = insert_status.first->second; - semanticVoxel = (*last_block)->getVoxelByVoxelIndex(local_voxel_idx); - semanticVoxel.total_number_of_layers = total_number_of_labels; 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; + } + } } // Only used if someone calls the getAllUpdatedBlocks I believe. (*last_block)->updated() = true; - return &(semanticVoxel); + return &((*last_block)->getVoxelByVoxelIndex(local_voxel_idx)); } // NOT THREAD SAFE @@ -291,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(), semantic_config_.total_number_of_layers); + 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(), semantic_config_.total_number_of_layers); + 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."; @@ -338,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 / 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 6602129..34d7cbf 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_config_.total_number_of_layers, &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 7a83917..aa2727f 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_config_.total_number_of_layers, &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/src/ros_params.cpp b/kimera_semantics_ros/src/ros_params.cpp index 0c46d08..a880548 100644 --- a/kimera_semantics_ros/src/ros_params.cpp +++ b/kimera_semantics_ros/src/ros_params.cpp @@ -49,10 +49,11 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) { static_cast(semantic_measurement_probability); // Get the total number of labels of the prediction - int total_number_of_layers = semantic_config.total_number_of_layers; - nh_private.param("total_number_of_layers", - total_number_of_layers, - total_number_of_layers); + 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"; From da793b8d5497ed56813b23eb53ed450409584c88 Mon Sep 17 00:00:00 2001 From: RozDavid Date: Mon, 8 Feb 2021 16:10:46 +0100 Subject: [PATCH 3/5] Launch file update for demo --- kimera_semantics_ros/launch/kimera_semantics.launch | 1 + 1 file changed, 1 insertion(+) 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 @@ + From f71dc48d2aa6936ea9de5524e67ceda86aad95a4 Mon Sep 17 00:00:00 2001 From: RozDavid Date: Tue, 9 Feb 2021 18:44:22 +0100 Subject: [PATCH 4/5] Pull request fixes --- .gitignore | 1 - .../include/kimera_semantics/color.h | 2 +- .../semantic_integrator_base.h | 2 +- .../include/kimera_semantics/semantic_voxel.h | 8 ++++---- kimera_semantics/src/color.cpp | 2 +- .../src/semantic_integrator_base.cpp | 20 +++++++++---------- .../src/semantic_tsdf_integrator_fast.cpp | 4 ++-- .../src/semantic_tsdf_integrator_merged.cpp | 4 ++-- kimera_semantics_ros/src/ros_params.cpp | 4 ++-- 9 files changed, 23 insertions(+), 24 deletions(-) delete mode 100644 .gitignore diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 3f5dcf4..0000000 --- a/.gitignore +++ /dev/null @@ -1 +0,0 @@ -cmake-build-debug/ diff --git a/kimera_semantics/include/kimera_semantics/color.h b/kimera_semantics/include/kimera_semantics/color.h index e3cedd9..f2de7c1 100644 --- a/kimera_semantics/include/kimera_semantics/color.h +++ b/kimera_semantics/include/kimera_semantics/color.h @@ -54,7 +54,7 @@ class SemanticLabel2Color { ColorToSemanticLabelMap color_to_semantic_label_; SemanticLabelToColorMap semantic_label_to_color_map_; - size_t number_of_colored_labels; + size_t number_of_colored_labels_; }; // Color map from semantic labels to colors. diff --git a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h index 9cd0ade..9da8535 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h +++ b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h @@ -78,7 +78,7 @@ class SemanticIntegratorBase { //The total number of labels in the segmenation node //Used by the confidence matrices - size_t total_number_of_labels = 21; + size_t total_number_of_labels_ = 21; /// How to color the semantic mesh. ColorMode color_mode = ColorMode::kSemantic; diff --git a/kimera_semantics/include/kimera_semantics/semantic_voxel.h b/kimera_semantics/include/kimera_semantics/semantic_voxel.h index 30825af..d55e27b 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_voxel.h +++ b/kimera_semantics/include/kimera_semantics/semantic_voxel.h @@ -16,11 +16,11 @@ namespace kimera { // 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; + // 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_; - // SemanticProbabilities::Constant(std::log(1 / total_number_of_labels)); + // SemanticProbabilities::Constant(std::log(1 / total_number_of_labels_)); SemanticProbabilities semantic_priors; // Initialize voxel with gray color diff --git a/kimera_semantics/src/color.cpp b/kimera_semantics/src/color.cpp index 2ca145f..3a16382 100644 --- a/kimera_semantics/src/color.cpp +++ b/kimera_semantics/src/color.cpp @@ -62,7 +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; + 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 a3f6e45..9c1f2fa 100644 --- a/kimera_semantics/src/semantic_integrator_base.cpp +++ b/kimera_semantics/src/semantic_integrator_base.cpp @@ -106,22 +106,22 @@ 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_.resize(semantic_config_.total_number_of_labels, semantic_config_.total_number_of_labels); + 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().setConstant(log_match_probability_); // TODO(Toni): sanity checks, set as DCHECK_EQ. CHECK_NEAR(semantic_log_likelihood_.diagonal().sum(), - semantic_config_.total_number_of_labels * log_match_probability_, + semantic_config_.total_number_of_labels_ * log_match_probability_, 10e-2); CHECK_NEAR( semantic_log_likelihood_.sum(), - 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_, + 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); } @@ -253,7 +253,7 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( 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).total_number_of_layers_ = total_number_of_labels; (*last_block)->getVoxelByLinearIndex(voxel_index).semantic_priors = sem_probs; } @@ -297,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(), semantic_config_.total_number_of_labels); + 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(), semantic_config_.total_number_of_labels); + 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."; @@ -344,9 +344,9 @@ void SemanticIntegratorBase::normalizeProbabilities( if (normalization_factor != 0.0) { unnormalized_probs->normalize(); } else { - CHECK_EQ(unnormalized_probs->size(), semantic_config_.total_number_of_labels); + CHECK_EQ(unnormalized_probs->size(), semantic_config_.total_number_of_labels_); static const SemanticProbability kUniformLogProbability = - std::log(1 / semantic_config_.total_number_of_labels); + std::log(1 / 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 34d7cbf..040fda9 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp @@ -129,10 +129,10 @@ void FastSemanticTsdfIntegrator::integrateSemanticFunction( updateTsdfVoxel(origin, point_G, global_voxel_idx, color, weight, voxel); SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr( - global_voxel_idx, semantic_config_.total_number_of_labels, &semantic_block, &semantic_block_idx); + 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.resize(semantic_config_.total_number_of_labels_, 1); semantic_label_frequencies.setZero(); CHECK_LT(semantic_label, semantic_label_frequencies.size()); diff --git a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp index aa2727f..c1f1ee0 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp @@ -252,7 +252,7 @@ void MergedSemanticTsdfIntegrator::integrateVoxel( // Calculate semantic labels frequencies to encode likelihood function. // Prefill with 0 frequency. SemanticProbabilities semantic_label_frequencies; - semantic_label_frequencies.resize(semantic_config_.total_number_of_labels, 1); + semantic_label_frequencies.resize(semantic_config_.total_number_of_labels_, 1); semantic_label_frequencies.setZero(); // Loop over all point indices inside current voxel. @@ -320,7 +320,7 @@ void MergedSemanticTsdfIntegrator::integrateVoxel( merged_weight, voxel); SemanticVoxel* semantic_voxel = - allocateStorageAndGetSemanticVoxelPtr(global_voxel_idx, semantic_config_.total_number_of_labels, &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/src/ros_params.cpp b/kimera_semantics_ros/src/ros_params.cpp index a880548..51285e5 100644 --- a/kimera_semantics_ros/src/ros_params.cpp +++ b/kimera_semantics_ros/src/ros_params.cpp @@ -49,11 +49,11 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) { 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); + 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); + semantic_config.total_number_of_labels_ = static_cast(total_number_of_labels); // Get semantic color mode std::string color_mode = "color"; From fc73d4165de3e6c557adb4173db11fee0f5c751d Mon Sep 17 00:00:00 2001 From: RozDavid Date: Tue, 9 Feb 2021 19:16:09 +0100 Subject: [PATCH 5/5] Pull request fixes part 2 --- kimera_semantics/src/semantic_integrator_base.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/kimera_semantics/src/semantic_integrator_base.cpp b/kimera_semantics/src/semantic_integrator_base.cpp index 9c1f2fa..e47f041 100644 --- a/kimera_semantics/src/semantic_integrator_base.cpp +++ b/kimera_semantics/src/semantic_integrator_base.cpp @@ -215,9 +215,6 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( const vxb::BlockIndex& block_idx = vxb::getBlockIndexFromGlobalVoxelIndex( global_voxel_idx, semantic_voxels_per_side_inv_); - const vxb::VoxelIndex local_voxel_idx = vxb::getLocalFromGlobalVoxelIndex( - global_voxel_idx, semantic_voxels_per_side_); - if ((block_idx != *last_block_idx) || (*last_block == nullptr)) { *last_block = semantic_layer_->getBlockPtrByIndex(block_idx); *last_block_idx = block_idx; @@ -250,7 +247,7 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( SemanticProbabilities sem_probs; sem_probs.resize(total_number_of_labels,1); - sem_probs.setConstant(std::log(1.0/float(total_number_of_labels))); + 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; @@ -263,6 +260,9 @@ SemanticVoxel* SemanticIntegratorBase::allocateStorageAndGetSemanticVoxelPtr( // 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_); + return &((*last_block)->getVoxelByVoxelIndex(local_voxel_idx)); } @@ -346,7 +346,7 @@ void SemanticIntegratorBase::normalizeProbabilities( } else { CHECK_EQ(unnormalized_probs->size(), semantic_config_.total_number_of_labels_); static const SemanticProbability kUniformLogProbability = - std::log(1 / semantic_config_.total_number_of_labels_); + 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;