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

Fix race condition in extrapolation of submap poses. (#268) 

PAIR=wohe
master
Holger Rapp 2017-05-09 15:21:30 +02:00 committed by GitHub
parent c664d7b966
commit 20e9cde53d
9 changed files with 128 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer/mapping/map_builder.cc
View File

@ -128,17 +128,17 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
const Submaps* const submaps =
trajectory_builders_.at(trajectory_id)->submaps();
if (submap_index < 0 || submap_index >= submaps->size()) {
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(submaps);
if (submap_index < 0 ||
static_cast<size_t>(submap_index) >= submap_transforms.size()) {
return "Requested submap " + std::to_string(submap_index) +
" from trajectory " + std::to_string(trajectory_id) +
" but there are only " + std::to_string(submaps->size()) +
" but there are only " + std::to_string(submap_transforms.size()) +
" submaps in this trajectory.";
}
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());
submaps->SubmapToProto(submap_index, sparse_pose_graph_->GetTrajectoryNodes(),
submap_transforms[submap_index], response);
return "";

4
cartographer/mapping/sparse_pose_graph.cc
View File

@ -150,8 +150,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
}
const Submaps* const submaps = group[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(*submaps)) {
const Submaps* const trajectory = group[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(trajectory)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}

6
cartographer/mapping/sparse_pose_graph.h
View File

@ -109,15 +109,15 @@ class SparsePoseGraph {
virtual std::vector<std::vector<const Submaps*>>
GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'submaps'.
// Returns the current optimized transforms for the given 'trajectory'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
const Submaps& submaps) = 0;
const Submaps* trajectory) = 0;
// Returns the transform converting data in the local map frame (i.e. the
// continuous, non-loop-closed frame) into the global map frame (i.e. the
// discontinuous, loop-closed frame).
virtual transform::Rigid3d GetLocalToGlobalTransform(
const Submaps& submaps) = 0;
const Submaps* submaps) = 0;
// Returns the current optimized trajectory.
virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;

2
cartographer/mapping/submaps.h
View File

@ -75,7 +75,7 @@ struct Submap {
}
// Origin of this submap.
Eigen::Vector3f origin;
const Eigen::Vector3f origin;
// Number of RangeData inserted.
int num_range_data = 0;

90
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -84,11 +84,11 @@ void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance,
const mapping::Submaps* submaps,
const mapping::Submaps* const trajectory,
const mapping::Submap* const matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) *
transform::Embed3D(pose));
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
const int j = trajectory_nodes_.size();
@ -96,19 +96,19 @@ void SparsePoseGraph::AddScan(
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory,
tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_.back(), optimized_pose,
});
trajectory_connectivity_.Add(submaps);
trajectory_connectivity_.Add(trajectory);
if (submap_indices_.count(insertion_submaps.back()) == 0) {
submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = submaps;
submap_states_.back().trajectory = trajectory;
CHECK_EQ(submap_states_.size(), submap_indices_.size());
}
const mapping::Submap* const finished_submap =
@ -117,8 +117,8 @@ void SparsePoseGraph::AddScan(
: nullptr;
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[submaps]) {
global_localization_samplers_[submaps] =
if (!global_localization_samplers_[trajectory]) {
global_localization_samplers_[trajectory] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
@ -347,13 +347,14 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
const auto new_submap_transforms =
ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@ -384,9 +385,12 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps& submaps) {
return GetSubmapTransforms(submaps).back() *
submaps.Get(submaps.size() - 1)->local_pose().inverse();
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
return extrapolated_submap_transforms.back() *
submaps->Get(extrapolated_submap_transforms.size() - 1)
->local_pose()
.inverse();
}
std::vector<std::vector<const mapping::Submaps*>>
@ -396,39 +400,51 @@ SparsePoseGraph::GetConnectedTrajectories() {
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps& submaps) {
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
}
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid2d>& submap_transforms,
const mapping::Submaps& submaps) const {
const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result;
int i = 0;
size_t flat_index = 0;
size_t flat_index_of_result_back = -1;
// Submaps for which we have optimized poses.
for (; i < submaps.size(); ++i) {
const mapping::Submap* submap = submaps.Get(i);
const auto submap_and_index = submap_indices_.find(submap);
// Determine if we have a loop-closed transform for this submap.
if (submap_and_index == submap_indices_.end() ||
static_cast<size_t>(submap_and_index->second) >=
submap_transforms.size()) {
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
continue;
}
if (flat_index >= submap_transforms.size()) {
break;
}
result.push_back(
transform::Embed3D(submap_transforms[submap_and_index->second]));
result.push_back(transform::Embed3D(submap_transforms[flat_index]));
flat_index_of_result_back = flat_index;
}
// Extrapolate to the remaining submaps.
for (; i < submaps.size(); ++i) {
if (i == 0) {
result.push_back(transform::Rigid3d::Identity());
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
continue;
}
result.push_back(result.back() *
submaps.Get(result.size() - 1)->local_pose().inverse() *
submaps.Get(result.size())->local_pose());
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Accessing local_pose() in Submaps is okay, since the member is const.
result.push_back(result.back() *
submap_states_[flat_index_of_result_back]
.submap->local_pose()
.inverse() *
state.submap->local_pose());
}
flat_index_of_result_back = flat_index;
}
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
}
return result;
}

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -72,7 +72,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance,
const mapping::Submaps* submaps,
const mapping::Submaps* trajectory,
const mapping::Submap* matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps)
EXCLUDES(mutex_);
@ -86,8 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps& submaps) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps)
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
@ -139,7 +139,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds extrapolated transforms, so that there are transforms for all submaps.
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid2d>& submap_transforms,
const mapping::Submaps& submaps) const REQUIRES(mutex_);
const mapping::Submaps* trajectory) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_;

