Keep 'trajectory_nodes_' per trajectory. (#291)

Fixes #256.

PAIR=SirVer
master
Wolfgang Hess 2017-05-17 11:15:34 +02:00 committed by GitHub
parent 5aa968748e
commit 6c9e24d7c5
17 changed files with 218 additions and 272 deletions

View File

@ -97,16 +97,19 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
const int flat_scan_index = trajectory_nodes_.size();
const int flat_scan_index = num_trajectory_nodes_;
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
trajectory_nodes_.resize(
std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
trajectory_nodes_[trajectory_id].push_back(mapping::TrajectoryNode{
&constant_node_data_.back(), optimized_pose,
});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
if (submap_ids_.count(insertion_submaps.back()) == 0) {
@ -157,10 +160,31 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
});
}
void SparsePoseGraph::ComputeConstraint(const int scan_index,
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
const transform::Rigid2d relative_pose = optimization_problem_.submap_data()
// Only globally match against submaps not in this trajectory.
if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
node_id,
&trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_2d.returns,
&trajectory_connectivity_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
reverse_connected_components_.at(node_id.trajectory_id) ==
reverse_connected_components_.at(submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
@ -168,35 +192,17 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const int scan_trajectory_id =
trajectory_nodes_[scan_index].constant_data->trajectory_id;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
node_id, scan_index, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(submap_id.trajectory_id);
if (scan_trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
node_id, scan_index,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose);
node_id,
&trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_2d.returns,
initial_relative_pose);
}
}
}
@ -209,7 +215,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index, submap_id);
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
}
}
}
@ -234,7 +240,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
@ -273,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(submap_index)
.node_ids.count(node_id),
0);
ComputeConstraint(scan_index,
ComputeConstraint(node_id,
mapping::SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(submap_index)});
}
@ -291,7 +299,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
ComputeConstraintsForOldScans(finished_submap);
finished_submap_state.finished = true;
}
constraint_builder_.NotifyEndOfScan(scan_index);
constraint_builder_.NotifyEndOfScan();
++num_scans_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 &&
num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@ -335,8 +343,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
return constraint_builder_.GetNumFinishedScans() ==
num_trajectory_nodes_;
},
common::FromSeconds(1.))) {
std::ostringstream progress_info;
@ -344,8 +352,7 @@ void SparsePoseGraph::WaitForAllComputations() {
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
(num_trajectory_nodes_ - num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
@ -379,30 +386,27 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) {
const mapping::NodeId node_id = scan_index_to_node_id_[i];
trajectory_nodes_[i].pose =
transform::Embed3D(node_data.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose);
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
++trajectory_id) {
size_t node_index = 0;
const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
for (; node_index != node_data[trajectory_id].size(); ++node_index) {
trajectory_nodes_[trajectory_id][node_index].pose = transform::Embed3D(
node_data[trajectory_id][node_index].point_cloud_pose);
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
if (extrapolation_transforms.count(trajectory_id) == 0) {
const auto new_submap_transforms = ExtrapolateSubmapTransforms(
optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory_id] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
const transform::Rigid3d extrapolation_transform =
new_submap_transforms.back() * old_submap_transforms.back().inverse();
for (; node_index < num_nodes; ++node_index) {
trajectory_nodes_[trajectory_id][node_index].pose =
extrapolation_transform *
trajectory_nodes_[trajectory_id][node_index].pose;
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -417,13 +421,7 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result;
for (const auto& node : trajectory_nodes_) {
result.resize(
std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

View File

@ -129,7 +129,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const int scan_index,
void ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished.
@ -201,7 +201,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Deque to keep references valid for the background computation when adding
// new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
GUARDED_BY(mutex_);
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>

View File

@ -62,8 +62,7 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index,
const sensor::PointCloud* const point_cloud,
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
@ -71,7 +70,6 @@ void ConstraintBuilder::MaybeAddConstraint(
}
if (sampler_.Pulse()) {
common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
@ -89,11 +87,9 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud) {
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
@ -108,9 +104,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
});
}
void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
void ConstraintBuilder::NotifyEndOfScan() {
common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_;
}

View File

@ -72,37 +72,32 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'flat_scan_index'.
// The 'initial_pose' is relative to the 'submap'.
// 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
// relative to the 'submap'.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const mapping::Submap* submap,
const mapping::NodeId& node_id,
const int flat_scan_index,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs
// full-submap matching.
// 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
// matching.
//
// The scan at 'flat_scan_index' should be from trajectory
// 'node_id.trajectory_id'. The 'trajectory_connectivity' is updated if the
// full-submap match succeeds.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud);
const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
mapping::TrajectoryConnectivity* trajectory_connectivity);
// Must be called after all computations related to 'flat_scan_index' are
// added.
void NotifyEndOfScan(const int flat_scan_index);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.

View File

@ -46,8 +46,8 @@ void GlobalTrajectoryBuilder::AddImuData(
void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
auto insertion_result = local_trajectory_builder_->AddRangefinderData(
time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex());
auto insertion_result =
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
if (insertion_result == nullptr) {
return;

View File

@ -72,7 +72,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, int next_trajectory_node_index) {
const sensor::PointCloud& ranges) {
if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized.";
return nullptr;
@ -114,18 +114,15 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
if (num_accumulated_ >= options_.scans_per_accumulation()) {
num_accumulated_ = 0;
return AddAccumulatedRangeData(
time,
sensor::TransformRangeData(accumulated_range_data_,
tracking_delta.inverse()),
next_trajectory_node_index);
time, sensor::TransformRangeData(accumulated_range_data_,
tracking_delta.inverse()));
}
return nullptr;
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const int trajectory_node_index) {
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -184,7 +181,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
pose_observation.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, pose_observation,
covariance_estimate, trajectory_node_index);
covariance_estimate);
}
void KalmanLocalTrajectoryBuilder::AddOdometerData(
@ -213,8 +210,7 @@ std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate,
const int trajectory_node_index) {
const kalman_filter::PoseCovariance& covariance_estimate) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
@ -224,10 +220,8 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
trajectory_node_index);
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate,
matching_submap, insertion_submaps});

