12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Wolfgang Hess 2017-09-07 11:01:03 +02:00 committed by GitHub
parent c2d9424342
commit 22f41d8e37
15 changed files with 80 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/map_builder.cc
View File

@ -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);

4
cartographer/mapping/trajectory_node.h
View File

@ -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; }

1
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -180,7 +180,6 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
std::make_shared<const mapping::TrajectoryNode::Data>(
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.

3
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -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<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0),
Compress(range_data),
Eigen::Quaterniond::Identity(),
range_data.returns,
{},
{},
{}}),
transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
}

14
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -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<float>())),
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<const mapping::TrajectoryNode::Data>(
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)});
}

1
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -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_;

2
cartographer/mapping_3d/local_trajectory_builder_options.cc
View File

@ -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;

1
cartographer/mapping_3d/local_trajectory_builder_test.cc
View File

@ -92,6 +92,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
},
imu_gravity_time_constant = 1.,
rotational_histogram_size = 120,
submaps = {
high_resolution = 0.2,

5
cartographer/mapping_3d/proto/local_trajectory_builder_options.proto
View File

@ -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;
}

62
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -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<mapping::TrajectoryNode>& 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<float>()),
histogram_size);
std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
const std::vector<mapping::TrajectoryNode>& nodes) {
std::vector<std::pair<Eigen::VectorXf, float>> 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<PrecomputationGridStack>(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<DiscreteScan> discrete_scans = GenerateDiscreteScans(
search_parameters, coarse_point_cloud, fine_point_cloud,
initial_pose_estimate.cast<float>());
search_parameters, point_cloud, rotational_scan_matcher_histogram,
gravity_alignment, initial_pose_estimate.cast<float>());
const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(search_parameters, discrete_scans);
@ -281,14 +280,15 @@ DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(
std::vector<DiscreteScan> 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<DiscreteScan> 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<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
angles.push_back(rz * angular_step_size);
}
const std::vector<float> 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<float>()),
angles);
for (size_t i = 0; i != angles.size(); ++i) {
if (scores[i] < options_.min_rotational_score()) {
continue;
@ -322,7 +322,7 @@ std::vector<DiscreteScan> 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;
}

30
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -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<DiscreteScan> 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<Candidate> GenerateLowestResolutionCandidates(
const SearchParameters& search_parameters, int num_discrete_scans) const;

20
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -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<FastCorrelativeScanMatcher>(
*hybrid_grid_, hybrid_grid_.get(),
std::vector<mapping::TrajectoryNode>(), options);
std::vector<mapping::TrajectoryNode>(
{{std::make_shared<const mapping::TrajectoryNode::Data>(
CreateConstantData(point_cloud_)),
pose.cast<double>()}}),
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);

3
cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto
View File

@ -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;

1
configuration_files/sparse_pose_graph.lua
View File

@ -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.,

1
configuration_files/trajectory_builder_3d.lua
View File

@ -60,6 +60,7 @@ TRAJECTORY_BUILDER_3D = {
},
imu_gravity_time_constant = 10.,
rotational_histogram_size = 120,
submaps = {
high_resolution = 0.10,