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

Store histogram in submap (#1277) 

The histogram of a submap is now stored in the submap (class and proto) itself. This change allows to accumulate the histogram of a submap in local SLAM by adding up the histogram of each new scan.

The main advantage is that the background thread doesn't have to loop over all `TrajectoryNode`s of a finished submap to compute the submap histogram for the `RotationalScanMatcher`. Instead this chunk of work is moved to the local SLAM thread but is split up into a few computations for each new scan. When running localization, the histogram of a submap can just be read from a map pbstream and does not have to be computed from the nodes.

In summary:
- This change improved the CPU time of offline SLAM by ~7%.
- Increases the readability of the code and performance of the background thread. (see `PoseGraph3D::ComputeConstraint`)
- No negative performance impacts on accuracy or finding loop-closures

However:
- With this change to the submap proto, old maps (pbstreams) are no longer supported and need to be re-created by running offline slam
master
Martin Schwörer 2018-09-04 12:02:57 +02:00 committed by Wally B. Feed
parent b1dfa30ee3
commit 3b511aa1ba
16 changed files with 109 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/io/internal/mapping_state_serialization.h
View File

@ -25,9 +25,8 @@ namespace cartographer {
namespace io {
// The current serialization format version.
static constexpr int kMappingStateSerializationFormatVersion = 1;
static constexpr int kFormatVersionWithoutSubmapHistograms =
kMappingStateSerializationFormatVersion;
static constexpr int kMappingStateSerializationFormatVersion = 2;
static constexpr int kFormatVersionWithoutSubmapHistograms = 1;
// Serialize mapping state to a pbstream.
void WritePbStream(

3
cartographer/io/proto_stream_deserializer.cc
View File

@ -31,7 +31,8 @@ mapping::proto::SerializationHeader ReadHeaderOrDie(
}
bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
return header.format_version() == kMappingStateSerializationFormatVersion;
return header.format_version() == kMappingStateSerializationFormatVersion ||
header.format_version() == kFormatVersionWithoutSubmapHistograms;
}
} // namespace

45
cartographer/mapping/3d/submap_3d.cc
View File

@ -197,12 +197,14 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
}
Submap3D::Submap3D(const float high_resolution, const float low_resolution,
const transform::Rigid3d& local_submap_pose)
const transform::Rigid3d& local_submap_pose,
const Eigen::VectorXf& rotational_scan_matcher_histogram)
: Submap(local_submap_pose),
high_resolution_hybrid_grid_(
absl::make_unique<HybridGrid>(high_resolution)),
low_resolution_hybrid_grid_(
absl::make_unique<HybridGrid>(low_resolution)) {}
absl::make_unique<HybridGrid>(low_resolution)),
rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {}
Submap3D::Submap3D(const proto::Submap3D& proto)
: Submap(transform::ToRigid3(proto.local_pose())) {
@ -266,9 +268,11 @@ void Submap3D::ToResponseProto(
response->add_textures());
}
void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
const RangeDataInserter3D& range_data_inserter,
const float high_resolution_max_range) {
void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
const RangeDataInserter3D& range_data_inserter,
const float high_resolution_max_range,
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& scan_histogram_in_gravity) {
CHECK(!insertion_finished());
// Transform range data into submap frame.
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
@ -280,6 +284,11 @@ void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
range_data_inserter.Insert(transformed_range_data,
low_resolution_hybrid_grid_.get());
set_num_range_data(num_range_data() + 1);
const float yaw_in_submap_from_gravity = transform::GetYaw(
local_pose().inverse().rotation() * local_from_gravity_aligned);
rotational_scan_matcher_histogram_ +=
scan_matching::RotationalScanMatcher::RotateHistogram(
scan_histogram_in_gravity, yaw_in_submap_from_gravity);
}
void Submap3D::Finish() {
@ -296,17 +305,21 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
submaps_.end());
}
std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertData(
const sensor::RangeData& range_data,
const Eigen::Quaterniond& local_from_gravity_aligned) {
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity) {
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
local_from_gravity_aligned));
local_from_gravity_aligned),
rotational_scan_matcher_histogram_in_gravity.size());
}
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_,
options_.high_resolution_max_range());
submap->InsertData(range_data, range_data_inserter_,
options_.high_resolution_max_range(),
local_from_gravity_aligned,
rotational_scan_matcher_histogram_in_gravity);
}
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish();
@ -314,16 +327,20 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
return submaps();
}
void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) {
void ActiveSubmaps3D::AddSubmap(
const transform::Rigid3d& local_submap_pose,
const int rotational_scan_matcher_histogram_size) {
if (submaps_.size() >= 2) {
// This will crop the finished Submap before inserting a new Submap to
// reduce peak memory usage a bit.
CHECK(submaps_.front()->insertion_finished());
submaps_.erase(submaps_.begin());
}
submaps_.emplace_back(new Submap3D(options_.high_resolution(),
options_.low_resolution(),
local_submap_pose));
const Eigen::VectorXf initial_rotational_scan_matcher_histogram =
Eigen::VectorXf::Zero(rotational_scan_matcher_histogram_size);
submaps_.emplace_back(new Submap3D(
options_.high_resolution(), options_.low_resolution(), local_submap_pose,
initial_rotational_scan_matcher_histogram));
}
} // namespace mapping