View File

@ -50,8 +50,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges,
int next_trajectory_node_index) override;
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override;
@ -59,14 +58,12 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& range_data_in_tracking,
int trajectory_node_index);
common::Time time, const sensor::RangeData& range_data_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate,
int trajectory_node_index);
const kalman_filter::PoseCovariance& covariance_estimate);
const proto::LocalTrajectoryBuilderOptions options_;
std::unique_ptr<mapping_3d::Submaps> submaps_;

View File

@ -260,8 +260,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
AddLinearOnlyImuObservation(node.time, node.pose);
const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangefinderData(
node.time, range_data.origin, range_data.returns, num_poses) !=
nullptr) {
node.time, range_data.origin, range_data.returns) != nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses;

View File

@ -54,7 +54,7 @@ class LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, int next_trajectory_node_index) = 0;
const sensor::PointCloud& ranges) = 0;
virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0;

View File

@ -135,7 +135,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData(
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, const int next_trajectory_node_index) {
const sensor::PointCloud& ranges) {
CHECK_GT(ranges.size(), 0);
// TODO(hrapp): Handle misses.
@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
++num_accumulated_;
RemoveObsoleteSensorData();
return MaybeOptimize(time, next_trajectory_node_index);
return MaybeOptimize(time);
}
void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
@ -215,8 +215,7 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::MaybeOptimize(
const common::Time time, const int trajectory_node_index) {
OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
// TODO(hrapp): Make the number of optimizations configurable.
if (num_accumulated_ < options_.scans_per_accumulation() &&
num_accumulated_ % 10 != 0) {
@ -349,15 +348,13 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(
}
return AddAccumulatedRangeData(time, optimized_pose,
accumulated_range_data_in_tracking,
trajectory_node_index);
accumulated_range_data_in_tracking);
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const transform::Rigid3d& optimized_pose,
const sensor::RangeData& range_data_in_tracking,
const int trajectory_node_index) {
const sensor::RangeData& range_data_in_tracking) {
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -375,8 +372,7 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
sensor::TransformPointCloud(filtered_range_data.returns,
optimized_pose.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, optimized_pose,
trajectory_node_index);
return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
}
const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@ -387,8 +383,7 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const {
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const int trajectory_node_index) {
const transform::Rigid3d& pose_observation) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
@ -398,10 +393,8 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
trajectory_node_index);
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity();

