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

Introduces mapping::MapById for submaps in 3D. (#584)

master
Wolfgang Hess 2017-10-16 10:48:43 +02:00 committed by GitHub
parent 006db45910
commit 7e0cfc3f22
6 changed files with 84 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer/mapping_3d/ceres_pose.cc
View File

@ -24,21 +24,22 @@ CeresPose::CeresPose(
std::unique_ptr<ceres::LocalParameterization> translation_parametrization, std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization, std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
ceres::Problem* problem) ceres::Problem* problem)
: translation_({{rigid.translation().x(), rigid.translation().y(), : data_(std::make_shared<CeresPose::Data>(
rigid.translation().z()}}), CeresPose::Data{{{rigid.translation().x(), rigid.translation().y(),
rotation_({{rigid.rotation().w(), rigid.rotation().x(), rigid.translation().z()}},
rigid.rotation().y(), rigid.rotation().z()}}) { {{rigid.rotation().w(), rigid.rotation().x(),
problem->AddParameterBlock(translation_.data(), 3, rigid.rotation().y(), rigid.rotation().z()}}})) {
problem->AddParameterBlock(data_->translation.data(), 3,
translation_parametrization.release()); translation_parametrization.release());
problem->AddParameterBlock(rotation_.data(), 4, problem->AddParameterBlock(data_->rotation.data(), 4,
rotation_parametrization.release()); rotation_parametrization.release());
} }
const transform::Rigid3d CeresPose::ToRigid() const { const transform::Rigid3d CeresPose::ToRigid() const {
const auto& rotation = data_->rotation;
return transform::Rigid3d( return transform::Rigid3d(
Eigen::Map<const Eigen::Vector3d>(translation_.data()), Eigen::Map<const Eigen::Vector3d>(data_->translation.data()),
Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3]));
rotation_[3]));
} }
} // namespace mapping_3d } // namespace mapping_3d

16
cartographer/mapping_3d/ceres_pose.h
View File

@ -35,18 +35,18 @@ class CeresPose {
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization, std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
ceres::Problem* problem); ceres::Problem* problem);
CeresPose(const CeresPose&) = delete;
CeresPose& operator=(const CeresPose&) = delete;
const transform::Rigid3d ToRigid() const; const transform::Rigid3d ToRigid() const;
double* translation() { return translation_.data(); } double* translation() { return data_->translation.data(); }
double* rotation() { return rotation_.data(); } double* rotation() { return data_->rotation.data(); }
private: private:
std::array<double, 3> translation_; struct Data {
// Rotation quaternion as (w, x, y, z). std::array<double, 3> translation;
std::array<double, 4> rotation_; // Rotation quaternion as (w, x, y, z).
std::array<double, 4> rotation;
};
std::shared_ptr<Data> data_;
}; };
} // namespace mapping_3d } // namespace mapping_3d