23
cartographer/mapping/3d/submap_3d.h
View File

@ -43,7 +43,9 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
class Submap3D : public Submap {
public:
Submap3D(float high_resolution, float low_resolution,
const transform::Rigid3d& local_submap_pose);
const transform::Rigid3d& local_submap_pose,
const Eigen::VectorXf& rotational_scan_matcher_histogram);
explicit Submap3D(const proto::Submap3D& proto);
proto::Submap ToProto(bool include_probability_grid_data) const override;
@ -64,9 +66,12 @@ class Submap3D : public Submap {
// Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet.
void InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter3D& range_data_inserter,
float high_resolution_max_range);
void InsertData(const sensor::RangeData& range_data,
const RangeDataInserter3D& range_data_inserter,
float high_resolution_max_range,
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& scan_histogram_in_gravity);
void Finish();
private:
@ -97,14 +102,18 @@ class ActiveSubmaps3D {
// Inserts 'range_data_in_local' into the Submap collection.
// 'local_from_gravity_aligned' is used for the orientation of new submaps so
// that the z axis approximately aligns with gravity.
std::vector<std::shared_ptr<const Submap3D>> InsertRangeData(
// 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
// submaps of the Submap collection.
std::vector<std::shared_ptr<const Submap3D>> InsertData(
const sensor::RangeData& range_data_in_local,
const Eigen::Quaterniond& local_from_gravity_aligned);
const Eigen::Quaterniond& local_from_gravity_aligned,
const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity);
std::vector<std::shared_ptr<const Submap3D>> submaps() const;
private:
void AddSubmap(const transform::Rigid3d& local_submap_pose);
void AddSubmap(const transform::Rigid3d& local_submap_pose,
int rotational_scan_matcher_histogram_size);
const proto::SubmapsOptions3D options_;
std::vector<std::shared_ptr<Submap3D>> submaps_;

7
cartographer/mapping/3d/submap_3d_test.cc
View File

@ -24,10 +24,13 @@ namespace mapping {
namespace {
TEST(SubmapsTest, ToFromProto) {
Eigen::VectorXf histogram(2);
histogram << 1.0f, 2.0f;
const Submap3D expected(
0.05, 0.25,
transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
Eigen::Quaterniond(0., 0., 0., 1.)));
Eigen::Quaterniond(0., 0., 0., 1.)),
histogram);
const proto::Submap proto =
expected.ToProto(true /* include_probability_grid_data */);
EXPECT_FALSE(proto.has_submap_2d());
@ -41,6 +44,8 @@ TEST(SubmapsTest, ToFromProto) {
EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished());
EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6);
EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6);
EXPECT_TRUE(expected.rotational_scan_matcher_histogram().isApprox(
actual.rotational_scan_matcher_histogram(), 1e-6));
}
} // namespace