View File

@ -53,8 +53,7 @@ class OptimizingLocalTrajectoryBuilder
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges,
int next_trajectory_node_index) override;
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override;
@ -102,16 +101,13 @@ class OptimizingLocalTrajectoryBuilder
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const transform::Rigid3d& pose_observation,
const sensor::RangeData& range_data_in_tracking,
const int next_trajectory_node_index);
const sensor::RangeData& range_data_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const int next_trajectory_node_index);
const transform::Rigid3d& pose_observation);
std::unique_ptr<InsertionResult> MaybeOptimize(
common::Time time, const int next_trajectory_node_index);
std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
const proto::LocalTrajectoryBuilderOptions options_;
const ceres::Solver::Options ceres_solver_options_;

View File

@ -96,15 +96,18 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
const int flat_scan_index = trajectory_nodes_.size();
const int flat_scan_index = num_trajectory_nodes_;
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id,
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(
trajectory_nodes_.resize(
std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
trajectory_nodes_[trajectory_id].push_back(
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
if (submap_ids_.count(insertion_submaps.back()) == 0) {
@ -135,11 +138,6 @@ void SparsePoseGraph::AddScan(
});
}
int SparsePoseGraph::GetNextTrajectoryNodeIndex() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_.size();
}
void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
if (scan_queue_ == nullptr) {
work_item();
@ -158,43 +156,64 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
});
}
void SparsePoseGraph::ComputeConstraint(const int scan_index,
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
const transform::Rigid3d relative_pose = optimization_problem_.submap_data()
const transform::Rigid3d inverse_submap_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const int scan_trajectory_id =
trajectory_nodes_[scan_index].constant_data->trajectory_id;
.pose.inverse();
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const mapping::NodeId& submap_node_id :
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.node_ids) {
submap_nodes.push_back(mapping::TrajectoryNode{
trajectory_nodes_.at(submap_node_id.trajectory_id)
.at(submap_node_id.node_index)
.constant_data,
inverse_submap_pose * trajectory_nodes_.at(submap_node_id.trajectory_id)
.at(submap_node_id.node_index)
.pose});
}
// Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_);
node_id,
&trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_3d.returns,
submap_nodes, &trajectory_connectivity_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
reverse_connected_components_.at(scan_trajectory_id) ==
reverse_connected_components_.at(node_id.trajectory_id) ==
reverse_connected_components_.at(submap_id.trajectory_id);
if (scan_trajectory_id == submap_id.trajectory_id ||
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
const transform::Rigid3d initial_relative_pose =
inverse_submap_pose * optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
constraint_builder_.MaybeAddConstraint(
submap_id,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
node_id, scan_index, trajectory_nodes_, relative_pose);
node_id,
&trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_3d.returns,
submap_nodes, initial_relative_pose);
}
}
}
@ -206,7 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
ComputeConstraint(scan_index, submap_id);
ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
}
}
}
@ -231,7 +250,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
@ -266,7 +287,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(submap_index)
.node_ids.count(node_id),
0);
ComputeConstraint(scan_index,
ComputeConstraint(node_id,
mapping::SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(submap_index)});
}
@ -284,7 +305,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
ComputeConstraintsForOldScans(finished_submap);
finished_submap_state.finished = true;
}
constraint_builder_.NotifyEndOfScan(scan_index);
constraint_builder_.NotifyEndOfScan();
++num_scans_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 &&
num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@ -328,8 +349,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
return constraint_builder_.GetNumFinishedScans() ==
num_trajectory_nodes_;
},
common::FromSeconds(1.))) {
std::ostringstream progress_info;
@ -337,8 +358,7 @@ void SparsePoseGraph::WaitForAllComputations() {
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
(num_trajectory_nodes_ - num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
@ -372,29 +392,27 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) {
const mapping::NodeId node_id = scan_index_to_node_id_[i];
trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
++trajectory_id) {
size_t node_index = 0;
const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
for (; node_index != node_data[trajectory_id].size(); ++node_index) {
trajectory_nodes_[trajectory_id][node_index].pose =
node_data[trajectory_id][node_index].point_cloud_pose;
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
if (extrapolation_transforms.count(trajectory_id) == 0) {
const auto new_submap_transforms = ExtrapolateSubmapTransforms(
optimization_problem_.submap_data(), trajectory_id);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory_id);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory_id] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
const transform::Rigid3d extrapolation_transform =
new_submap_transforms.back() * old_submap_transforms.back().inverse();
for (; node_index < num_nodes; ++node_index) {
trajectory_nodes_[trajectory_id][node_index].pose =
extrapolation_transform *
trajectory_nodes_[trajectory_id][node_index].pose;
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -409,13 +427,7 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result;
for (const auto& node : trajectory_nodes_) {
result.resize(
std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

View File

@ -73,10 +73,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
// The index into the flat vector of trajectory nodes used internally for the
// next node added with AddScan() is returned.
int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
@ -131,7 +127,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const int scan_index,
void ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished.
@ -202,7 +198,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Deque to keep references valid for the background computation when adding
// new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
GUARDED_BY(mutex_);
int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>

View File

@ -39,25 +39,6 @@ namespace cartographer {
namespace mapping_3d {
namespace sparse_pose_graph {
namespace {
std::vector<mapping::TrajectoryNode> ComputeSubmapNodes(
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const Submap* const submap, const int flat_scan_index,
const transform::Rigid3d& initial_relative_pose) {
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const int node_index : submap->trajectory_node_indices) {
submap_nodes.push_back(mapping::TrajectoryNode{
trajectory_nodes[node_index].constant_data,
transform::Rigid3d(initial_relative_pose *
trajectory_nodes[flat_scan_index].pose.inverse() *
trajectory_nodes[node_index].pose)});
}
return submap_nodes;
}
} // namespace
ConstraintBuilder::ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool)
@ -77,31 +58,27 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
return;
}
if (sampler_.Pulse()) {
const auto submap_nodes = ComputeSubmapNodes(
trajectory_nodes, submap, flat_scan_index, initial_relative_pose);
common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
const auto* const point_cloud =
&trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
ComputeConstraint(
submap_id, submap, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
compressed_point_cloud, initial_relative_pose, constraint);
FinishComputation(current_computation);
});
}
@ -109,34 +86,28 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
const auto submap_nodes =
ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index,
transform::Rigid3d::Identity());
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
const auto* const point_cloud =
&trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
trajectory_connectivity, compressed_point_cloud,
transform::Rigid3d::Identity(), constraint);
FinishComputation(current_computation);
});
}
void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
void ConstraintBuilder::NotifyEndOfScan() {
common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_;
}

