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

Delete SubmapData from trimmed submaps in the OptimizationProblem. (#366) 

Related to #283.
master
Holger Rapp 2017-06-28 12:51:09 +02:00 committed by Wolfgang Hess
parent f88fcec851
commit 471bcb6207
4 changed files with 97 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -65,20 +65,25 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
trajectory_id,
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
}
const mapping::SubmapId submap_id{
trajectory_id, static_cast<int>(submap_data[trajectory_id].size()) - 1};
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() - 1)};
trajectory_id, static_cast<int>(submap_data.at(trajectory_id).size() +
num_trimmed_submaps - 1)};
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).pose;
submap_data.at(trajectory_id)
.at(last_submap_id.submap_index - num_trimmed_submaps)
.pose;
optimization_problem_.AddSubmap(
trajectory_id,
first_submap_pose *
@ -186,7 +191,9 @@ 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)
.at(submap_id.submap_index -
optimization_problem_.num_trimmed_submaps(
submap_id.trajectory_id))
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
@ -231,10 +238,11 @@ 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)
.at(matching_id.submap_index - num_trimmed_submaps)
.pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
@ -389,6 +397,14 @@ void SparsePoseGraph::RunOptimization() {
optimization_problem_.Solve(constraints_);
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) {
@ -403,9 +419,10 @@ void SparsePoseGraph::RunOptimization() {
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform(
optimization_problem_.submap_data(), trajectory_id);
submap_data, num_trimmed_submaps, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id);
optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
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) {
@ -414,7 +431,8 @@ void SparsePoseGraph::RunOptimization() {
old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
}
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
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) {
@ -444,6 +462,7 @@ 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);
}
@ -486,8 +505,9 @@ SparsePoseGraph::GetAllSubmapData() {
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
const std::vector<std::deque<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()) {
@ -495,7 +515,8 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
}
const mapping::SubmapId last_optimized_submap_id{
trajectory_id,
static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)};
static_cast<int>(submap_transforms.at(trajectory_id).size() +
num_trimmed_submaps.at(trajectory_id) - 1)};
// Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) *
submap_data_.at(last_optimized_submap_id)
@ -509,20 +530,25 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
return {};
}
auto submap = submap_data_.at(submap_id).submap;
// We already have an optimized pose.
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
.at(submap_id.trajectory_id)
.size())) {
return {submap, transform::Embed3D(
optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose)};
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);
if (submap_data_index <
optimized_submap_transforms_.at(submap_id.trajectory_id).size()) {
// We already have an optimized pose.
return {submap, transform::Embed3D(optimized_submap_transforms_
.at(submap_id.trajectory_id)
.at(submap_data_index)
.pose)};
}
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_id.trajectory_id) *
return {submap, ComputeLocalToGlobalTransform(
optimized_submap_transforms_,
num_trimmed_submaps_at_last_optimization_,
submap_id.trajectory_id) *
submap->local_pose()};
}
@ -531,7 +557,8 @@ 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();
return parent_->optimization_problem_.submap_data().at(trajectory_id).size() +
parent_->optimization_problem_.num_trimmed_submaps(trajectory_id);
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
@ -585,6 +612,7 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
CHECK(submap_data.submap != nullptr);
submap_data.submap.reset();
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id);
// Mark the 'nodes_to_remove' as trimmed and remove their data.
for (const mapping::NodeId& node_id : nodes_to_remove) {
@ -592,11 +620,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
parent_->trajectory_nodes_.at(node_id).constant_data.reset();
parent_->optimization_problem_.TrimTrajectoryNode(node_id);
}
// TODO(whess): The optimization problem should no longer include the submap.
// TODO(whess): If the first submap is gone, we want to tie the first not
// yet trimmed submap to be set fixed to its current pose.
}
} // namespace mapping_2d

8
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -150,9 +150,10 @@ 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::vector<sparse_pose_graph::SubmapData>>&
const std::vector<std::deque<sparse_pose_graph::SubmapData>>&
submap_transforms,
int trajectory_id) const REQUIRES(mutex_);
const std::vector<int>& num_trimmed_submaps, int trajectory_id) const
REQUIRES(mutex_);
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
@ -199,7 +200,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>
std::vector<int> num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_);
std::vector<std::deque<sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.

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

@ -104,6 +104,18 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
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()));
}
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;
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -122,23 +134,22 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
std::vector<std::vector<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) {
for (size_t submap_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
if (trajectory_id == 0 && submap_index == 0) {
// Fix the pose of the first submap of the first trajectory.
C_submaps[trajectory_id].push_back(
FromPose(transform::Rigid2d::Identity()));
problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
// 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);
if (first_submap) {
first_submap = false;
// Fix the pose of the first submap.
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().data());
} else {
C_submaps[trajectory_id].push_back(
FromPose(submap_data_[trajectory_id][submap_index].pose));
problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3);
}
}
}
@ -163,7 +174,9 @@ 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)
.at(constraint.submap_id.submap_index -
trajectory_data_.at(constraint.submap_id.trajectory_id)
.num_trimmed_submaps)
.data(),
C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index -
@ -207,10 +220,10 @@ 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_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
submap_data_[trajectory_id][submap_index].pose =
ToPose(C_submaps[trajectory_id][submap_index]);
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 (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@ -229,7 +242,7 @@ const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data()
return node_data_;
}
const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
const std::vector<std::deque<SubmapData>>& OptimizationProblem::submap_data()
const {
return submap_data_;
}
@ -238,6 +251,10 @@ 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

@ -65,6 +65,7 @@ class OptimizationProblem {
const transform::Rigid2d& point_cloud_pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
@ -72,19 +73,21 @@ class OptimizationProblem {
void Solve(const std::vector<Constraint>& constraints);
const std::vector<std::deque<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const;
const std::vector<std::deque<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 num_trimmed_nodes = 0;
int num_trimmed_submaps = 0;
};
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<std::deque<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
std::vector<std::deque<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
};