Moves 2D laser options into 2D trajectory builder. (#114)

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -27,8 +27,14 @@ namespace mapping_2d {
 proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::LocalTrajectoryBuilderOptions options;
+  options.set_laser_min_range(
+      parameter_dictionary->GetDouble("laser_min_range"));
+  options.set_laser_max_range(
+      parameter_dictionary->GetDouble("laser_max_range"));
   options.set_laser_min_z(parameter_dictionary->GetDouble("laser_min_z"));
   options.set_laser_max_z(parameter_dictionary->GetDouble("laser_max_z"));
+  options.set_laser_missing_echo_ray_length(
+      parameter_dictionary->GetDouble("laser_missing_echo_ray_length"));
   options.set_laser_voxel_filter_size(
       parameter_dictionary->GetDouble("laser_voxel_filter_size"));
   options.set_use_online_correlative_scan_matching(
@@ -70,17 +76,33 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
 const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
 
-sensor::LaserFan LocalTrajectoryBuilder::BuildCroppedLaserFan(
+sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
     const transform::Rigid3f& tracking_to_tracking_2d,
     const sensor::LaserFan& laser_fan) const {
-  const sensor::LaserFan cropped_fan = sensor::CropLaserFan(
+  // Drop any returns below the minimum range and convert returns beyond the
-      sensor::TransformLaserFan(laser_fan, tracking_to_tracking_2d),
+  // maximum range into misses.
+  sensor::LaserFan returns_and_misses{laser_fan.origin, {}, {}, {}};
+  for (const Eigen::Vector3f& return_ : laser_fan.returns) {
+    const float range = (return_ - laser_fan.origin).norm();
+    if (range >= options_.laser_min_range()) {
+        if (range <= options_.laser_max_range()) {
+          returns_and_misses.returns.push_back(return_);
+        } else {
+          returns_and_misses.misses.push_back(
+              laser_fan.origin +
+              options_.laser_missing_echo_ray_length() *
+                  (return_ - laser_fan.origin).normalized());
+        }
+    }
+  }
+  const sensor::LaserFan cropped = sensor::CropLaserFan(
+      sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d),
       options_.laser_min_z(), options_.laser_max_z());
   return sensor::LaserFan{
-      cropped_fan.origin,
+      cropped.origin,
-      sensor::VoxelFiltered(cropped_fan.returns,
+      sensor::VoxelFiltered(cropped.returns,
                             options_.laser_voxel_filter_size()),
-      sensor::VoxelFiltered(cropped_fan.misses,
+      sensor::VoxelFiltered(cropped.misses,
                             options_.laser_voxel_filter_size())};
 }
 
@@ -157,8 +179,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
               -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
           pose_prediction.rotation());
 
-  const sensor::LaserFan laser_fan_in_tracking_2d =
+  const sensor::LaserFan laser_fan_in_tracking_2d = TransformAndFilterLaserFan(
-      BuildCroppedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
+      tracking_to_tracking_2d.cast<float>(), laser_fan);
 
   if (laser_fan_in_tracking_2d.returns.empty()) {
     LOG(WARNING) << "Dropped empty horizontal laser point cloud.";

cartographer/mapping_2d/local_trajectory_builder.h

@@ -73,8 +73,7 @@ class LocalTrajectoryBuilder {
   const Submaps* submaps() const;
 
  private:
-  // Transforms 'laser_scan', crops and voxel filters.
+  sensor::LaserFan TransformAndFilterLaserFan(
-  sensor::LaserFan BuildCroppedLaserFan(
       const transform::Rigid3f& tracking_to_tracking_2d,
       const sensor::LaserFan& laser_fan) const;
 

cartographer/mapping_2d/proto/local_trajectory_builder_options.proto

@@ -24,10 +24,16 @@ import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.p
 import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
 
 message LocalTrajectoryBuilderOptions {
-  // Cropping parameters for the laser fan.
+  // Laser returns outside these ranges will be dropped.
+  optional float laser_min_range = 14;
+  optional float laser_max_range = 15;
   optional float laser_min_z = 1;
   optional float laser_max_z = 2;
 
+  // Laser returns beyond 'laser_max_range' will be inserted with this length as
+  // empty space.
+  optional float laser_missing_echo_ray_length = 16;
+
   // Voxel filter that gets applied to the horizontal laser immediately after
   // cropping.
   optional float laser_voxel_filter_size = 3;

cartographer/sensor/laser.cc

@@ -37,31 +37,25 @@ std::vector<uint8> ReorderReflectivities(
 
 }  // namespace
 
-LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range,
+PointCloud ToPointCloud(const proto::LaserScan& proto) {
-                    const float max_range,
+  CHECK_GE(proto.range_min(), 0.f);
-                    const float missing_echo_ray_length) {
+  CHECK_GE(proto.range_max(), proto.range_min());
-  CHECK_GE(min_range, 0.f);
   CHECK_GT(proto.angle_increment(), 0.f);
   CHECK_GT(proto.angle_max(), proto.angle_min());
-  LaserFan laser_fan = {Eigen::Vector3f::Zero(), {}, {}};
+  PointCloud point_cloud;
   float angle = proto.angle_min();
   for (const auto& range : proto.range()) {
     if (range.value_size() > 0) {
       const float first_echo = range.value(0);
-      if (!std::isnan(first_echo) && first_echo >= min_range) {
+      if (proto.range_min() <= first_echo && first_echo <= proto.range_max()) {
         const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
-        if (first_echo <= max_range) {
+        point_cloud.push_back(rotation *
-          laser_fan.returns.push_back(rotation *
+                              (first_echo * Eigen::Vector3f::UnitX()));
-                                      (first_echo * Eigen::Vector3f::UnitX()));
-        } else {
-          laser_fan.misses.push_back(
-              rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX()));
-        }
       }
     }
     angle += proto.angle_increment();
   }
-  return laser_fan;
+  return point_cloud;
 }
 
 proto::LaserFan ToProto(const LaserFan& laser_fan) {

cartographer/sensor/laser.h

@@ -38,14 +38,11 @@ struct LaserFan {
   std::vector<uint8> reflectivities;
 };
 
-// Builds a LaserFan from 'proto' and separates any beams with ranges outside
+// Builds a PointCloud of returns from 'proto', dropping any beams with ranges
-// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted
+// outside the valid range described by 'proto'.
-// into the 'misses' point cloud with length 'missing_echo_ray_length'. The
+PointCloud ToPointCloud(const proto::LaserScan& proto);
-// points in both clouds are stored in scan order.
-LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range,
-                    float max_range, float missing_echo_ray_length);
 
-// Converts 3D 'laser_fan' to a proto::LaserFan.
+// Converts 'laser_fan' to a proto::LaserFan.
 proto::LaserFan ToProto(const LaserFan& laser_fan);
 
 // Converts 'proto' to a LaserFan.

cartographer/sensor/laser_test.cc

@@ -28,7 +28,7 @@ namespace {
 using ::testing::Contains;
 using ::testing::PrintToString;
 
-TEST(ProjectorTest, ToLaserFan) {
+TEST(LaserTest, ToPointCloud) {
   proto::LaserScan laser_scan;
   for (int i = 0; i < 8; ++i) {
     laser_scan.add_range()->add_value(1.f);
@@ -36,24 +36,26 @@ TEST(ProjectorTest, ToLaserFan) {
   laser_scan.set_angle_min(0.f);
   laser_scan.set_angle_max(8.f * static_cast<float>(M_PI_4));
   laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
+  laser_scan.set_range_min(0.f);
+  laser_scan.set_range_max(10.f);
 
-  const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f);
+  const auto point_cloud = ToPointCloud(laser_scan);
-  EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[1].isApprox(
+  EXPECT_TRUE(point_cloud[1].isApprox(
       Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[3].isApprox(
+  EXPECT_TRUE(point_cloud[3].isApprox(
       Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[5].isApprox(
+  EXPECT_TRUE(point_cloud[5].isApprox(
       Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
       1e-6));
-  EXPECT_TRUE(fan.returns[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[7].isApprox(
+  EXPECT_TRUE(point_cloud[7].isApprox(
       Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
 }
 
-TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
+TEST(LaserTest, ToPointCloudWithInfinityAndNaN) {
   proto::LaserScan laser_scan;
   laser_scan.add_range()->add_value(1.f);
   laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity());
@@ -63,15 +65,13 @@ TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
   laser_scan.set_angle_min(0.f);
   laser_scan.set_angle_max(3.f * static_cast<float>(M_PI_4));
   laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
+  laser_scan.set_range_min(2.f);
+  laser_scan.set_range_max(10.f);
 
-  const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f);
+  const auto point_cloud = ToPointCloud(laser_scan);
-  ASSERT_EQ(2, fan.returns.size());
+  ASSERT_EQ(2, point_cloud.size());
-  EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
-  EXPECT_TRUE(fan.returns[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
-
-  ASSERT_EQ(1, fan.misses.size());
-  EXPECT_TRUE(fan.misses[0].isApprox(
-      Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
 }
 
 // Custom matcher for pair<eigen::Vector3f, int> entries.
@@ -82,12 +82,13 @@ MATCHER_P(PairApproximatelyEquals, expected,
 }
 
 TEST(LaserTest, Compression) {
-  LaserFan fan = {Eigen::Vector3f(1, 1, 1),
+  const LaserFan laser_fan = {
-                  {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
+      Eigen::Vector3f(1, 1, 1),
-                   Eigen::Vector3f(0, 1, 2)},
+      {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
-                  {Eigen::Vector3f(7, 8, 9)},
+       Eigen::Vector3f(0, 1, 2)},
-                  {1, 2, 3}};
+      {Eigen::Vector3f(7, 8, 9)},
-  LaserFan actual = Decompress(Compress(fan));
+      {1, 2, 3}};
+  const LaserFan actual = Decompress(Compress(laser_fan));
   EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
   EXPECT_EQ(3, actual.returns.size());
   EXPECT_EQ(1, actual.misses.size());

configuration_files/trajectory_builder_2d.lua

@@ -14,8 +14,11 @@
 
 TRAJECTORY_BUILDER_2D = {
   use_imu_data = true,
+  laser_min_range = 0.,
+  laser_max_range = 30.,
   laser_min_z = -0.8,
   laser_max_z = 2.,
+  laser_missing_echo_ray_length = 5.,
   laser_voxel_filter_size = 0.025,
 
   use_online_correlative_scan_matching = false,