diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 5d40196..23c8879 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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( - sensor::TransformLaserFan(laser_fan, tracking_to_tracking_2d), + // Drop any returns below the minimum range and convert returns beyond the + // 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, - sensor::VoxelFiltered(cropped_fan.returns, + cropped.origin, + 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 = - BuildCroppedLaserFan(tracking_to_tracking_2d.cast(), laser_fan); + const sensor::LaserFan laser_fan_in_tracking_2d = TransformAndFilterLaserFan( + tracking_to_tracking_2d.cast(), laser_fan); if (laser_fan_in_tracking_2d.returns.empty()) { LOG(WARNING) << "Dropped empty horizontal laser point cloud."; diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index c24b87a..9681581 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/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 BuildCroppedLaserFan( + sensor::LaserFan TransformAndFilterLaserFan( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::LaserFan& laser_fan) const; diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index 6d2c094..8df426d 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/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; diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 36bbb10..6015183 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -37,31 +37,25 @@ std::vector ReorderReflectivities( } // namespace -LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range, - const float max_range, - const float missing_echo_ray_length) { - CHECK_GE(min_range, 0.f); +PointCloud ToPointCloud(const proto::LaserScan& proto) { + CHECK_GE(proto.range_min(), 0.f); + CHECK_GE(proto.range_max(), proto.range_min()); 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) { - laser_fan.returns.push_back(rotation * - (first_echo * Eigen::Vector3f::UnitX())); - } else { - laser_fan.misses.push_back( - rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX())); - } + point_cloud.push_back(rotation * + (first_echo * Eigen::Vector3f::UnitX())); } } angle += proto.angle_increment(); } - return laser_fan; + return point_cloud; } proto::LaserFan ToProto(const LaserFan& laser_fan) { diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 0a023be..3e838d4 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -38,14 +38,11 @@ struct LaserFan { std::vector reflectivities; }; -// Builds a LaserFan from 'proto' and separates any beams with ranges outside -// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted -// into the 'misses' point cloud with length 'missing_echo_ray_length'. The -// 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); +// Builds a PointCloud of returns from 'proto', dropping any beams with ranges +// outside the valid range described by 'proto'. +PointCloud ToPointCloud(const proto::LaserScan& proto); -// 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. diff --git a/cartographer/sensor/laser_test.cc b/cartographer/sensor/laser_test.cc index 6331e45..e1bfd6d 100644 --- a/cartographer/sensor/laser_test.cc +++ b/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(M_PI_4)); laser_scan.set_angle_increment(static_cast(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); - EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); - EXPECT_TRUE(fan.returns[1].isApprox( + const auto point_cloud = ToPointCloud(laser_scan); + EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); + 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(fan.returns[3].isApprox( + EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6)); + 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(fan.returns[5].isApprox( + EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6)); + 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(fan.returns[7].isApprox( + EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6)); + 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::infinity()); @@ -63,15 +65,13 @@ TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) { laser_scan.set_angle_min(0.f); laser_scan.set_angle_max(3.f * static_cast(M_PI_4)); laser_scan.set_angle_increment(static_cast(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); - ASSERT_EQ(2, fan.returns.size()); - EXPECT_TRUE(fan.returns[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)); - - 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)); + const auto point_cloud = ToPointCloud(laser_scan); + ASSERT_EQ(2, point_cloud.size()); + EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); + EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); } // Custom matcher for pair entries. @@ -82,12 +82,13 @@ MATCHER_P(PairApproximatelyEquals, expected, } TEST(LaserTest, Compression) { - LaserFan fan = {Eigen::Vector3f(1, 1, 1), - {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), - Eigen::Vector3f(0, 1, 2)}, - {Eigen::Vector3f(7, 8, 9)}, - {1, 2, 3}}; - LaserFan actual = Decompress(Compress(fan)); + const LaserFan laser_fan = { + Eigen::Vector3f(1, 1, 1), + {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), + Eigen::Vector3f(0, 1, 2)}, + {Eigen::Vector3f(7, 8, 9)}, + {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()); diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index bc5bcf4..62a14f4 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/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,