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

15
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -565,17 +565,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
} }
auto submap = submap_data_.at(submap_id).submap; auto submap = submap_data_.at(submap_id).submap;
if (submap_id.trajectory_id < if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size())) { static_cast<int>(optimized_submap_transforms_.size()) &&
const size_t submap_data_index = submap_id.submap_index; submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
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_id.trajectory_id)
.at(submap_data_index) .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)}; .pose)};
} }
}
// We have to extrapolate. // We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
submap_id.trajectory_id) * submap_id.trajectory_id) *

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

@ -85,8 +85,7 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.resize( node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
auto& trajectory_data = trajectory_data_[trajectory_id];
auto& trajectory_data = trajectory_data_.at(trajectory_id);
node_data_[trajectory_id].emplace(trajectory_data.next_node_index, node_data_[trajectory_id].emplace(trajectory_data.next_node_index,
NodeData{time, initial_pose, pose}); NodeData{time, initial_pose, pose});
++trajectory_data.next_node_index; ++trajectory_data.next_node_index;
@ -98,9 +97,7 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
if (!node_data.empty() && if (!node_data.empty() &&
node_id.trajectory_id < static_cast<int>(imu_data_.size())) { node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
auto node_it = node_data.begin(); const common::Time node_time = node_data.begin()->second.time;
const common::Time node_time = node_it->second.time;
auto& imu_data = imu_data_.at(node_id.trajectory_id); auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) { while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front(); 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)); std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize( trajectory_data_.resize(
std::max(trajectory_data_.size(), submap_data_.size())); std::max(trajectory_data_.size(), submap_data_.size()));
auto& trajectory_data = trajectory_data_[trajectory_id];
auto& trajectory_data = trajectory_data_.at(trajectory_id);
submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index, submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index,
SubmapData{submap_pose}); SubmapData{submap_pose});
++trajectory_data.next_submap_index; ++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, optimization_problem_.AddSubmap(trajectory_id,
insertion_submaps[0]->local_pose()); insertion_submaps[0]->local_pose());
} }
const mapping::SubmapId submap_id{ CHECK_EQ(submap_data[trajectory_id].size(), 1);
trajectory_id, static_cast<int>(submap_data[trajectory_id].size()) - 1}; const mapping::SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
return {submap_id}; return {submap_id};
} }
CHECK_EQ(2, insertion_submaps.size()); CHECK_EQ(2, insertion_submaps.size());
CHECK(!submap_data.at(trajectory_id).empty());
const mapping::SubmapId last_submap_id{ const mapping::SubmapId last_submap_id{
trajectory_id, trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
static_cast<int>(submap_data.at(trajectory_id).size() - 1)};
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new. // and 'insertions_submaps.back()' is new.
@ -98,6 +98,7 @@ void SparsePoseGraph::AddScan(
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose); GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append( trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); 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(), CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
submap_id.submap_index); submap_id.submap_index);
optimized_submap_transforms_.at(trajectory_id) 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_) { AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -478,6 +480,7 @@ void SparsePoseGraph::RunOptimization() {
optimization_problem_.Solve(constraints_, frozen_trajectories_); optimization_problem_.Solve(constraints_, frozen_trajectories_);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0; for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) { trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
@ -490,8 +493,8 @@ void SparsePoseGraph::RunOptimization() {
node_data[trajectory_id][node_index].pose; node_data[trajectory_id][node_index].pose;
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform( const auto local_to_new_global =
optimization_problem_.submap_data(), trajectory_id); ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform( const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id); optimized_submap_transforms_, trajectory_id);
const transform::Rigid3d old_global_to_new_global = 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; 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); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
@ -571,18 +574,18 @@ SparsePoseGraph::GetAllSubmapData() {
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( 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, submap_transforms,
const int trajectory_id) const { const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) || if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) { submap_transforms.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity(); return transform::Rigid3d::Identity();
} }
const mapping::SubmapId last_optimized_submap_id{
trajectory_id, const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)}; const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
// Accessing 'local_pose' in Submap is okay, since the member is const. // 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_data_.at(last_optimized_submap_id)
.submap->local_pose() .submap->local_pose()
.inverse(); .inverse();
@ -591,12 +594,12 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
auto submap = submap_data_.at(submap_id).submap; auto submap = submap_data_.at(submap_id).submap;
// We already have an optimized pose.
if (submap_id.trajectory_id < if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) && static_cast<int>(optimized_submap_transforms_.size()) &&
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_ submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
.at(submap_id.trajectory_id) .at(submap_id.trajectory_id)
.size())) { .size())) {
// We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id) return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.pose}; .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 // Computes the local to global frame transform based on the given optimized
// 'submap_transforms'. // 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::vector<sparse_pose_graph::SubmapData>>& const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
@ -205,7 +205,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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::map<int, sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_); optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.

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

