diff --git a/kimera_semantics/CMakeLists.txt b/kimera_semantics/CMakeLists.txt index fb5f28f..30359e4 100644 --- a/kimera_semantics/CMakeLists.txt +++ b/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() diff --git a/kimera_semantics/include/kimera_semantics/color.h b/kimera_semantics/include/kimera_semantics/color.h index 1d7c715..008cbf3 100644 --- a/kimera_semantics/include/kimera_semantics/color.h +++ b/kimera_semantics/include/kimera_semantics/color.h @@ -36,7 +36,6 @@ typedef std::unordered_map typedef std::unordered_map 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::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 diff --git a/kimera_semantics/include/kimera_semantics/common.h b/kimera_semantics/include/kimera_semantics/common.h index 606f502..706a2d0 100644 --- a/kimera_semantics/include/kimera_semantics/common.h +++ b/kimera_semantics/include/kimera_semantics/common.h @@ -14,34 +14,24 @@ namespace kimera { namespace vxb = voxblox; -typedef uint8_t SemanticLabel; -typedef vxb::AlignedVector 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 SemanticLabels; + 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 - SemanticLikelihoodFunction; +typedef Eigen::Matrix + SemanticLikelihoodFunction; typedef vxb::LongIndexHashMapType>::type VoxelMap; typedef VoxelMap::value_type VoxelMapElement; -// Add compatibility for c++11's lack of make_unique. -template -std::unique_ptr make_unique(Args&&... args) { - return std::unique_ptr(new T(std::forward(args)...)); -} - - } // namespace kimera diff --git a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h b/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h index d203238..79b0c11 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_integrator_base.h +++ b/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 Ptr; - typedef vxb:: - ApproxHashArray<12, std::mutex, vxb::GlobalIndex, vxb::LongIndexHash> - Mutexes; + using Ptr = std::shared_ptr; + using Mutexes = vxb:: + ApproxHashArray<12, std::mutex, vxb::GlobalIndex, vxb::LongIndexHash>; + using LabelFrequencyMap = std::map; 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::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 diff --git a/kimera_semantics/include/kimera_semantics/semantic_voxel.h b/kimera_semantics/include/kimera_semantics/semantic_voxel.h index 7454539..3de4a99 100644 --- a/kimera_semantics/include/kimera_semantics/semantic_voxel.h +++ b/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 diff --git a/kimera_semantics/src/color.cpp b/kimera_semantics/src/color.cpp index c7d79db..10cbf0f 100644 --- a/kimera_semantics/src/color.cpp +++ b/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 diff --git a/kimera_semantics/src/semantic_integrator_base.cpp b/kimera_semantics/src/semantic_integrator_base.cpp index 6b43a8f..b42c763 100644 --- a/kimera_semantics/src/semantic_integrator_base.cpp +++ b/kimera_semantics/src/semantic_integrator_base.cpp @@ -59,17 +59,7 @@ SemanticIntegratorBase::SemanticIntegratorBase( vxb::Layer* 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(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& 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 diff --git a/kimera_semantics/src/semantic_tsdf_integrator_factory.cpp b/kimera_semantics/src/semantic_tsdf_integrator_factory.cpp index dabf6b6..a4e8fcb 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_factory.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_factory.cpp @@ -18,14 +18,15 @@ // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** * @file semantic_tsdf_integrator_factory.cpp @@ -40,8 +41,7 @@ namespace kimera { -std::unique_ptr -SemanticTsdfIntegratorFactory::create( +std::unique_ptr SemanticTsdfIntegratorFactory::create( const std::string& integrator_type_name, const vxb::TsdfIntegratorBase::Config& config, const SemanticIntegratorBase::SemanticConfig& semantic_config, @@ -54,7 +54,10 @@ SemanticTsdfIntegratorFactory::create( kSemanticTsdfIntegratorTypeNames) { if (integrator_type_name == valid_integrator_type_name) { return create(static_cast(integrator_type), - config, semantic_config, tsdf_layer, semantic_layer); + config, + semantic_config, + tsdf_layer, + semantic_layer); } ++integrator_type; } @@ -62,8 +65,7 @@ SemanticTsdfIntegratorFactory::create( return nullptr; } -std::unique_ptr -SemanticTsdfIntegratorFactory::create( +std::unique_ptr SemanticTsdfIntegratorFactory::create( const SemanticTsdfIntegratorType& integrator_type, const vxb::TsdfIntegratorBase::Config& config, const SemanticIntegratorBase::SemanticConfig& semantic_config, @@ -71,18 +73,18 @@ SemanticTsdfIntegratorFactory::create( vxb::Layer* semantic_layer) { CHECK_NOTNULL(tsdf_layer); switch (integrator_type) { - case SemanticTsdfIntegratorType::kFast: - return kimera::make_unique( + case SemanticTsdfIntegratorType::kFast: + return std::make_unique( config, semantic_config, tsdf_layer, semantic_layer); - break; - case SemanticTsdfIntegratorType::kMerged: - return kimera::make_unique( + break; + case SemanticTsdfIntegratorType::kMerged: + return std::make_unique( config, semantic_config, tsdf_layer, semantic_layer); - break; - default: - LOG(FATAL) << "Unknown Semantic/TSDF integrator type: " - << static_cast(integrator_type); - break; + break; + default: + LOG(FATAL) << "Unknown Semantic/TSDF integrator type: " + << static_cast(integrator_type); + break; } return nullptr; } diff --git a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp index a27e994..6f31d46 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_fast.cpp @@ -129,12 +129,8 @@ void FastSemanticTsdfIntegrator::integrateSemanticFunction( SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr( global_voxel_idx, &semantic_block, &semantic_block_idx); - SemanticProbabilities semantic_label_frequencies = - SemanticProbabilities::Zero(); - CHECK_LT(semantic_label, semantic_label_frequencies.size()); - semantic_label_frequencies[semantic_label] += 1.0f; updateSemanticVoxel(global_voxel_idx, - semantic_label_frequencies, + {{semantic_label, 1}}, &mutexes_, voxel, semantic_voxel); diff --git a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp index 1915d5e..27ac021 100644 --- a/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp +++ b/kimera_semantics/src/semantic_tsdf_integrator_merged.cpp @@ -249,10 +249,7 @@ void MergedSemanticTsdfIntegrator::integrateVoxel( HashableColor merged_color; vxb::Point merged_point_C = vxb::Point::Zero(); vxb::FloatingPoint merged_weight = 0.0f; - // Calculate semantic labels frequencies to encode likelihood function. - // Prefill with 0 frequency. - SemanticProbabilities semantic_label_frequencies = - SemanticProbabilities::Zero(); + std::map label_frequencies; // Loop over all point indices inside current voxel. // Generate merged values to propagate to other voxels: @@ -274,9 +271,12 @@ void MergedSemanticTsdfIntegrator::integrateVoxel( merged_color, merged_weight, color, point_weight); merged_weight += point_weight; - const SemanticLabel& semantic_label = semantic_labels[pt_idx]; - CHECK_LT(semantic_label, semantic_label_frequencies.size()); - semantic_label_frequencies[semantic_label] += 1.0f; + const SemanticLabel& label = semantic_labels[pt_idx]; + auto iter = label_frequencies.find(label); + if (iter == label_frequencies.end()) { + iter = label_frequencies.emplace(label, 0).first; + } + iter->second += 1; // only take first point when clearing if (clearing_ray) { @@ -321,7 +321,7 @@ void MergedSemanticTsdfIntegrator::integrateVoxel( SemanticVoxel* semantic_voxel = allocateStorageAndGetSemanticVoxelPtr(global_voxel_idx, &semantic_block, &semantic_block_idx); updateSemanticVoxel(global_voxel_idx, - semantic_label_frequencies, + label_frequencies, &mutexes_, voxel, semantic_voxel); diff --git a/kimera_semantics_ros/CMakeLists.txt b/kimera_semantics_ros/CMakeLists.txt index 4b02142..363fe19 100644 --- a/kimera_semantics_ros/CMakeLists.txt +++ b/kimera_semantics_ros/CMakeLists.txt @@ -1,6 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1) project(kimera_semantics_ros) +SET(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + find_package(catkin_simple REQUIRED) catkin_simple() diff --git a/kimera_semantics_ros/cfg/tesse_multiscene_office1_segmentation_mapping.csv b/kimera_semantics_ros/cfg/tesse_multiscene_office1_segmentation_mapping.csv index 50e4ea1..6c36ed2 100644 --- a/kimera_semantics_ros/cfg/tesse_multiscene_office1_segmentation_mapping.csv +++ b/kimera_semantics_ros/cfg/tesse_multiscene_office1_segmentation_mapping.csv @@ -202,3 +202,4 @@ WindowBlinds_A,32,32,32,255,0 WindowBlinds_B,32,32,32,255,0 WindowBlinds_Closed,32,32,32,255,0 WindowBlinds_D,32,32,32,255,0 +BadLabel,125,218,3,255,0 diff --git a/kimera_semantics_ros/src/kimera_semantics_rosbag.cpp b/kimera_semantics_ros/src/kimera_semantics_rosbag.cpp index 024c6c2..9def76d 100644 --- a/kimera_semantics_ros/src/kimera_semantics_rosbag.cpp +++ b/kimera_semantics_ros/src/kimera_semantics_rosbag.cpp @@ -27,12 +27,12 @@ int main(int argc, char** argv) { ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - ros::Publisher depth_img_pub = nh.advertise( - "depth_img", 10, true); - ros::Publisher semantic_img_pub = nh.advertise( - "semantic_img", 10, true); - ros::Publisher rgb_img_pub = nh.advertise( - "rgb_img", 10, true); + ros::Publisher depth_img_pub = + nh.advertise("depth_img", 10, true); + ros::Publisher semantic_img_pub = + nh.advertise("semantic_img", 10, true); + ros::Publisher rgb_img_pub = + nh.advertise("rgb_img", 10, true); std::string depth_cam_frame_id_; std::string base_link_frame_id_; @@ -49,10 +49,9 @@ int main(int argc, char** argv) { CHECK(nh_private.getParam("metric_semantic_reconstruction", metric_semantic_reconstruction)); if (metric_semantic_reconstruction) { - tsdf_server = - kimera::make_unique(nh, nh_private); + tsdf_server = std::make_unique(nh, nh_private); } else { - tsdf_server = kimera::make_unique(nh, nh_private); + tsdf_server = std::make_unique(nh, nh_private); } kimera::RosbagDataProvider rosbag; @@ -155,10 +154,9 @@ int main(int argc, char** argv) { voxblox::EsdfServer esdf_server(nh, nh_private); esdf_server.loadMap(tsdf_esdf_filename); esdf_server.disableIncrementalUpdate(); - if (kGenerateEsdf || - esdf_server.getEsdfMapPtr() - ->getEsdfLayerPtr() - ->getNumberOfAllocatedBlocks() == 0) { + if (kGenerateEsdf || esdf_server.getEsdfMapPtr() + ->getEsdfLayerPtr() + ->getNumberOfAllocatedBlocks() == 0) { static constexpr bool kFullEuclideanDistance = true; esdf_server.updateEsdfBatch(kFullEuclideanDistance); } diff --git a/kimera_semantics_ros/src/ros_params.cpp b/kimera_semantics_ros/src/ros_params.cpp index 133640b..f50c4b9 100644 --- a/kimera_semantics_ros/src/ros_params.cpp +++ b/kimera_semantics_ros/src/ros_params.cpp @@ -17,14 +17,13 @@ namespace kimera { -std::string -getSemanticTsdfIntegratorTypeFromRosParam(const ros::NodeHandle& nh_private) { +std::string getSemanticTsdfIntegratorTypeFromRosParam( + const ros::NodeHandle& nh_private) { // Get semantic tsdf integrator type, by default using "fast" // (could be "merged"). std::string semantic_tsdf_integrator_type = "fast"; - nh_private.param("method", - semantic_tsdf_integrator_type, - semantic_tsdf_integrator_type); + nh_private.param( + "method", semantic_tsdf_integrator_type, semantic_tsdf_integrator_type); return semantic_tsdf_integrator_type; } @@ -63,7 +62,7 @@ getSemanticTsdfIntegratorConfigFromRosParam(const ros::NodeHandle& nh_private) { // Get semantic map semantic_config.semantic_label_to_color_ = - kimera::make_unique( + std::make_unique( getSemanticLabelToColorCsvFilepathFromRosParam(nh_private)); std::vector dynamic_labels; diff --git a/kimera_semantics_ros/src/rosbag_data_provider.cpp b/kimera_semantics_ros/src/rosbag_data_provider.cpp index 71c6033..a3b2867 100644 --- a/kimera_semantics_ros/src/rosbag_data_provider.cpp +++ b/kimera_semantics_ros/src/rosbag_data_provider.cpp @@ -4,15 +4,15 @@ * @author Antoni Rosinol */ -#include "kimera_semantics/common.h" #include "kimera_semantics_ros/rosbag_data_provider.h" +#include "kimera_semantics/common.h" #include #include -#include #include +#include #include #include @@ -69,8 +69,7 @@ RosbagDataProvider::RosbagDataProvider() nh_.advertise(depth_imgs_topic_, kQueueSize); semantic_img_pub_ = nh_.advertise(semantic_imgs_topic_, kQueueSize); - rgb_img_pub_ = - nh_.advertise(rgb_imgs_topic_, kQueueSize); + rgb_img_pub_ = nh_.advertise(rgb_imgs_topic_, kQueueSize); } void RosbagDataProvider::initialize() { @@ -83,7 +82,6 @@ void RosbagDataProvider::initialize() { bool RosbagDataProvider::parseRosbag(const std::string& bag_path) { LOG(INFO) << "Parsing rosbag data."; - // Fill in rosbag to data_ rosbag::Bag bag; bag.open(bag_path, rosbag::bagmode::Read); @@ -101,7 +99,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path) { rosbag::View view(bag, rosbag::TopicQuery(topics)); ros::Duration rosbag_duration = view.getEndTime() - view.getBeginTime(); - rosbag_data_ = kimera::make_unique(rosbag_duration); + rosbag_data_ = std::make_unique(rosbag_duration); // For some datasets, we have duplicated measurements for the same time. static constexpr bool kEarlyStopForDebug = false; @@ -128,8 +126,7 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path) { if (kEarlyStopForDebug && rosbag_data_->depth_imgs_.size() == rosbag_data_->semantic_imgs_.size() && - rosbag_data_->depth_imgs_.size() == - rosbag_data_->rgb_imgs_.size() && + rosbag_data_->depth_imgs_.size() == rosbag_data_->rgb_imgs_.size() && rosbag_data_->depth_imgs_.size() >= 300u) { LOG(ERROR) << "Early break."; break; @@ -182,8 +179,9 @@ bool RosbagDataProvider::parseRosbag(const std::string& bag_path) { << "No semantic images parsed from rosbag."; LOG_IF(FATAL, rosbag_data_->rgb_imgs_.size() == 0) << "No rgb images parsed from rosbag."; - LOG_IF(FATAL, - rosbag_data_->depth_imgs_.size() != rosbag_data_->semantic_imgs_.size()) + LOG_IF( + FATAL, + rosbag_data_->depth_imgs_.size() != rosbag_data_->semantic_imgs_.size()) << "Unequal number of depth and semantic images."; LOG_IF(FATAL, rosbag_data_->depth_imgs_.size() != rosbag_data_->rgb_imgs_.size()) diff --git a/kimera_semantics_ros/src/semantic_simulation_eval.cpp b/kimera_semantics_ros/src/semantic_simulation_eval.cpp index aedabe8..19d1421 100644 --- a/kimera_semantics_ros/src/semantic_simulation_eval.cpp +++ b/kimera_semantics_ros/src/semantic_simulation_eval.cpp @@ -1,7 +1,7 @@ #include -#include #include +#include #include "kimera_semantics/common.h" #include "kimera_semantics_ros/semantic_simulation_server.h" @@ -15,16 +15,16 @@ class SemanticSimulationServerImpl : public SemanticSimulationServer { void prepareWorld() { CHECK(world_); - world_->addObject(make_unique( + world_->addObject(std::make_unique( vxb::Point(0.0, 0.0, 2.0), 2.0, vxb::Color::Red())); - world_->addObject(make_unique( + world_->addObject(std::make_unique( vxb::Point(-2.0, -4.0, 2.0), vxb::Point(0, 1, 0), vxb::Color::White())); - world_->addObject(make_unique( + world_->addObject(std::make_unique( vxb::Point(4.0, 0.0, 0.0), vxb::Point(-1, 0, 0), vxb::Color::Pink())); - world_->addObject(make_unique( + world_->addObject(std::make_unique( vxb::Point(-4.0, 4.0, 2.0), vxb::Point(4, 4, 4), vxb::Color::Green())); world_->addGroundLevel(0.03); diff --git a/kimera_semantics_ros/src/semantic_simulation_server.cpp b/kimera_semantics_ros/src/semantic_simulation_server.cpp index 6e9a140..a97158c 100644 --- a/kimera_semantics_ros/src/semantic_simulation_server.cpp +++ b/kimera_semantics_ros/src/semantic_simulation_server.cpp @@ -8,7 +8,7 @@ SemanticSimulationServer::SemanticSimulationServer( : vxb::SimulationServer(nh, nh_private), semantic_config_( getSemanticTsdfIntegratorConfigFromRosParam(nh_private)) { - world_ = make_unique(); + world_ = std::make_unique(); semantic_gt_.reset( new vxb::Layer(voxel_size_, voxels_per_side_));