Add laser rangefinder measurement decimation

Controlled by the laser_decimation_factor setting in the trajectory builder lua dictionary.
cartographer-project · Mar 29, 2017 · 07fd408 · 07fd408
1 parent 1ce97b8
commit 07fd408
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 1 deletion.
diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc
@@ -24,7 +24,8 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
     SparsePoseGraph* sparse_pose_graph)
     : options_(options),
       sparse_pose_graph_(sparse_pose_graph),
-      local_trajectory_builder_(options) {}
+      local_trajectory_builder_(options),
+      laser_decimation_counter_(0) {}
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
@@ -35,6 +36,17 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const {
 void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {
+  // TODO average laser scans instead of discarding them?
+  if (options_.laser_decimation_factor() > 1) {
+    laser_decimation_counter_ += 1;
+    if (laser_decimation_counter_ < options_.laser_decimation_factor()) {
+      return;
+    }
+    else {
+      laser_decimation_counter_ -= options_.laser_decimation_factor();
+    }
+  }
+
   std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
       local_trajectory_builder_.AddHorizontalRangeData(
           time, sensor::RangeData{origin, ranges, {}, {}});

diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h
@@ -51,6 +51,8 @@ class GlobalTrajectoryBuilder
   const proto::LocalTrajectoryBuilderOptions options_;
   SparsePoseGraph* const sparse_pose_graph_;
   LocalTrajectoryBuilder local_trajectory_builder_;
+
+  double laser_decimation_counter_;
 };
 
 }  // namespace mapping_2d

diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc
@@ -37,6 +37,9 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
       parameter_dictionary->GetDouble("laser_missing_echo_ray_length"));
   options.set_laser_voxel_filter_size(
       parameter_dictionary->GetDouble("laser_voxel_filter_size"));
+  options.set_laser_decimation_factor(
+      parameter_dictionary->GetDouble("laser_decimation_factor"));
+  CHECK_GE(options.laser_decimation_factor(), 1.);
   options.set_use_online_correlative_scan_matching(
       parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
   *options.mutable_adaptive_voxel_filter_options() =

diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
@@ -28,6 +28,7 @@ message LocalTrajectoryBuilderOptions {
   optional float laser_max_range = 15;
   optional float laser_min_z = 1;
   optional float laser_max_z = 2;
+  optional double laser_decimation_factor = 19;
 
   // Laser returns beyond 'laser_max_range' will be inserted with this length as
   // empty space.

diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua
@@ -20,6 +20,7 @@ TRAJECTORY_BUILDER_2D = {
   laser_max_z = 2.,
   laser_missing_echo_ray_length = 5.,
   laser_voxel_filter_size = 0.025,
+  laser_decimation_factor = 1.,
 
   use_online_correlative_scan_matching = false,
   adaptive_voxel_filter = {