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

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(

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

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

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

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

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

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());
}
}

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() {}

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

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;

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

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

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.

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 =

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

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