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)); GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_); 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()); CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose, time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose}); 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, &constant_node_data_.back(), optimized_pose,
}); });
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id); trajectory_connectivity_.Add(trajectory_id);
if (submap_ids_.count(insertion_submaps.back()) == 0) { 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::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. // Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id && if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) { global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_id,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
node_id, scan_index, &trajectory_connectivity_, node_id,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); &trajectory_nodes_.at(node_id.trajectory_id)
.at(node_id.node_index)
.constant_data->range_data_2d.returns,
&trajectory_connectivity_);
} else { } else {
const bool scan_and_submap_trajectories_connected = 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_.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); 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) { 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( constraint_builder_.MaybeAddConstraint(
submap_id, submap_id,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
node_id, scan_index, node_id,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns, &trajectory_nodes_.at(node_id.trajectory_id)
relative_pose); .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(); const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { 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) { 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); scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id]; ++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = 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); CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose); matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
@ -273,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(submap_index) .at(submap_index)
.node_ids.count(node_id), .node_ids.count(node_id),
0); 0);
ComputeConstraint(scan_index, ComputeConstraint(node_id,
mapping::SubmapId{static_cast<int>(trajectory_id), mapping::SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(submap_index)}); static_cast<int>(submap_index)});
} }
@ -291,7 +299,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
ComputeConstraintsForOldScans(finished_submap); ComputeConstraintsForOldScans(finished_submap);
finished_submap_state.finished = true; finished_submap_state.finished = true;
} }
constraint_builder_.NotifyEndOfScan(scan_index); constraint_builder_.NotifyEndOfScan();
++num_scans_since_last_loop_closure_; ++num_scans_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && if (options_.optimize_every_n_scans() > 0 &&
num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@ -335,8 +343,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.GetNumFinishedScans(); constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout( while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { [this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) == return constraint_builder_.GetNumFinishedScans() ==
trajectory_nodes_.size(); num_trajectory_nodes_;
}, },
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
@ -344,8 +352,7 @@ void SparsePoseGraph::WaitForAllComputations() {
<< 100. * << 100. *
(constraint_builder_.GetNumFinishedScans() - (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) / num_finished_scans_at_start) /
(trajectory_nodes_.size() - (num_trajectory_nodes_ - num_finished_scans_at_start)
num_finished_scans_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }
@ -379,30 +386,27 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { ++trajectory_id) {
const mapping::NodeId node_id = scan_index_to_node_id_[i]; size_t node_index = 0;
trajectory_nodes_[i].pose = const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
transform::Embed3D(node_data.at(node_id.trajectory_id) for (; node_index != node_data[trajectory_id].size(); ++node_index) {
.at(node_id.node_index) trajectory_nodes_[trajectory_id][node_index].pose = transform::Embed3D(
.point_cloud_pose); node_data[trajectory_id][node_index].point_cloud_pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<int, transform::Rigid3d> extrapolation_transforms; const auto new_submap_transforms = ExtrapolateSubmapTransforms(
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { optimization_problem_.submap_data(), trajectory_id);
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id; const auto old_submap_transforms = ExtrapolateSubmapTransforms(
if (extrapolation_transforms.count(trajectory_id) == 0) { optimized_submap_transforms_, trajectory_id);
const auto new_submap_transforms = ExtrapolateSubmapTransforms( CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
optimization_problem_.submap_data(), trajectory_id); const transform::Rigid3d extrapolation_transform =
const auto old_submap_transforms = ExtrapolateSubmapTransforms( new_submap_transforms.back() * old_submap_transforms.back().inverse();
optimized_submap_transforms_, trajectory_id); for (; node_index < num_nodes; ++node_index) {
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); trajectory_nodes_[trajectory_id][node_index].pose =
extrapolation_transforms[trajectory_id] = extrapolation_transform *
transform::Rigid3d(new_submap_transforms.back() * trajectory_nodes_[trajectory_id][node_index].pose;
old_submap_transforms.back().inverse());
} }
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
} }
optimized_submap_transforms_ = optimization_problem_.submap_data(); optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -417,13 +421,7 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>> std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() { SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result; return trajectory_nodes_;
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;
} }
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { 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_); const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair. // 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_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished. // 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 // Deque to keep references valid for the background computation when adding
// new data. // new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_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. // Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>> 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( void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) { const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() > if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) { options_.max_constraint_distance()) {
@ -71,7 +70,6 @@ void ConstraintBuilder::MaybeAddConstraint(
} }
if (sampler_.Pulse()) { if (sampler_.Pulse()) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back(); constraints_.emplace_back();
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
@ -89,11 +87,9 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* const trajectory_connectivity) {
const sensor::PointCloud* const point_cloud) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back(); constraints_.emplace_back();
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_]; ++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_); common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_; ++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; ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by // Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'flat_scan_index'. // 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
// The 'initial_pose' is relative to the 'submap'. // relative to the 'submap'.
// //
// The pointees of 'submap' and 'point_cloud' must stay valid until all // The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished. // computations are finished.
void MaybeAddConstraint(const mapping::SubmapId& submap_id, void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const mapping::Submap* submap, const mapping::Submap* submap,
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
const int flat_scan_index,
const sensor::PointCloud* point_cloud, const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose); const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by // Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs // 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
// full-submap matching. // matching.
// //
// The scan at 'flat_scan_index' should be from trajectory // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
// 'node_id.trajectory_id'. The 'trajectory_connectivity' is updated if the
// full-submap match succeeds.
// //
// The pointees of 'submap' and 'point_cloud' must stay valid until all // The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished. // computations are finished.
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::SubmapId& submap_id, const mapping::Submap* submap,
const mapping::NodeId& node_id, const int flat_scan_index, const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity);
const sensor::PointCloud* point_cloud);
// Must be called after all computations related to 'flat_scan_index' are // Must be called after all computations related to one node have been added.
// added. void NotifyEndOfScan();
void NotifyEndOfScan(const int flat_scan_index);
// Registers the 'callback' to be called with the results, after all // Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished. // 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( void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin, const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) { const sensor::PointCloud& ranges) {
auto insertion_result = local_trajectory_builder_->AddRangefinderData( auto insertion_result =
time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex()); local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
if (insertion_result == nullptr) { if (insertion_result == nullptr) {
return; return;

22
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -72,7 +72,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddRangefinderData( KalmanLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin, const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, int next_trajectory_node_index) { const sensor::PointCloud& ranges) {
if (!pose_tracker_) { if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized."; LOG(INFO) << "PoseTracker not yet initialized.";
return nullptr; return nullptr;
@ -114,18 +114,15 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
if (num_accumulated_ >= options_.scans_per_accumulation()) { if (num_accumulated_ >= options_.scans_per_accumulation()) {
num_accumulated_ = 0; num_accumulated_ = 0;
return AddAccumulatedRangeData( return AddAccumulatedRangeData(
time, time, sensor::TransformRangeData(accumulated_range_data_,
sensor::TransformRangeData(accumulated_range_data_, tracking_delta.inverse()));
tracking_delta.inverse()),
next_trajectory_node_index);
} }
return nullptr; return nullptr;
} }
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::RangeData& range_data_in_tracking, const common::Time time, const sensor::RangeData& range_data_in_tracking) {
const int trajectory_node_index) {
const sensor::RangeData filtered_range_data = { const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin, range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns, sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -184,7 +181,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
pose_observation.cast<float>())}; pose_observation.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, pose_observation, return InsertIntoSubmap(time, filtered_range_data, pose_observation,
covariance_estimate, trajectory_node_index); covariance_estimate);
} }
void KalmanLocalTrajectoryBuilder::AddOdometerData( void KalmanLocalTrajectoryBuilder::AddOdometerData(
@ -213,8 +210,7 @@ std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap( KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking, const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation, const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate, const kalman_filter::PoseCovariance& covariance_estimate) {
const int trajectory_node_index) {
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
} }
@ -224,10 +220,8 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
submaps_->InsertRangeData( submaps_->InsertRangeData(sensor::TransformRangeData(
sensor::TransformRangeData(range_data_in_tracking, range_data_in_tracking, pose_observation.cast<float>()));
pose_observation.cast<float>()),
trajectory_node_index);
return std::unique_ptr<InsertionResult>(new InsertionResult{ return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate, time, range_data_in_tracking, pose_observation, covariance_estimate,
matching_submap, insertion_submaps}); 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; const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData( std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin, common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, const sensor::PointCloud& ranges) override;
int next_trajectory_node_index) override;
void AddOdometerData(common::Time time, void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override; const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override; const mapping_3d::Submaps* submaps() const override;
@ -59,14 +58,12 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
private: private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData( std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& range_data_in_tracking, common::Time time, const sensor::RangeData& range_data_in_tracking);
int trajectory_node_index);
std::unique_ptr<InsertionResult> InsertIntoSubmap( std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_tracking, common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation, const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate, const kalman_filter::PoseCovariance& covariance_estimate);
int trajectory_node_index);
const proto::LocalTrajectoryBuilderOptions options_; const proto::LocalTrajectoryBuilderOptions options_;
std::unique_ptr<mapping_3d::Submaps> submaps_; 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); AddLinearOnlyImuObservation(node.time, node.pose);
const auto range_data = GenerateRangeData(node.pose); const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangefinderData( if (local_trajectory_builder_->AddRangefinderData(
node.time, range_data.origin, range_data.returns, num_poses) != node.time, range_data.origin, range_data.returns) != nullptr) {
nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate(); const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses; ++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; const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddRangefinderData( virtual std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin, 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, virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0; 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> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddRangefinderData( OptimizingLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin, 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); CHECK_GT(ranges.size(), 0);
// TODO(hrapp): Handle misses. // TODO(hrapp): Handle misses.
@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
++num_accumulated_; ++num_accumulated_;
RemoveObsoleteSensorData(); RemoveObsoleteSensorData();
return MaybeOptimize(time, next_trajectory_node_index); return MaybeOptimize(time);
} }
void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() { void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
@ -215,8 +215,7 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
} }
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::MaybeOptimize( OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
const common::Time time, const int trajectory_node_index) {
// TODO(hrapp): Make the number of optimizations configurable. // TODO(hrapp): Make the number of optimizations configurable.
if (num_accumulated_ < options_.scans_per_accumulation() && if (num_accumulated_ < options_.scans_per_accumulation() &&
num_accumulated_ % 10 != 0) { num_accumulated_ % 10 != 0) {
@ -349,15 +348,13 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(
} }
return AddAccumulatedRangeData(time, optimized_pose, return AddAccumulatedRangeData(time, optimized_pose,
accumulated_range_data_in_tracking, accumulated_range_data_in_tracking);
trajectory_node_index);
} }
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData( OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const transform::Rigid3d& optimized_pose, const common::Time time, const transform::Rigid3d& optimized_pose,
const sensor::RangeData& range_data_in_tracking, const sensor::RangeData& range_data_in_tracking) {
const int trajectory_node_index) {
const sensor::RangeData filtered_range_data = { const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin, range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns, sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -375,8 +372,7 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
sensor::TransformPointCloud(filtered_range_data.returns, sensor::TransformPointCloud(filtered_range_data.returns,
optimized_pose.cast<float>())}; optimized_pose.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, optimized_pose, return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
trajectory_node_index);
} }
const OptimizingLocalTrajectoryBuilder::PoseEstimate& const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@ -387,8 +383,7 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const {
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking, const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation, const transform::Rigid3d& pose_observation) {
const int trajectory_node_index) {
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
} }
@ -398,10 +393,8 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
submaps_->InsertRangeData( submaps_->InsertRangeData(sensor::TransformRangeData(
sensor::TransformRangeData(range_data_in_tracking, range_data_in_tracking, pose_observation.cast<float>()));
pose_observation.cast<float>()),
trajectory_node_index);
const kalman_filter::PoseCovariance kCovariance = const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity(); 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; const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData( std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin, common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges, const sensor::PointCloud& ranges) override;
int next_trajectory_node_index) override;
void AddOdometerData(common::Time time, void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override; const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override; const mapping_3d::Submaps* submaps() const override;
@ -102,16 +101,13 @@ class OptimizingLocalTrajectoryBuilder
std::unique_ptr<InsertionResult> AddAccumulatedRangeData( std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const transform::Rigid3d& pose_observation, common::Time time, const transform::Rigid3d& pose_observation,
const sensor::RangeData& range_data_in_tracking, const sensor::RangeData& range_data_in_tracking);
const int next_trajectory_node_index);
std::unique_ptr<InsertionResult> InsertIntoSubmap( std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking, const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation, const transform::Rigid3d& pose_observation);
const int next_trajectory_node_index);
std::unique_ptr<InsertionResult> MaybeOptimize( std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
common::Time time, const int next_trajectory_node_index);
const proto::LocalTrajectoryBuilderOptions options_; const proto::LocalTrajectoryBuilderOptions options_;
const ceres::Solver::Options ceres_solver_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); GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_); 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()); CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id, sensor::Compress(range_data_in_tracking), trajectory_id,
transform::Rigid3d::Identity()}); 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}); mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id); trajectory_connectivity_.Add(trajectory_id);
if (submap_ids_.count(insertion_submaps.back()) == 0) { 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) { void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
if (scan_queue_ == nullptr) { if (scan_queue_ == nullptr) {
work_item(); 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::SubmapId& submap_id) {
const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index); const transform::Rigid3d inverse_submap_pose =
const transform::Rigid3d relative_pose = optimization_problem_.submap_data() optimization_problem_.submap_data()
.at(submap_id.trajectory_id) .at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.pose.inverse() * .pose.inverse();
optimization_problem_.node_data()
.at(node_id.trajectory_id) std::vector<mapping::TrajectoryNode> submap_nodes;
.at(node_id.node_index) for (const mapping::NodeId& submap_node_id :
.point_cloud_pose; submap_states_.at(submap_id.trajectory_id)
const int scan_trajectory_id = .at(submap_id.submap_index)
trajectory_nodes_[scan_index].constant_data->trajectory_id; .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. // Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_id.trajectory_id && if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) { global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_id,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .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 { } else {
const bool scan_and_submap_trajectories_connected = 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_.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); 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) { 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( constraint_builder_.MaybeAddConstraint(
submap_id, submap_id,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .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(); const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { 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) { 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); scan_index_to_node_id_.push_back(node_id);
++num_nodes_in_trajectory_[matching_id.trajectory_id]; ++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = 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); CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose); scan_data->time, optimized_pose);
@ -266,7 +287,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(submap_index) .at(submap_index)
.node_ids.count(node_id), .node_ids.count(node_id),
0); 0);
ComputeConstraint(scan_index, ComputeConstraint(node_id,
mapping::SubmapId{static_cast<int>(trajectory_id), mapping::SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(submap_index)}); static_cast<int>(submap_index)});
} }
@ -284,7 +305,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
ComputeConstraintsForOldScans(finished_submap); ComputeConstraintsForOldScans(finished_submap);
finished_submap_state.finished = true; finished_submap_state.finished = true;
} }
constraint_builder_.NotifyEndOfScan(scan_index); constraint_builder_.NotifyEndOfScan();
++num_scans_since_last_loop_closure_; ++num_scans_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && if (options_.optimize_every_n_scans() > 0 &&
num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@ -328,8 +349,8 @@ void SparsePoseGraph::WaitForAllComputations() {
constraint_builder_.GetNumFinishedScans(); constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout( while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { [this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) == return constraint_builder_.GetNumFinishedScans() ==
trajectory_nodes_.size(); num_trajectory_nodes_;
}, },
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
@ -337,8 +358,7 @@ void SparsePoseGraph::WaitForAllComputations() {
<< 100. * << 100. *
(constraint_builder_.GetNumFinishedScans() - (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) / num_finished_scans_at_start) /
(trajectory_nodes_.size() - (num_trajectory_nodes_ - num_finished_scans_at_start)
num_finished_scans_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }
@ -372,29 +392,27 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { ++trajectory_id) {
const mapping::NodeId node_id = scan_index_to_node_id_[i]; size_t node_index = 0;
trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id) const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
.at(node_id.node_index) for (; node_index != node_data[trajectory_id].size(); ++node_index) {
.point_cloud_pose; 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; // Extrapolate all point cloud poses that were added later.
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { const auto new_submap_transforms = ExtrapolateSubmapTransforms(
const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id; optimization_problem_.submap_data(), trajectory_id);
if (extrapolation_transforms.count(trajectory_id) == 0) { const auto old_submap_transforms = ExtrapolateSubmapTransforms(
const auto new_submap_transforms = ExtrapolateSubmapTransforms( optimized_submap_transforms_, trajectory_id);
optimization_problem_.submap_data(), trajectory_id); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
const auto old_submap_transforms = ExtrapolateSubmapTransforms( const transform::Rigid3d extrapolation_transform =
optimized_submap_transforms_, trajectory_id); new_submap_transforms.back() * old_submap_transforms.back().inverse();
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); for (; node_index < num_nodes; ++node_index) {
extrapolation_transforms[trajectory_id] = trajectory_nodes_[trajectory_id][node_index].pose =
transform::Rigid3d(new_submap_transforms.back() * extrapolation_transform *
old_submap_transforms.back().inverse()); 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(); optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents(); connected_components_ = trajectory_connectivity_.ConnectedComponents();
@ -409,13 +427,7 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>> std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() { SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result; return trajectory_nodes_;
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;
} }
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { 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) const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_); 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. // Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
@ -131,7 +127,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair. // 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_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished. // 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 // Deque to keep references valid for the background computation when adding
// new data. // new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_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. // Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>> 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 mapping_3d {
namespace sparse_pose_graph { 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( ConstraintBuilder::ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool) common::ThreadPool* const thread_pool)
@ -77,31 +58,27 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index, const mapping::NodeId& node_id,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes, const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose) { const transform::Rigid3d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() > if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) { options_.max_constraint_distance()) {
return; return;
} }
if (sampler_.Pulse()) { if (sampler_.Pulse()) {
const auto submap_nodes = ComputeSubmapNodes(
trajectory_nodes, submap, flat_scan_index, initial_relative_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back(); constraints_.emplace_back();
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
const int current_computation = 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( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, ComputeConstraint(
false, /* match_full_submap */ submap_id, submap, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */ nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint); compressed_point_cloud, initial_relative_pose, constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
}); });
} }
@ -109,34 +86,28 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const int flat_scan_index, const mapping::NodeId& node_id,
mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) { const std::vector<mapping::TrajectoryNode>& submap_nodes,
const auto submap_nodes = mapping::TrajectoryConnectivity* const trajectory_connectivity) {
ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index,
transform::Rigid3d::Identity());
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back(); constraints_.emplace_back();
auto* const constraint = &constraints_.back(); auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
const int current_computation = 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( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */ true, /* match_full_submap */
trajectory_connectivity, point_cloud, trajectory_connectivity, compressed_point_cloud,
transform::Rigid3d::Identity(), constraint); transform::Rigid3d::Identity(), constraint);
FinishComputation(current_computation); FinishComputation(current_computation);
}); });
} }
void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) { void ConstraintBuilder::NotifyEndOfScan() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_; ++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; ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by // Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'submap_id', and the 'point_cloud' for 'node_id'.
// 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'. // 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. // all computations are finished.
void MaybeAddConstraint( void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index, const mapping::NodeId& node_id,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes, const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose); const transform::Rigid3d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by // Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// 'flat_scan_index'. This performs full-submap matching. // 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 '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. // all computations are finished.
void MaybeAddGlobalConstraint( void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index, const mapping::NodeId& node_id,
mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes); const std::vector<mapping::TrajectoryNode>& submap_nodes,
mapping::TrajectoryConnectivity* trajectory_connectivity);
// Must be called after all computations related to 'flat_scan_index' are // Must be called after all computations related to one node have been added.
// added. void NotifyEndOfScan();
void NotifyEndOfScan(int flat_scan_index);
// Registers the 'callback' to be called with the results, after all // Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished. // computations triggered by MaybeAddConstraint() have finished.
@ -133,14 +133,14 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional // Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore. // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will // 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'. // 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'. // As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint( void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap, const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity, mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud, const sensor::CompressedPointCloud* compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose, const transform::Rigid3d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_); 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()))); global_submap_pose.translation().z())));
} }
void Submaps::InsertRangeData(const sensor::RangeData& range_data, void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
const int trajectory_node_index) {
for (const int index : insertion_indices()) { for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get(); Submap* submap = submaps_[index].get();
range_data_inserter_.Insert( range_data_inserter_.Insert(
@ -284,7 +283,6 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
range_data_inserter_.Insert(range_data, range_data_inserter_.Insert(range_data,
&submap->low_resolution_hybrid_grid); &submap->low_resolution_hybrid_grid);
++submap->num_range_data; ++submap->num_range_data;
submap->trajectory_node_indices.push_back(trajectory_node_index);
} }
const Submap* const last_submap = Get(size() - 1); const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) { 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 "Eigen/Geometry"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/probability_grid.h"
@ -52,8 +53,6 @@ struct Submap : public mapping::Submap {
HybridGrid high_resolution_hybrid_grid; HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid;
bool finished = false; 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. // A container of Submaps.
@ -71,8 +70,7 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) const override; mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'range_data' into the Submap collection. // Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data, void InsertRangeData(const sensor::RangeData& range_data);
int trajectory_node_index);
// Returns the 'high_resolution' HybridGrid to be used for matching. // Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const; const HybridGrid& high_resolution_matching_grid() const;