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

Also use vector<map<>> submaps in 3D. (#512) 

This reduces the difference between 2D and 3D and moves
3D towards localization and trimming.
master
Wolfgang Hess 2017-09-08 15:54:28 +02:00 committed by GitHub
parent 1a367f0549
commit 35aa38f73f
6 changed files with 69 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -565,16 +565,15 @@ 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;
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)};
}
static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(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_id.submap_index)
.pose)};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,

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

@ -85,8 +85,7 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
auto& trajectory_data = trajectory_data_.at(trajectory_id);
auto& trajectory_data = trajectory_data_[trajectory_id];
node_data_[trajectory_id].emplace(trajectory_data.next_node_index,
NodeData{time, initial_pose, pose});
++trajectory_data.next_node_index;
@ -98,9 +97,7 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
if (!node_data.empty() &&
node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
auto node_it = node_data.begin();
const common::Time node_time = node_it->second.time;
const common::Time node_time = node_data.begin()->second.time;
auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front();
@ -115,8 +112,7 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize(
std::max(trajectory_data_.size(), submap_data_.size()));
auto& trajectory_data = trajectory_data_.at(trajectory_id);
auto& trajectory_data = trajectory_data_[trajectory_id];
submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
SubmapData{submap_pose});
++trajectory_data.next_submap_index;

31
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -64,15 +64,15 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
optimization_problem_.AddSubmap(trajectory_id,
insertion_submaps[0]->local_pose());
}
const mapping::SubmapId submap_id{
trajectory_id, static_cast<int>(submap_data[trajectory_id].size()) - 1};
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());
CHECK(!submap_data.at(trajectory_id).empty());
const mapping::SubmapId last_submap_id{
trajectory_id,
static_cast<int>(submap_data.at(trajectory_id).size() - 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.
@ -98,6 +98,7 @@ void SparsePoseGraph::AddScan(
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
@ -411,7 +412,8 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
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});
.emplace(submap_id.submap_index,
sparse_pose_graph::SubmapData{initial_pose});
AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -478,6 +480,7 @@ void SparsePoseGraph::RunOptimization() {
optimization_problem_.Solve(constraints_, frozen_trajectories_);
common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@ -490,8 +493,8 @@ void SparsePoseGraph::RunOptimization() {
node_data[trajectory_id][node_index].pose;
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform(
optimization_problem_.submap_data(), trajectory_id);
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
@ -502,7 +505,7 @@ 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;
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
@ -571,18 +574,18 @@ SparsePoseGraph::GetAllSubmapData() {
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
submap_transforms,
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() - 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 submap_transforms.at(trajectory_id).back().pose *
return submap_transforms.at(trajectory_id).at(submap_index).pose *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
@ -591,12 +594,12 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
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())) {
// We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose};

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -157,7 +157,7 @@ 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::map<int, sparse_pose_graph::SubmapData>>&
submap_transforms,
int trajectory_id) const REQUIRES(mutex_);
@ -205,7 +205,7 @@ 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<std::map<int, sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.

60
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -19,6 +19,7 @@
#include <algorithm>
#include <array>
#include <cmath>
#include <iterator>
#include <map>
#include <memory>
#include <string>
@ -93,7 +94,12 @@ 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_[trajectory_id];
submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
SubmapData{submap_pose});
++trajectory_data.next_submap_index;
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -123,33 +129,40 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
CHECK(!submap_data_.empty());
CHECK(!submap_data_[0].empty());
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
std::vector<std::map<int, CeresPose>> C_submaps(submap_data_.size());
std::vector<std::deque<CeresPose>> 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);
for (size_t submap_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
if (trajectory_id == 0 && submap_index == 0) {
// Tie the first submap of the first trajectory to the origin.
C_submaps[trajectory_id].emplace_back(
transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem);
for (const auto& index_submap_data : submap_data_[trajectory_id]) {
const int submap_index = index_submap_data.first;
if (first_submap) {
first_submap = false;
// Fix the first submap of the first trajectory except for allowing
// gravity alignment.
C_submaps[trajectory_id].emplace(
std::piecewise_construct, std::forward_as_tuple(submap_index),
std::forward_as_tuple(
index_submap_data.second.pose, translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem));
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().translation());
C_submaps[trajectory_id].at(submap_index).translation());
} else {
C_submaps[trajectory_id].emplace_back(
submap_data_[trajectory_id][submap_index].pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
C_submaps[trajectory_id].emplace(
std::piecewise_construct, std::forward_as_tuple(submap_index),
std::forward_as_tuple(
index_submap_data.second.pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
}
if (frozen) {
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().rotation());
C_submaps[trajectory_id].at(submap_index).rotation());
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().translation());
C_submaps[trajectory_id].at(submap_index).translation());
}
}
}
@ -170,7 +183,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
}
}
}
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
@ -321,7 +333,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();
for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
@ -344,10 +355,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_index = 0;
submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
submap_data_[trajectory_id][submap_index].pose =
C_submaps[trajectory_id][submap_index].ToRigid();
for (auto& index_submap_data : submap_data_[trajectory_id]) {
index_submap_data.second.pose =
C_submaps[trajectory_id].at(index_submap_data.first).ToRigid();
}
}
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@ -365,7 +375,7 @@ const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
return node_data_;
}
const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
const {
return submap_data_;
}

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -82,12 +82,13 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories);
const std::vector<std::vector<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const;
const std::vector<std::map<int, SubmapData>>& submap_data() const;
private:
struct TrajectoryData {
double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
int next_submap_index = 0;
};
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
@ -95,7 +96,7 @@ class OptimizationProblem {
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::vector<NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::vector<SubmapData>> submap_data_;
std::vector<std::map<int, SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
};