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

Use vector<map<>> instead of vector<deque> for submap_data. (#422)

master
Jihoon Lee 2017-08-23 12:16:42 +02:00 committed by Wolfgang Hess
parent 2e53586818
commit 2dd2d6f448
5 changed files with 61 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/pose_graph_trimmer.cc
View File

@ -28,8 +28,7 @@ PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
}
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
const int total_num_submaps = pose_graph->num_submaps(trajectory_id_);
while (total_num_submaps > num_submaps_trimmed_ + num_submaps_to_keep_) {
while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) {
const int submap_index_to_trim_next = num_submaps_trimmed_;
pose_graph->MarkSubmapAsTrimmed(
SubmapId{trajectory_id_, submap_index_to_trim_next});

95
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -64,25 +64,20 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
trajectory_id,
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
}
CHECK_EQ(optimization_problem_.num_trimmed_submaps(trajectory_id), 0);
CHECK_EQ(submap_data[trajectory_id].size(), 1);
const mapping::SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
return {submap_id};
}
CHECK_EQ(2, insertion_submaps.size());
const int num_trimmed_submaps =
optimization_problem_.num_trimmed_submaps(trajectory_id);
const mapping::SubmapId last_submap_id{
trajectory_id, static_cast<int>(submap_data.at(trajectory_id).size() +
num_trimmed_submaps - 1)};
trajectory_id,
submap_data.at(trajectory_id).rbegin()->first};
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new.
const auto& first_submap_pose =
submap_data.at(trajectory_id)
.at(last_submap_id.submap_index - num_trimmed_submaps)
.pose;
submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose;
optimization_problem_.AddSubmap(
trajectory_id,
first_submap_pose *
@ -194,14 +189,13 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index -
optimization_problem_.num_trimmed_submaps(
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 - optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.at(node_id.node_index -
optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.point_cloud_pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -241,12 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const int num_trimmed_submaps =
optimization_problem_.num_trimmed_submaps(trajectory_id);
const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index - num_trimmed_submaps)
.at(matching_id.submap_index)
.pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
@ -356,21 +348,19 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(num_trajectory_nodes_ - num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
constraint_builder_.WhenDone([this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;
});
locker.Await([&notification]() { return notification; });
}
@ -401,14 +391,11 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
optimized_submap_transforms_.size());
optimized_submap_transforms_.resize(submap_data_.num_trajectories());
CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
num_trimmed_submaps_at_last_optimization_.size());
num_trimmed_submaps_at_last_optimization_.resize(
submap_data_.num_trajectories());
CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
submap_id.submap_index);
optimized_submap_transforms_.at(trajectory_id)
.push_back(sparse_pose_graph::SubmapData{initial_pose_2d});
.emplace(submap_id.submap_index,
sparse_pose_graph::SubmapData{initial_pose_2d});
AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -447,14 +434,7 @@ void SparsePoseGraph::RunOptimization() {
optimization_problem_.Solve(constraints_, frozen_trajectories_);
common::MutexLocker locker(&mutex_);
std::vector<int> num_trimmed_submaps;
const auto& submap_data = optimization_problem_.submap_data();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) {
num_trimmed_submaps.push_back(
optimization_problem_.num_trimmed_submaps(trajectory_id));
}
const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@ -468,11 +448,10 @@ void SparsePoseGraph::RunOptimization() {
node_data[trajectory_id][node_data_index].point_cloud_pose);
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform(
submap_data, num_trimmed_submaps, trajectory_id);
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
trajectory_id);
optimized_submap_transforms_, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
for (; node_index < num_nodes; ++node_index) {
@ -482,7 +461,6 @@ void SparsePoseGraph::RunOptimization() {
}
}
optimized_submap_transforms_ = submap_data;
num_trimmed_submaps_at_last_optimization_ = num_trimmed_submaps;
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
@ -511,9 +489,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(
optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
trajectory_id);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id);
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -555,20 +532,19 @@ SparsePoseGraph::GetAllSubmapData() {
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
submap_transforms,
const std::vector<int>& num_trimmed_submaps,
const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity();
}
const mapping::SubmapId last_optimized_submap_id{
trajectory_id,
static_cast<int>(submap_transforms.at(trajectory_id).size() +
num_trimmed_submaps.at(trajectory_id) - 1)};
const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
// Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) *
return transform::Embed3D(
submap_transforms.at(trajectory_id).at(submap_index).pose) *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
@ -582,9 +558,7 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
auto submap = submap_data_.at(submap_id).submap;
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size())) {
const size_t submap_data_index =
submap_id.submap_index -
num_trimmed_submaps_at_last_optimization_.at(submap_id.trajectory_id);
const size_t submap_data_index = submap_id.submap_index;
if (submap_data_index <
optimized_submap_transforms_.at(submap_id.trajectory_id).size()) {
// We already have an optimized pose.
@ -595,10 +569,8 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
}
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(
optimized_submap_transforms_,
num_trimmed_submaps_at_last_optimization_,
submap_id.trajectory_id) *
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_id.trajectory_id) *
submap->local_pose()};
}
@ -607,8 +579,7 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const {
return parent_->optimization_problem_.submap_data().at(trajectory_id).size() +
parent_->optimization_problem_.num_trimmed_submaps(trajectory_id);
return parent_->optimization_problem_.submap_data().at(trajectory_id).size();
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -155,10 +155,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Computes the local to global frame transform based on the given optimized
// 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
submap_transforms,
const std::vector<int>& num_trimmed_submaps, int trajectory_id) const
REQUIRES(mutex_);
int trajectory_id) const REQUIRES(mutex_);
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
@ -205,8 +204,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
std::vector<int> num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_);
std::vector<std::deque<sparse_pose_graph::SubmapData>>
std::vector<std::map<int, sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.

