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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
cartographer/mapping_2d/sparse_pose_graph.cc
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,46 +160,49 @@ 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()
.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;
// 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_[scan_index].constant_data->range_data_2d.returns);
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(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::Rigid2d initial_relative_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;
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);
}
// 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());
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.
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());
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() {

6
cartographer/mapping_2d/sparse_pose_graph.h
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>>

13
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
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_;
}

23
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
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.

4
cartographer/mapping_3d/global_trajectory_builder.cc
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;

22
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
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});

9
cartographer/mapping_3d/kalman_local_trajectory_builder.h
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_;

3
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
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;

2
cartographer/mapping_3d/local_trajectory_builder_interface.h
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;

25
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
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();

12
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
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_;

138
cartographer/mapping_3d/sparse_pose_graph.cc
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()
.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;
const transform::Rigid3d inverse_submap_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.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;
}
// 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());
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.
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());
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() {

10
cartographer/mapping_3d/sparse_pose_graph.h
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>>

55
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
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 */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
ComputeConstraint(
submap_id, submap, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
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_;
}

34
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
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_);

4
cartographer/mapping_3d/submaps.cc
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()) {

6
cartographer/mapping_3d/submaps.h
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;