18
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -352,19 +352,19 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_estimate)) {
return nullptr;
}
const auto local_from_gravity_aligned =
pose_estimate.rotation() * gravity_alignment.inverse();
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
active_submaps_.InsertRangeData(filtered_range_data_in_local,
local_from_gravity_aligned);
const Eigen::VectorXf rotational_scan_matcher_histogram =
const Eigen::VectorXf rotational_scan_matcher_histogram_in_gravity =
scan_matching::RotationalScanMatcher::ComputeHistogram(
sensor::TransformPointCloud(
filtered_range_data_in_tracking.returns,
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
options_.rotational_histogram_size());
const Eigen::Quaterniond local_from_gravity_aligned =
pose_estimate.rotation() * gravity_alignment.inverse();
std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
active_submaps_.InsertData(filtered_range_data_in_local,
local_from_gravity_aligned,
rotational_scan_matcher_histogram_in_gravity);
return absl::make_unique<InsertionResult>(
InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
@ -373,7 +373,7 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking,
rotational_scan_matcher_histogram,
rotational_scan_matcher_histogram_in_gravity,
pose_estimate}),
std::move(insertion_submaps)});
}

22
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -246,14 +246,10 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
const transform::Rigid3d global_submap_pose =
optimization_problem_->submap_data().at(submap_id).global_pose;
const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse();
bool maybe_add_local_constraint = false;
bool maybe_add_global_constraint = false;
const TrajectoryNode::Data* constant_data;
const Submap3D* submap;
std::vector<TrajectoryNode> submap_nodes;
{
absl::MutexLock locker(&mutex_);
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
@ -263,14 +259,6 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
return;
}
for (const NodeId& submap_node_id :
data_.submap_data.at(submap_id).node_ids) {
submap_nodes.push_back(TrajectoryNode{
data_.trajectory_nodes.at(submap_node_id).constant_data,
global_submap_pose_inverse *
data_.trajectory_nodes.at(submap_node_id).global_pose});
}
const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
const common::Time last_connection_time =
data_.trajectory_connectivity_state.LastConnectionTime(
@ -299,13 +287,13 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
}
if (maybe_add_local_constraint) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap, node_id, constant_data, submap_nodes,
global_node_pose, global_submap_pose);
constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id,
constant_data, global_node_pose,
global_submap_pose);
} else if (maybe_add_global_constraint) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap, node_id, constant_data, submap_nodes,
global_node_pose.rotation(), global_submap_pose.rotation());
submap_id, submap, node_id, constant_data, global_node_pose.rotation(),
global_submap_pose.rotation());
}
}

22
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc
View File

@ -109,28 +109,10 @@ struct Candidate3D {
bool operator>(const Candidate3D& other) const { return score > other.score; }
};
namespace {
std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
const std::vector<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.global_pose *
transform::Rigid3d::Rotation(
node.constant_data->gravity_alignment.inverse())));
}
return histograms_at_angles;
}
} // namespace
FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* const low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const Eigen::VectorXf* rotational_scan_matcher_histogram,
const proto::FastCorrelativeScanMatcherOptions3D& options)
: options_(options),
resolution_(hybrid_grid.resolution()),
@ -138,7 +120,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
precomputation_grid_stack_(
absl::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
rotational_scan_matcher_(rotational_scan_matcher_histogram) {}
FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}

2
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h
View File

@ -75,7 +75,7 @@ class FastCorrelativeScanMatcher3D {
FastCorrelativeScanMatcher3D(
const HybridGrid& hybrid_grid,
const HybridGrid* low_resolution_hybrid_grid,
const std::vector<TrajectoryNode>& nodes,
const Eigen::VectorXf* rotational_scan_matcher_histogram,
const proto::FastCorrelativeScanMatcherOptions3D& options);
~FastCorrelativeScanMatcher3D();

14
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
View File

@ -105,11 +105,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
hybrid_grid_->FinishUpdate();
return absl::make_unique<FastCorrelativeScanMatcher3D>(
*hybrid_grid_, hybrid_grid_.get(),
std::vector<TrajectoryNode>(
{{std::make_shared<const TrajectoryNode::Data>(
CreateConstantData(point_cloud_)),
pose.cast<double>()}}),
*hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(),
options);
}
@ -120,7 +116,11 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
{},
point_cloud_,
low_resolution_point_cloud,
Eigen::VectorXf::Zero(10)};
GetRotationalScanMatcherHistogram()};
}
const Eigen::VectorXf& GetRotationalScanMatcherHistogram() {
return rotational_scan_matcher_histogram_;
}
std::mt19937 prng_ = std::mt19937(42);
@ -130,6 +130,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
const proto::FastCorrelativeScanMatcherOptions3D options_;
sensor::PointCloud point_cloud_;
std::unique_ptr<HybridGrid> hybrid_grid_;
const Eigen::VectorXf rotational_scan_matcher_histogram_ =
Eigen::VectorXf::Zero(10);
};
constexpr float kMinScore = 0.1f;