65
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -59,25 +59,23 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one. // If we don't already have an entry for the first submap, add one.
if (static_cast<size_t>(trajectory_id) >= submap_data.size() || if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
submap_data[trajectory_id].empty()) {
optimization_problem_.AddSubmap(trajectory_id, optimization_problem_.AddSubmap(trajectory_id,
insertion_submaps[0]->local_pose()); insertion_submaps[0]->local_pose());
} }
CHECK_EQ(submap_data[trajectory_id].size(), 1); CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0}; 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 auto end_it = submap_data.EndOfTrajectory(trajectory_id);
const mapping::SubmapId last_submap_id{ CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
trajectory_id, submap_data.at(trajectory_id).rbegin()->first}; const mapping::SubmapId last_submap_id = std::prev(end_it)->id;
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.
const auto& first_submap_pose = const auto& first_submap_pose = submap_data.at(last_submap_id).pose;
submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose;
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose * trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose().inverse() * insertion_submaps[0]->local_pose().inverse() *
@ -177,10 +175,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const transform::Rigid3d inverse_submap_pose = const transform::Rigid3d inverse_submap_pose =
optimization_problem_.submap_data() optimization_problem_.submap_data().at(submap_id).pose.inverse();
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse();
const transform::Rigid3d initial_relative_pose = const transform::Rigid3d initial_relative_pose =
inverse_submap_pose * optimization_problem_.node_data() inverse_submap_pose * optimization_problem_.node_data()
@ -273,10 +268,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid3d& pose = constant_data->initial_pose; const transform::Rigid3d& pose = constant_data->initial_pose;
const transform::Rigid3d optimized_pose = const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data() optimization_problem_.submap_data().at(matching_id).pose *
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
insertion_submaps.front()->local_pose().inverse() * pose; insertion_submaps.front()->local_pose().inverse() * pose;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose); matching_id.trajectory_id, constant_data->time, pose, optimized_pose);
@ -446,14 +438,9 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
submap_data_.Append(trajectory_id, SubmapData()); submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the optimized pose. // Immediately show the submap at the optimized pose.
CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()), CHECK_EQ(submap_id,
optimized_submap_transforms_.size()); optimized_submap_transforms_.Append(
optimized_submap_transforms_.resize(submap_data_.num_trajectories()); trajectory_id, sparse_pose_graph::SubmapData{initial_pose}));
CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(),
submap_id.submap_index);
optimized_submap_transforms_.at(trajectory_id)
.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;
@ -512,9 +499,7 @@ void SparsePoseGraph::LogResidualHistograms() {
const cartographer::transform::Rigid3d node_to_submap_constraint = const cartographer::transform::Rigid3d node_to_submap_constraint =
constraint.pose.zbar_ij; constraint.pose.zbar_ij;
const cartographer::transform::Rigid3d optimized_submap_to_map = const cartographer::transform::Rigid3d optimized_submap_to_map =
optimized_submap_transforms_.at(constraint.submap_id.trajectory_id) optimized_submap_transforms_.at(constraint.submap_id).pose;
.at(constraint.submap_id.submap_index)
.pose;
const cartographer::transform::Rigid3d optimized_node_to_submap = const cartographer::transform::Rigid3d optimized_node_to_submap =
optimized_submap_to_map.inverse() * optimized_node_to_map; optimized_submap_to_map.inverse() * optimized_node_to_map;
const cartographer::transform::Rigid3d residual = const cartographer::transform::Rigid3d residual =
@ -632,18 +617,17 @@ SparsePoseGraph::GetAllSubmapData() {
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>& const mapping::MapById<mapping::SubmapId, 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()) || auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
submap_transforms.at(trajectory_id).empty()) { auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
return transform::Rigid3d::Identity(); return transform::Rigid3d::Identity();
} }
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
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. // Accessing 'local_pose' in Submap is okay, since the member is const.
return submap_transforms.at(trajectory_id).at(submap_index).pose * return submap_transforms.at(last_optimized_submap_id).pose *
submap_data_.at(last_optimized_submap_id) submap_data_.at(last_optimized_submap_id)
.submap->local_pose() .submap->local_pose()
.inverse(); .inverse();
@ -655,15 +639,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
return {}; return {};
} }
auto submap = submap_data_.at(submap_id).submap; auto submap = submap_data_.at(submap_id).submap;
if (submap_id.trajectory_id < if (optimized_submap_transforms_.Contains(submap_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. // We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id) return {submap, optimized_submap_transforms_.at(submap_id).pose};
.at(submap_id.submap_index)
.pose};
} }
// We have to extrapolate. // We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
@ -676,7 +654,8 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
int SparsePoseGraph::TrimmingHandle::num_submaps( int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const { const int trajectory_id) const {
return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
} }
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -161,7 +161,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::map<int, sparse_pose_graph::SubmapData>>& const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
@ -218,7 +218,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::map<int, sparse_pose_graph::SubmapData>> mapping::MapById<mapping::SubmapId, 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.

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