50
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -112,19 +112,18 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
CHECK_GE(trajectory_id, 0);
submap_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
trajectory_data_.resize(
std::max(trajectory_data_.size(), submap_data_.size()));
auto& trajectory_data = trajectory_data_.at(trajectory_id);
submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
SubmapData{submap_pose});
++trajectory_data.next_submap_index;
}
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
auto& trajectory_data = trajectory_data_.at(submap_id.trajectory_id);
// We only allow trimming from the start.
CHECK_EQ(trajectory_data.num_trimmed_submaps, submap_id.submap_index);
auto& submap_data = submap_data_.at(submap_id.trajectory_id);
CHECK(!submap_data.empty());
submap_data.pop_front();
++trajectory_data.num_trimmed_submaps;
CHECK(submap_data.erase(submap_id.submap_index));
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -144,25 +143,27 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::vector<std::array<double, 3>>> C_submaps(
std::vector<std::map<int, std::array<double, 3>>> C_submaps(
submap_data_.size());
std::vector<std::vector<std::array<double, 3>>> C_nodes(node_data_.size());
bool first_submap = true;
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
const bool frozen = frozen_trajectories.count(trajectory_id);
// Reserve guarantees that data does not move, so the pointers for Ceres
// stay valid.
C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size());
for (const SubmapData& submap_data : submap_data_[trajectory_id]) {
C_submaps[trajectory_id].push_back(FromPose(submap_data.pose));
problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
for (const auto& index_submap_data : submap_data_[trajectory_id]) {
const int submap_index = index_submap_data.first;
const SubmapData& submap_data = index_submap_data.second;
C_submaps[trajectory_id].emplace(
submap_index, FromPose(submap_data.pose));
problem.AddParameterBlock(
C_submaps[trajectory_id].at(submap_index).data(), 3);
if (first_submap || frozen) {
first_submap = false;
// Fix the pose of the first submap or all submaps of a frozen
// trajectory.
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().data());
C_submaps[trajectory_id].at(submap_index).data());
}
}
}
@ -190,9 +191,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index -
trajectory_data_.at(constraint.submap_id.trajectory_id)
.num_trimmed_submaps)
.at(constraint.submap_id.submap_index)
.data(),
C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index -
@ -244,7 +243,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
ceres::Solve(
common::CreateCeresSolverOptions(options_.ceres_solver_options()),
&problem, &summary);
if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport();
}
@ -252,11 +250,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Store the result.
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
for (size_t submap_data_index = 0;
submap_data_index != submap_data_[trajectory_id].size();
++submap_data_index) {
submap_data_[trajectory_id][submap_data_index].pose =
ToPose(C_submaps[trajectory_id][submap_data_index]);
for (auto& index_submap_data : submap_data_[trajectory_id]) {
index_submap_data.second.pose =
ToPose(C_submaps[trajectory_id].at(index_submap_data.first));
}
}
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@ -275,7 +271,7 @@ const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data()
return node_data_;
}
const std::vector<std::deque<SubmapData>>& OptimizationProblem::submap_data()
const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
const {
return submap_data_;
}
@ -284,10 +280,6 @@ int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const {
return trajectory_data_.at(trajectory_id).num_trimmed_nodes;
}
int OptimizationProblem::num_trimmed_submaps(int trajectory_id) const {
return trajectory_data_.at(trajectory_id).num_trimmed_submaps;
}
} // namespace sparse_pose_graph
} // namespace mapping_2d
} // namespace cartographer

7
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -77,22 +77,21 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories);
const std::vector<std::deque<NodeData>>& node_data() const;
const std::vector<std::deque<SubmapData>>& submap_data() const;
const std::vector<std::map<int, SubmapData>>& submap_data() const;
int num_trimmed_nodes(int trajectory_id) const;
int num_trimmed_submaps(int trajectory_id) const;
private:
struct TrajectoryData {
// TODO(hrapp): Remove, once we can relabel constraints.
int next_submap_index = 0;
int num_trimmed_nodes = 0;
int num_trimmed_submaps = 0;
};
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::deque<NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::deque<SubmapData>> submap_data_;
std::vector<std::map<int, SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
};