Metric maps: load insertion options from field 'insertOpts' instead o…

…f 'insertionOptions' for compatibility with all other MRPT maps
MOLAorg · May 10, 2024 · 70d2c3f · 70d2c3f
1 parent bd25f4b
commit 70d2c3f
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 29 deletions.
diff --git a/mola_metric_maps/src/HashedVoxelPointCloud.cpp b/mola_metric_maps/src/HashedVoxelPointCloud.cpp
@@ -67,8 +67,8 @@ void HashedVoxelPointCloud::TMapDefinition::loadFromConfigFile_map_specific(
  const std::string sSectCreation = sectionPrefix + "_creationOpts"s;
  MRPT_LOAD_CONFIG_VAR(voxel_size, float, s, sSectCreation);
 
- ASSERT_(s.sectionExists(sectionPrefix + "_insertionOpts"s));
- insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertionOpts"s);
+ ASSERT_(s.sectionExists(sectionPrefix + "_insertOpts"s));
+ insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertOpts"s);
 
  if (s.sectionExists(sectionPrefix + "_likelihoodOpts"s))
  likelihoodOpts.loadFromConfigFile(
@@ -301,35 +301,35 @@ bool HashedVoxelPointCloud::internal_insertObservation(
  {
  robotPose2D = mrpt::poses::CPose2D(*robotPose);
  robotPose3D = (*robotPose);
-
- if (insertionOptions.remove_voxels_farther_than > 0)
- {
- const int distInGrid = static_cast<int>(std::ceil(
- insertionOptions.remove_voxels_farther_than * voxel_size_inv_));
-
- const auto idxCurObs =
- coordToGlobalIdx(robotPose3D.translation().cast<float>());
-
- for (auto it = voxels_.begin(); it != voxels_.end();)
- {
- // manhattan distance:
- const int dist = mrpt::max3(
- std::abs(it->first.cx - idxCurObs.cx),
- std::abs(it->first.cy - idxCurObs.cy),
- std::abs(it->first.cz - idxCurObs.cz));
-
- if (dist > distInGrid)
- it = voxels_.erase(it);
- else
- ++it;
- }
- }
  }
  else
  {
  // Default values are (0,0,0)
  }
 
+ if (insertionOptions.remove_voxels_farther_than > 0)
+ {
+ const int distInGrid = static_cast<int>(std::ceil(
+ insertionOptions.remove_voxels_farther_than * voxel_size_inv_));
+
+ const auto idxCurObs =
+ coordToGlobalIdx(robotPose3D.translation().cast<float>());
+
+ for (auto it = voxels_.begin(); it != voxels_.end();)
+ {
+ // manhattan distance:
+ const int dist = mrpt::max3(
+ std::abs(it->first.cx - idxCurObs.cx),
+ std::abs(it->first.cy - idxCurObs.cy),
+ std::abs(it->first.cz - idxCurObs.cz));
+
+ if (dist > distInGrid)
+ it = voxels_.erase(it);
+ else
+ ++it;
+ }
+ }
+
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
  /********************************************************************

diff --git a/mola_metric_maps/src/SparseTreesPointCloud.cpp b/mola_metric_maps/src/SparseTreesPointCloud.cpp
@@ -65,8 +65,8 @@ void SparseTreesPointCloud::TMapDefinition::loadFromConfigFile_map_specific(
  const std::string sSectCreation = sectionPrefix + "_creationOpts"s;
  MRPT_LOAD_CONFIG_VAR(grid_size, float, s, sSectCreation);
 
- ASSERT_(s.sectionExists(sectionPrefix + "_insertionOpts"s));
- insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertionOpts"s);
+ ASSERT_(s.sectionExists(sectionPrefix + "_insertOpts"s));
+ insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertOpts"s);
 
  ASSERT_(s.sectionExists(sectionPrefix + "_likelihoodOpts"s));
  likelihoodOpts.loadFromConfigFile(s, sectionPrefix + "_likelihoodOpts"s);

diff --git a/mola_metric_maps/src/SparseVoxelPointCloud.cpp b/mola_metric_maps/src/SparseVoxelPointCloud.cpp
@@ -65,8 +65,8 @@ void SparseVoxelPointCloud::TMapDefinition::loadFromConfigFile_map_specific(
  const std::string sSectCreation = sectionPrefix + "_creationOpts"s;
  MRPT_LOAD_CONFIG_VAR(voxel_size, float, s, sSectCreation);
 
- ASSERT_(s.sectionExists(sectionPrefix + "_insertionOpts"s));
- insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertionOpts"s);
+ ASSERT_(s.sectionExists(sectionPrefix + "_insertOpts"s));
+ insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertOpts"s);
 
  ASSERT_(s.sectionExists(sectionPrefix + "_likelihoodOpts"s));
  likelihoodOpts.loadFromConfigFile(s, sectionPrefix + "_likelihoodOpts"s);