@ -110,19 +110,13 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid3d& submap_pose) { const transform::Rigid3d& submap_pose) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
submap_data_.resize( trajectory_data_.resize(std::max(trajectory_data_.size(),
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1)); static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize( submap_data_.Append(trajectory_id, SubmapData{submap_pose});
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::TrimSubmap(const mapping::SubmapId& submap_id) { void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
auto& submap_data = submap_data_.at(submap_id.trajectory_id); submap_data_.Trim(submap_id);
CHECK(submap_data.erase(submap_id.submap_index));
} }
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -150,43 +144,37 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Set the starting point. // Set the starting point.
CHECK(!submap_data_.empty()); CHECK(!submap_data_.empty());
CHECK(!submap_data_[0].empty()); CHECK(submap_data_.Contains(mapping::SubmapId{0, 0}));
// TODO(hrapp): Move ceres data into SubmapData. mapping::MapById<mapping::SubmapId, CeresPose> C_submaps;
std::vector<std::map<int, CeresPose>> C_submaps(submap_data_.size());
std::vector<std::map<int, CeresPose>> C_nodes(node_data_.size()); std::vector<std::map<int, CeresPose>> C_nodes(node_data_.size());
bool first_submap = true; bool first_submap = true;
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); for (const auto& submap_id_data : submap_data_) {
++trajectory_id) { const bool frozen =
const bool frozen = frozen_trajectories.count(trajectory_id) != 0; frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
for (const auto& index_submap_data : submap_data_[trajectory_id]) { if (first_submap) {
const int submap_index = index_submap_data.first; first_submap = false;
if (first_submap) { // Fix the first submap of the first trajectory except for allowing
first_submap = false; // gravity alignment.
// Fix the first submap of the first trajectory except for allowing C_submaps.Insert(
// gravity alignment. submap_id_data.id,
C_submaps[trajectory_id].emplace( CeresPose(submap_id_data.data.pose, translation_parameterization(),
std::piecewise_construct, std::forward_as_tuple(submap_index), common::make_unique<ceres::AutoDiffLocalParameterization<
std::forward_as_tuple( ConstantYawQuaternionPlus, 4, 2>>(),
index_submap_data.second.pose, translation_parameterization(), &problem));
common::make_unique<ceres::AutoDiffLocalParameterization< problem.SetParameterBlockConstant(
ConstantYawQuaternionPlus, 4, 2>>(), C_submaps.at(submap_id_data.id).translation());
&problem)); } else {
problem.SetParameterBlockConstant( C_submaps.Insert(
C_submaps[trajectory_id].at(submap_index).translation()); submap_id_data.id,
} else { CeresPose(submap_id_data.data.pose, translation_parameterization(),
C_submaps[trajectory_id].emplace( common::make_unique<ceres::QuaternionParameterization>(),
std::piecewise_construct, std::forward_as_tuple(submap_index), &problem));
std::forward_as_tuple( }
index_submap_data.second.pose, translation_parameterization(), if (frozen) {
common::make_unique<ceres::QuaternionParameterization>(), problem.SetParameterBlockConstant(
&problem)); C_submaps.at(submap_id_data.id).rotation());
} problem.SetParameterBlockConstant(
if (frozen) { C_submaps.at(submap_id_data.id).translation());
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].at(submap_index).rotation());
problem.SetParameterBlockConstant(
C_submaps[trajectory_id].at(submap_index).translation());
}
} }
} }
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@ -217,12 +205,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
constraint.tag == Constraint::INTER_SUBMAP constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale()) ? new ceres::HuberLoss(options_.huber_scale())
: nullptr, : nullptr,
C_submaps.at(constraint.submap_id.trajectory_id) C_submaps.at(constraint.submap_id).rotation(),
.at(constraint.submap_id.submap_index) C_submaps.at(constraint.submap_id).translation(),
.rotation(),
C_submaps.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index)
.translation(),
C_nodes.at(constraint.node_id.trajectory_id) C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index) .at(constraint.node_id.node_index)
.rotation(), .rotation(),
@ -446,12 +430,8 @@ 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 (const auto& C_submap_id_data : C_submaps) {
++trajectory_id) { submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.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(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
@ -467,8 +447,8 @@ const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
return node_data_; return node_data_;
} }
const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data() const mapping::MapById<mapping::SubmapId, SubmapData>&
const { OptimizationProblem::submap_data() const {
return submap_data_; return submap_data_;
} }

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

@ -27,6 +27,7 @@
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
@ -84,13 +85,12 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories); const std::set<int>& frozen_trajectories);
const std::vector<std::map<int, NodeData>>& node_data() const; const std::vector<std::map<int, NodeData>>& node_data() const;
const std::vector<std::map<int, SubmapData>>& submap_data() const; const mapping::MapById<mapping::SubmapId, 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;
int next_node_index = 0; int next_node_index = 0;
}; };
@ -99,7 +99,7 @@ class OptimizationProblem {
std::vector<std::deque<sensor::ImuData>> imu_data_; std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::map<int, NodeData>> node_data_; std::vector<std::map<int, NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_; std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::map<int, SubmapData>> submap_data_; mapping::MapById<mapping::SubmapId, 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_;
}; };