Keep 'trajectory_nodes_' per trajectory. (#291)

Fixes #256.

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

View File

@ -97,16 +97,19 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); 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() {

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

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

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.

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;

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

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

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;

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;

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

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

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

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

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

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

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

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;