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

Store rotational histogram as part of the node data. () 

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

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,