From 22f41d8e372bd937a9c75a0b30ddfba43dfe2cb1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 7 Sep 2017 11:01:03 +0200 Subject: [PATCH] Store rotational histogram as part of the node data. (#503) The compressed point cloud is no longer stored as part of the node data to reduce memory consumption. --- cartographer/mapping/map_builder.cc | 4 +- cartographer/mapping/trajectory_node.h | 4 +- .../mapping_2d/local_trajectory_builder.cc | 1 - .../mapping_2d/sparse_pose_graph_test.cc | 3 +- .../mapping_3d/local_trajectory_builder.cc | 14 ++++- .../mapping_3d/local_trajectory_builder.h | 1 + .../local_trajectory_builder_options.cc | 2 + .../local_trajectory_builder_test.cc | 1 + .../local_trajectory_builder_options.proto | 5 +- .../fast_correlative_scan_matcher.cc | 62 +++++++++---------- .../fast_correlative_scan_matcher.h | 30 ++++----- .../fast_correlative_scan_matcher_test.cc | 20 +++--- ...ast_correlative_scan_matcher_options.proto | 3 - configuration_files/sparse_pose_graph.lua | 1 - configuration_files/trajectory_builder_3d.lua | 1 + 15 files changed, 80 insertions(+), 72 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 8635bff..c5ade1b 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -181,9 +181,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { // TODO(whess): Handle trimmed data. range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id); range_data_proto->mutable_node_id()->set_node_index(node_index); - const auto& data = *node_data[trajectory_id][node_index].constant_data; - *range_data_proto->mutable_range_data() = - sensor::ToProto(data.range_data); + range_data_proto->mutable_range_data(); // TODO(whess): Only enable optionally? Resulting pbstream files will be // a lot larger now. writer->WriteProto(proto); diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 9b0445c..3d7ad98 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -32,9 +32,6 @@ struct TrajectoryNode { struct Data { common::Time time; - // Range data in 'tracking' frame. Only used in 3D. - sensor::CompressedRangeData range_data; - // Transform to approximately gravity align the tracking frame as // determined by local SLAM. Eigen::Quaterniond gravity_alignment; @@ -46,6 +43,7 @@ struct TrajectoryNode { // Used for loop closure in 3D. sensor::PointCloud high_resolution_point_cloud; sensor::PointCloud low_resolution_point_cloud; + Eigen::VectorXf rotational_scan_matcher_histogram; }; common::Time time() const { return constant_data->time; } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 02dbe40..955b863 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -180,7 +180,6 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( std::make_shared( mapping::TrajectoryNode::Data{ time, - {}, // 'range_data' is only used in 3D. gravity_alignment.rotation(), filtered_gravity_aligned_point_cloud, {}, // 'high_resolution_point_cloud' is only used in 3D. diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index f641491..3688e2d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -92,7 +92,6 @@ class SparsePoseGraphTest : public ::testing::Test { fast_correlative_scan_matcher_3d = { branch_and_bound_depth = 3, full_resolution_depth = 3, - rotational_histogram_size = 30, min_rotational_score = 0.1, min_low_resolution_score = 0.5, linear_xy_search_window = 4., @@ -160,10 +159,10 @@ class SparsePoseGraphTest : public ::testing::Test { sparse_pose_graph_->AddScan( std::make_shared( mapping::TrajectoryNode::Data{common::FromUniversal(0), - Compress(range_data), Eigen::Quaterniond::Identity(), range_data.returns, {}, + {}, {}}), transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps); } diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index fcce9f4..cede6be 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -24,6 +24,7 @@ #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/submaps_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "glog/logging.h" namespace cartographer { @@ -158,6 +159,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( extrapolator_->AddPose(time, pose_estimate); const Eigen::Quaterniond gravity_alignment = extrapolator_->EstimateGravityOrientation(time); + const auto rotational_scan_matcher_histogram = + scan_matching::RotationalScanMatcher::ComputeHistogram( + sensor::TransformPointCloud( + filtered_range_data.returns, + transform::Rigid3f::Rotation(gravity_alignment.cast())), + options_.rotational_histogram_size()); last_pose_estimate_ = { time, pose_estimate, @@ -167,7 +174,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( return InsertIntoSubmap(time, filtered_range_data, gravity_alignment, filtered_point_cloud_in_tracking, low_resolution_point_cloud_in_tracking, - pose_estimate); + rotational_scan_matcher_histogram, pose_estimate); } void LocalTrajectoryBuilder::AddOdometerData( @@ -190,6 +197,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap( const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& high_resolution_point_cloud, const sensor::PointCloud& low_resolution_point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; @@ -209,11 +217,11 @@ LocalTrajectoryBuilder::InsertIntoSubmap( std::make_shared( mapping::TrajectoryNode::Data{ time, - sensor::Compress(range_data_in_tracking), gravity_alignment, {}, // 'filtered_point_cloud' is only used in 2D. high_resolution_point_cloud, - low_resolution_point_cloud}), + low_resolution_point_cloud, + rotational_scan_matcher_histogram}), pose_observation, std::move(insertion_submaps)}); } diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index f78115a..103c119 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -68,6 +68,7 @@ class LocalTrajectoryBuilder { const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& high_resolution_point_cloud, const sensor::PointCloud& low_resolution_point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; diff --git a/cartographer/mapping_3d/local_trajectory_builder_options.cc b/cartographer/mapping_3d/local_trajectory_builder_options.cc index eabfc20..b4cdeac 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options.cc @@ -59,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( parameter_dictionary->GetDictionary("motion_filter").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); + options.set_rotational_histogram_size( + parameter_dictionary->GetInt("rotational_histogram_size")); *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions( parameter_dictionary->GetDictionary("submaps").get()); return options; diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index 5a2be7e..461cda8 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -92,6 +92,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { }, imu_gravity_time_constant = 1., + rotational_histogram_size = 120, submaps = { high_resolution = 0.2, diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto index ea31993..968c555 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto @@ -22,7 +22,7 @@ import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_m import "cartographer/mapping_3d/proto/submaps_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; -// NEXT ID: 17 +// NEXT ID: 18 message LocalTrajectoryBuilderOptions { // Rangefinder points outside these ranges will be dropped. optional float min_range = 1; @@ -59,5 +59,8 @@ message LocalTrajectoryBuilderOptions { // constant is increased) is balanced. optional double imu_gravity_time_constant = 15; + // Number of histogram buckets for the rotational scan matcher. + optional int32 rotational_histogram_size = 17; + optional SubmapsOptions submaps_options = 8; } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index d0c3746..2fd9987 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -42,8 +42,6 @@ CreateFastCorrelativeScanMatcherOptions( parameter_dictionary->GetInt("branch_and_bound_depth")); options.set_full_resolution_depth( parameter_dictionary->GetInt("full_resolution_depth")); - options.set_rotational_histogram_size( - parameter_dictionary->GetInt("rotational_histogram_size")); options.set_min_rotational_score( parameter_dictionary->GetDouble("min_rotational_score")); options.set_min_low_resolution_score( @@ -127,18 +125,17 @@ struct Candidate { namespace { -scan_matching::RotationalScanMatcher CreateRotationalScanMatcher( - const std::vector& nodes, - const int histogram_size) { - Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size); - for (const mapping::TrajectoryNode& node : nodes) { - histogram += scan_matching::RotationalScanMatcher::ComputeHistogram( - sensor::TransformPointCloud( - node.constant_data->range_data.returns.Decompress(), - node.pose.cast()), - histogram_size); +std::vector> HistogramsAtAnglesFromNodes( + const std::vector& nodes) { + std::vector> histograms_at_angles; + for (const auto& node : nodes) { + histograms_at_angles.emplace_back( + node.constant_data->rotational_scan_matcher_histogram, + transform::GetYaw( + node.pose * transform::Rigid3d::Rotation( + node.constant_data->gravity_alignment.inverse()))); } - return scan_matching::RotationalScanMatcher({{histogram, 0.f}}); + return histograms_at_angles; } } // namespace @@ -154,8 +151,7 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( precomputation_grid_stack_( common::make_unique(hybrid_grid, options)), low_resolution_hybrid_grid_(low_resolution_hybrid_grid), - rotational_scan_matcher_(CreateRotationalScanMatcher( - nodes, options_.rotational_histogram_size())) {} + rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} @@ -173,8 +169,9 @@ bool FastCorrelativeScanMatcher::Match( return MatchWithSearchParameters( search_parameters, initial_pose_estimate, constant_data.high_resolution_point_cloud, - constant_data.range_data.returns.Decompress(), min_score, score, - pose_estimate, rotational_score, low_resolution_score); + constant_data.rotational_scan_matcher_histogram, + constant_data.gravity_alignment, min_score, score, pose_estimate, + rotational_score, low_resolution_score); } bool FastCorrelativeScanMatcher::MatchFullSubmap( @@ -199,23 +196,25 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap( return MatchWithSearchParameters( search_parameters, initial_pose_estimate, constant_data.high_resolution_point_cloud, - constant_data.range_data.returns.Decompress(), min_score, score, - pose_estimate, rotational_score, low_resolution_score); + constant_data.rotational_scan_matcher_histogram, + constant_data.gravity_alignment, min_score, score, pose_estimate, + rotational_score, low_resolution_score); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, const float min_score, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, const float min_score, float* const score, transform::Rigid3d* const pose_estimate, float* const rotational_score, float* const low_resolution_score) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); const std::vector discrete_scans = GenerateDiscreteScans( - search_parameters, coarse_point_cloud, fine_point_cloud, - initial_pose_estimate.cast()); + search_parameters, point_cloud, rotational_scan_matcher_histogram, + gravity_alignment, initial_pose_estimate.cast()); const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(search_parameters, discrete_scans); @@ -281,14 +280,15 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, const transform::Rigid3f& initial_pose) const { std::vector result; // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution_; - for (const Eigen::Vector3f& point : coarse_point_cloud) { + for (const Eigen::Vector3f& point : point_cloud) { const float range = point.norm(); max_scan_range = std::max(range, max_scan_range); } @@ -305,10 +305,10 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( angles.push_back(rz * angular_step_size); } const std::vector scores = rotational_scan_matcher_.Match( - RotationalScanMatcher::ComputeHistogram( - sensor::TransformPointCloud(fine_point_cloud, initial_pose), - options_.rotational_histogram_size()), - 0.f /* initial_angle */, angles); + rotational_scan_matcher_histogram, + transform::GetYaw(initial_pose.rotation() * + gravity_alignment.inverse().cast()), + angles); for (size_t i = 0; i != angles.size(); ++i) { if (scores[i] < options_.min_rotational_score()) { continue; @@ -322,7 +322,7 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( transform::AngleAxisVectorToRotationQuaternion(angle_axis) * initial_pose.rotation()); result.push_back( - DiscretizeScan(search_parameters, coarse_point_cloud, pose, scores[i])); + DiscretizeScan(search_parameters, point_cloud, pose, scores[i])); } return result; } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h index bc069b4..1c57418 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -60,22 +60,20 @@ class FastCorrelativeScanMatcher { FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = delete; - // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an - // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) - // is possible, true is returned, and 'score', 'pose_estimate', + // Aligns the node with the given 'constant_data' within the 'hybrid_grid' + // given an 'initial_pose_estimate'. If a score above 'min_score' (excluding + // equality) is possible, true is returned, and 'score', 'pose_estimate', // 'rotational_score', and 'low_resolution_score' are updated with the result. - // 'fine_point_cloud' is used to compute the rotational scan matcher score. bool Match(const transform::Rigid3d& initial_pose_estimate, const mapping::TrajectoryNode::Data& constant_data, float min_score, float* score, transform::Rigid3d* pose_estimate, float* rotational_score, float* low_resolution_score) const; - // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which - // is expected to be approximately gravity aligned. If a score above - // 'min_score' (excluding equality) is possible, true is returned, and - // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score' - // are updated with the result. 'fine_point_cloud' is used to compute the - // rotational scan matcher score. + // Aligns the node with the given 'constant_data' within the 'hybrid_grid' + // given a rotation which is expected to be approximately gravity aligned. + // If a score above 'min_score' (excluding equality) is possible, true is + // returned, and 'score', 'pose_estimate', 'rotational_score', and + // 'low_resolution_score' are updated with the result. bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, const mapping::TrajectoryNode::Data& constant_data, float min_score, float* score, @@ -94,9 +92,10 @@ class FastCorrelativeScanMatcher { bool MatchWithSearchParameters( const SearchParameters& search_parameters, const transform::Rigid3d& initial_pose_estimate, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, float min_score, float* score, - transform::Rigid3d* pose_estimate, float* rotational_score, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, float min_score, + float* score, transform::Rigid3d* pose_estimate, float* rotational_score, float* low_resolution_score) const; DiscreteScan DiscretizeScan(const SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, @@ -104,8 +103,9 @@ class FastCorrelativeScanMatcher { float rotational_score) const; std::vector GenerateDiscreteScans( const SearchParameters& search_parameters, - const sensor::PointCloud& coarse_point_cloud, - const sensor::PointCloud& fine_point_cloud, + const sensor::PointCloud& point_cloud, + const Eigen::VectorXf& rotational_scan_matcher_histogram, + const Eigen::Quaterniond& gravity_alignment, const transform::Rigid3f& initial_pose) const; std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters, int num_discrete_scans) const; diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index f7c3dce..0b3654f 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -70,7 +70,6 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { "full_resolution_depth = " + std::to_string(branch_and_bound_depth) + ", " - "rotational_histogram_size = 30, " "min_rotational_score = 0.1, " // Unknown space has kMinProbability = 0.1, so we need to make sure here // to pick a larger number otherwise we always find matches. @@ -106,18 +105,21 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test { return common::make_unique( *hybrid_grid_, hybrid_grid_.get(), - std::vector(), options); + std::vector( + {{std::make_shared( + CreateConstantData(point_cloud_)), + pose.cast()}}), + options); } mapping::TrajectoryNode::Data CreateConstantData( const sensor::PointCloud& low_resolution_point_cloud) { - return mapping::TrajectoryNode::Data{ - common::FromUniversal(0), - Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}), - Eigen::Quaterniond::Identity(), - {}, - point_cloud_, - low_resolution_point_cloud}; + return mapping::TrajectoryNode::Data{common::FromUniversal(0), + Eigen::Quaterniond::Identity(), + {}, + point_cloud_, + low_resolution_point_cloud, + Eigen::VectorXf::Zero(10)}; } std::mt19937 prng_ = std::mt19937(42); diff --git a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto index 76f3e3c..3615d88 100644 --- a/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto +++ b/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto @@ -24,9 +24,6 @@ message FastCorrelativeScanMatcherOptions { // resolution by half each. optional int32 full_resolution_depth = 8; - // Number of histogram buckets for the rotational scan matcher. - optional int32 rotational_histogram_size = 3; - // Minimum score for the rotational scan matcher. optional double min_rotational_score = 4; diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 0de0012..a16d2da 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -40,7 +40,6 @@ SPARSE_POSE_GRAPH = { fast_correlative_scan_matcher_3d = { branch_and_bound_depth = 8, full_resolution_depth = 3, - rotational_histogram_size = 120, min_rotational_score = 0.77, min_low_resolution_score = 0.55, linear_xy_search_window = 5., diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 881728c..fdb8d16 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -60,6 +60,7 @@ TRAJECTORY_BUILDER_3D = { }, imu_gravity_time_constant = 10., + rotational_histogram_size = 120, submaps = { high_resolution = 0.10,