20
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
View File

@ -133,6 +133,11 @@ float MatchHistograms(const Eigen::VectorXf& submap_histogram,
} // namespace
RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram)
: histogram_(histogram) {}
// Rotates the given 'histogram' by the given 'angle'. This might lead to
// rotations of a fractional bucket which is handled by linearly interpolating.
Eigen::VectorXf RotationalScanMatcher::RotateHistogram(
const Eigen::VectorXf& histogram, const float angle) {
const float rotate_by_buckets = -angle * histogram.size() / M_PI;
@ -166,19 +171,6 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
return histogram;
}
RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf& histogram)
: histogram_(histogram) {}
RotationalScanMatcher::RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
: histogram_(
Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) {
for (const auto& histogram_at_angle : histograms_at_angles) {
histogram_ +=
RotateHistogram(histogram_at_angle.first, histogram_at_angle.second);
}
}
std::vector<float> RotationalScanMatcher::Match(
const Eigen::VectorXf& histogram, const float initial_angle,
const std::vector<float>& angles) const {
@ -187,7 +179,7 @@ std::vector<float> RotationalScanMatcher::Match(
for (const float angle : angles) {
const Eigen::VectorXf scan_histogram =
RotateHistogram(histogram, initial_angle + angle);
result.push_back(MatchHistograms(histogram_, scan_histogram));
result.push_back(MatchHistograms(*histogram_, scan_histogram));
}
return result;
}

11
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h
View File

@ -38,14 +38,7 @@ class RotationalScanMatcher {
static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud,
int histogram_size);
explicit RotationalScanMatcher(const Eigen::VectorXf& histogram);
// Creates a matcher from the given histograms rotated by the given angles.
// The angles should be chosen to bring the histograms into approximately the
// same frame.
explicit RotationalScanMatcher(
const std::vector<std::pair<Eigen::VectorXf, float>>&
histograms_at_angles);
explicit RotationalScanMatcher(const Eigen::VectorXf* histogram);
// Scores how well 'histogram' rotated by 'initial_angle' can be understood as
// further rotated by certain 'angles' relative to the 'nodes'. Each angle
@ -55,7 +48,7 @@ class RotationalScanMatcher {
const std::vector<float>& angles) const;
private:
Eigen::VectorXf histogram_;
const Eigen::VectorXf* histogram_;
};
} // namespace scan_matching

10
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher_test.cc
View File