View File

@ -68,35 +68,35 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
// 'submap_id', and the 'point_cloud' for 'node_id'.
// The 'initial_relative_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'flat_scan_index'. This performs full-submap matching.
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The scan at 'flat_scan_index' should be from 'node_id.trajectory_id'.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
mapping::TrajectoryConnectivity* trajectory_connectivity);
// Must be called after all computations related to 'flat_scan_index' are
// added.
void NotifyEndOfScan(int flat_scan_index);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.
@ -133,14 +133,14 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will
// connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
// connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const sensor::CompressedPointCloud* compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);

View File

@ -273,8 +273,7 @@ void Submaps::SubmapToProto(
global_submap_pose.translation().z())));
}
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
const int trajectory_node_index) {
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
range_data_inserter_.Insert(
@ -284,7 +283,6 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
range_data_inserter_.Insert(range_data,
&submap->low_resolution_hybrid_grid);
++submap->num_range_data;
submap->trajectory_node_indices.push_back(trajectory_node_index);
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {

View File

@ -23,6 +23,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/port.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h"
@ -52,8 +53,6 @@ struct Submap : public mapping::Submap {
HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid;
bool finished = false;
// Indices into the nodes of the SparsePoseGraph used to visualize the submap.
std::vector<int> trajectory_node_indices;
};
// A container of Submaps.
@ -71,8 +70,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data,
int trajectory_node_index);
void InsertRangeData(const sensor::RangeData& range_data);
// Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const;