@ -19,6 +19,7 @@
#include <algorithm> #include <algorithm>
#include <array> #include <array>
#include <cmath> #include <cmath>
#include <iterator>
#include <map> #include <map>
#include <memory> #include <memory>
#include <string> #include <string>
@ -93,7 +94,12 @@ void OptimizationProblem::AddSubmap(const int trajectory_id,
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
submap_data_.resize( submap_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1)); 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) { 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_.empty());
CHECK(!submap_data_[0].empty()); CHECK(!submap_data_[0].empty());
// TODO(hrapp): Move ceres data into SubmapData. // 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()); 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(); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) { ++trajectory_id) {
const bool frozen = frozen_trajectories.count(trajectory_id); const bool frozen = frozen_trajectories.count(trajectory_id);
for (size_t submap_index = 0; for (const auto& index_submap_data : submap_data_[trajectory_id]) {
submap_index != submap_data_[trajectory_id].size(); ++submap_index) { const int submap_index = index_submap_data.first;
if (trajectory_id == 0 && submap_index == 0) { if (first_submap) {
// Tie the first submap of the first trajectory to the origin. first_submap = false;
C_submaps[trajectory_id].emplace_back( // Fix the first submap of the first trajectory except for allowing
transform::Rigid3d::Identity(), translation_parameterization(), // 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< common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(), ConstantYawQuaternionPlus, 4, 2>>(),
&problem); &problem));
problem.SetParameterBlockConstant( problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().translation()); C_submaps[trajectory_id].at(submap_index).translation());
} else { } else {
C_submaps[trajectory_id].emplace_back( C_submaps[trajectory_id].emplace(
submap_data_[trajectory_id][submap_index].pose, std::piecewise_construct, std::forward_as_tuple(submap_index),
translation_parameterization(), std::forward_as_tuple(
common::make_unique<ceres::QuaternionParameterization>(), &problem); index_submap_data.second.pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(),
&problem));
} }
if (frozen) { if (frozen) {
problem.SetParameterBlockConstant( problem.SetParameterBlockConstant(
C_submaps[trajectory_id].back().rotation()); C_submaps[trajectory_id].at(submap_index).rotation());
problem.SetParameterBlockConstant( 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. // Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) { for (const Constraint& constraint : constraints) {
problem.AddResidualBlock( problem.AddResidualBlock(
@ -321,7 +333,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
ceres::Solve( ceres::Solve(
common::CreateCeresSolverOptions(options_.ceres_solver_options()), common::CreateCeresSolverOptions(options_.ceres_solver_options()),
&problem, &summary); &problem, &summary);
if (options_.log_solver_summary()) { if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport(); LOG(INFO) << summary.FullReport();
for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size(); 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. // Store the result.
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t submap_index = 0; for (auto& index_submap_data : submap_data_[trajectory_id]) {
submap_index != submap_data_[trajectory_id].size(); ++submap_index) { index_submap_data.second.pose =
submap_data_[trajectory_id][submap_index].pose = C_submaps[trajectory_id].at(index_submap_data.first).ToRigid();
C_submaps[trajectory_id][submap_index].ToRigid();
} }
} }
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); 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_; return node_data_;
} }
const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data() const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
const { const {
return submap_data_; 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::set<int>& frozen_trajectories);
const std::vector<std::vector<NodeData>>& node_data() const; 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: private:
struct TrajectoryData { struct TrajectoryData {
double gravity_constant = 9.8; double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}}; std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
int next_submap_index = 0;
}; };
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
@ -95,7 +96,7 @@ class OptimizationProblem {
std::vector<std::deque<sensor::ImuData>> imu_data_; std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::vector<NodeData>> node_data_; std::vector<std::vector<NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_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<TrajectoryData> trajectory_data_;
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_; std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
}; };