@ -28,9 +28,7 @@ namespace {
TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
Eigen::VectorXf histogram(7);
histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
{histogram, 0.f}};
RotationalScanMatcher matcher(histogram_at_angle);
RotationalScanMatcher matcher(&histogram);
const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f});
ASSERT_EQ(2, scores.size());
EXPECT_NEAR(1.f, scores[0], 1e-6);
@ -41,9 +39,9 @@ TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
constexpr int kNumBuckets = 10;
constexpr float kAnglePerBucket = M_PI / kNumBuckets;
constexpr float kNoInitialRotation = 0.f;
const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
{Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}};
RotationalScanMatcher matcher(histogram_at_angle);
const Eigen::VectorXf histogram_no_initial_rotation =
Eigen::VectorXf::Unit(kNumBuckets, 3);
RotationalScanMatcher matcher(&histogram_no_initial_rotation);
for (float t = 0.f; t < 1.f; t += 0.1f) {
// 't' is the fraction of overlap and we have to divide by the norm of the
// histogram to get the expected score.

19
cartographer/mapping/internal/constraints/constraint_builder_3d.cc
View File

@ -77,7 +77,6 @@ ConstraintBuilder3D::~ConstraintBuilder3D() {
void ConstraintBuilder3D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose) {
if ((global_node_pose.translation() - global_submap_pose.translation())
@ -94,8 +93,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
@ -111,7 +109,6 @@ void ConstraintBuilder3D::MaybeAddConstraint(
void ConstraintBuilder3D::MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap3D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation) {
absl::MutexLock locker(&mutex_);
@ -122,8 +119,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
@ -165,9 +161,8 @@ void ConstraintBuilder3D::WhenDone(
}
const ConstraintBuilder3D::SubmapScanMatcher*
ConstraintBuilder3D::DispatchScanMatcherConstruction(
const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
const Submap3D* submap) {
ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
const Submap3D* submap) {
if (submap_scan_matchers_.count(submap_id) != 0) {
return &submap_scan_matchers_.at(submap_id);
}
@ -178,13 +173,15 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(
&submap->low_resolution_hybrid_grid();
auto& scan_matcher_options =
options_.fast_correlative_scan_matcher_options_3d();
const Eigen::VectorXf* histogram =
&submap->rotational_scan_matcher_histogram();
auto scan_matcher_task = absl::make_unique<common::Task>();
scan_matcher_task->SetWorkItem(
[&submap_scan_matcher, &scan_matcher_options, submap_nodes]() {
[&submap_scan_matcher, &scan_matcher_options, histogram]() {
submap_scan_matcher.fast_correlative_scan_matcher =
absl::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
*submap_scan_matcher.high_resolution_hybrid_grid,
submap_scan_matcher.low_resolution_hybrid_grid, submap_nodes,
submap_scan_matcher.low_resolution_hybrid_grid, histogram,
scan_matcher_options);
});
submap_scan_matcher.creation_task_handle =

5
cartographer/mapping/internal/constraints/constraint_builder_3d.h
View File

@ -78,7 +78,6 @@ class ConstraintBuilder3D {
void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose);
@ -94,7 +93,6 @@ class ConstraintBuilder3D {
void MaybeAddGlobalConstraint(
const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
const TrajectoryNode::Data* const constant_data,
const std::vector<TrajectoryNode>& submap_nodes,
const Eigen::Quaterniond& global_node_rotation,
const Eigen::Quaterniond& global_submap_rotation);
@ -126,8 +124,7 @@ class ConstraintBuilder3D {
// The returned 'grid' and 'fast_correlative_scan_matcher' must only be
// accessed after 'creation_task_handle' has completed.
const SubmapScanMatcher* DispatchScanMatcherConstruction(
const SubmapId& submap_id,
const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
const SubmapId& submap_id, const Submap3D* submap)
EXCLUSIVE_LOCKS_REQUIRED(mutex_);
// Runs in a background thread and does computations for an additional

16
cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc
View File

@ -71,7 +71,6 @@ TEST_F(ConstraintBuilder3DTest, CallsBack) {
}
TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
TrajectoryNode node;
auto node_data = std::make_shared<TrajectoryNode::Data>();
node_data->gravity_alignment = Eigen::Quaterniond::Identity();
node_data->high_resolution_point_cloud.push_back(
@ -80,23 +79,20 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
{Eigen::Vector3f(0.1, 0.2, 0.3)});
node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
node_data->local_pose = transform::Rigid3d::Identity();
node.constant_data = node_data;
std::vector<TrajectoryNode> submap_nodes = {node};
SubmapId submap_id{0, 1};
Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity());
Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity(),
Eigen::VectorXf::Zero(3));
int expected_nodes = 0;
for (int i = 0; i < 2; ++i) {
EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
for (int j = 0; j < 2; ++j) {
constraint_builder_->MaybeAddConstraint(
submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
submap_nodes, transform::Rigid3d::Identity(),
transform::Rigid3d::Identity());
submap_id, &submap, NodeId{0, 0}, node_data.get(),
transform::Rigid3d::Identity(), transform::Rigid3d::Identity());
}
constraint_builder_->MaybeAddGlobalConstraint(
submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
submap_nodes, Eigen::Quaterniond::Identity(),
Eigen::Quaterniond::Identity());
submap_id, &submap, NodeId{0, 0}, node_data.get(),
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity());
constraint_builder_->NotifyEndOfNode();
thread_pool_.WaitUntilIdle();
EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);