91
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -83,11 +83,11 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance, const Submaps* submaps,
const Submap* const matching_submap,
const kalman_filter::PoseCovariance& covariance,
const Submaps* const trajectory, const Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) *
pose);
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory) * pose);
common::MutexLocker locker(&mutex_);
const int j = trajectory_nodes_.size();
@ -95,26 +95,26 @@ void SparsePoseGraph::AddScan(
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), submaps,
sensor::Compress(range_data_in_tracking), trajectory,
transform::Rigid3d::Identity()});
trajectory_nodes_.push_back(
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
trajectory_connectivity_.Add(submaps);
trajectory_connectivity_.Add(trajectory);
if (submap_indices_.count(insertion_submaps.back()) == 0) {
submap_indices_.emplace(insertion_submaps.back(),
static_cast<int>(submap_indices_.size()));
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = submaps;
submap_states_.back().trajectory = trajectory;
CHECK_EQ(submap_states_.size(), submap_indices_.size());
}
const Submap* const finished_submap =
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[submaps]) {
global_localization_samplers_[submaps] =
if (!global_localization_samplers_[trajectory]) {
global_localization_samplers_[trajectory] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
@ -341,13 +341,14 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
const auto new_submap_transforms =
ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() *
old_submap_transforms.back().inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@ -388,9 +389,12 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps& submaps) {
return GetSubmapTransforms(submaps).back() *
submaps.Get(submaps.size() - 1)->local_pose().inverse();
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
return extrapolated_submap_transforms.back() *
submaps->Get(extrapolated_submap_transforms.size() - 1)
->local_pose()
.inverse();
}
std::vector<std::vector<const mapping::Submaps*>>
@ -400,38 +404,51 @@ SparsePoseGraph::GetConnectedTrajectories() {
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps& submaps) {
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps);
return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
}
std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid3d>& submap_transforms,
const mapping::Submaps& submaps) const {
const mapping::Submaps* const trajectory) const {
std::vector<transform::Rigid3d> result;
int i = 0;
size_t flat_index = 0;
size_t flat_index_of_result_back = -1;
// Submaps for which we have optimized poses.
for (; i < submaps.size(); ++i) {
const mapping::Submap* submap = submaps.Get(i);
const auto submap_and_index = submap_indices_.find(submap);
// Determine if we have a loop-closed transform for this submap.
if (submap_and_index == submap_indices_.end() ||
static_cast<size_t>(submap_and_index->second) >=
submap_transforms.size()) {
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
continue;
}
if (flat_index >= submap_transforms.size()) {
break;
}
result.push_back(submap_transforms[submap_and_index->second]);
result.push_back(submap_transforms[flat_index]);
flat_index_of_result_back = flat_index;
}
// Extrapolate to the remaining submaps.
for (; i < submaps.size(); ++i) {
if (i == 0) {
result.push_back(transform::Rigid3d::Identity());
for (; flat_index != submap_states_.size(); ++flat_index) {
const auto& state = submap_states_[flat_index];
if (state.trajectory != trajectory) {
continue;
}
result.push_back(result.back() *
submaps.Get(result.size() - 1)->local_pose().inverse() *
submaps.Get(result.size())->local_pose());
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Accessing local_pose() in Submaps is okay, since the member is const.
result.push_back(result.back() *
submap_states_[flat_index_of_result_back]
.submap->local_pose()
.inverse() *
state.submap->local_pose());
}
flat_index_of_result_back = flat_index;
}
if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
}
return result;
}

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap,
const Submaps* trajectory, const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
@ -88,8 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps& submaps) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps)
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
@ -159,7 +159,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds extrapolated transforms, so that there are transforms for all submaps.
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid3d>& submap_transforms,
const mapping::Submaps& submaps) const REQUIRES(mutex_);
const mapping::Submaps* trajectory) const REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_;

2
cartographer/mapping_3d/submaps.h
View File

@ -109,6 +109,8 @@ class Submaps : public mapping::Submaps {
const proto::SubmapsOptions options_;
// 'submaps_' contains pointers, so that resizing the vector does not
// invalidate handed out Submap* pointers.
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_;
};