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

14
cartographer/mapping_3d/ceres_pose.h
View File

@ -35,18 +35,18 @@ class CeresPose {
std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
ceres::Problem* problem);
CeresPose(const CeresPose&) = delete;
CeresPose& operator=(const CeresPose&) = delete;
const transform::Rigid3d ToRigid() const;
double* translation() { return translation_.data(); }
double* rotation() { return rotation_.data(); }
double* translation() { return data_->translation.data(); }
double* rotation() { return data_->rotation.data(); }
private:
std::array<double, 3> translation_;
struct Data {
std::array<double, 3> translation;
// Rotation quaternion as (w, x, y, z).
std::array<double, 4> rotation_;
std::array<double, 4> rotation;
};
std::shared_ptr<Data> data_;
};
} // 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();
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
submap_data[trajectory_id].empty()) {
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
optimization_problem_.AddSubmap(trajectory_id,
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};
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, submap_data.at(trajectory_id).rbegin()->first};
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const mapping::SubmapId last_submap_id = std::prev(end_it)->id;
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;
const auto& first_submap_pose = submap_data.at(last_submap_id).pose;
optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose *
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);
const transform::Rigid3d inverse_submap_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse();
optimization_problem_.submap_data().at(submap_id).pose.inverse();
const transform::Rigid3d initial_relative_pose =
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 transform::Rigid3d& pose = constant_data->initial_pose;
const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
optimization_problem_.submap_data().at(matching_id).pose *
insertion_submaps.front()->local_pose().inverse() * pose;
optimization_problem_.AddTrajectoryNode(
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_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the optimized pose.
CHECK_GE(static_cast<size_t>(submap_data_.num_trajectories()),
optimized_submap_transforms_.size());
optimized_submap_transforms_.resize(submap_data_.num_trajectories());
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});
CHECK_EQ(submap_id,
optimized_submap_transforms_.Append(
trajectory_id, 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;
@ -512,9 +499,7 @@ void SparsePoseGraph::LogResidualHistograms() {
const cartographer::transform::Rigid3d node_to_submap_constraint =
constraint.pose.zbar_ij;
const cartographer::transform::Rigid3d optimized_submap_to_map =
optimized_submap_transforms_.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index)
.pose;
optimized_submap_transforms_.at(constraint.submap_id).pose;
const cartographer::transform::Rigid3d optimized_node_to_submap =
optimized_submap_to_map.inverse() * optimized_node_to_map;
const cartographer::transform::Rigid3d residual =
@ -632,18 +617,17 @@ SparsePoseGraph::GetAllSubmapData() {
}
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,
const int trajectory_id) const {
if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
submap_transforms.at(trajectory_id).empty()) {
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
return transform::Rigid3d::Identity();
}
const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first;
const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index};
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// 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->local_pose()
.inverse();
@ -655,15 +639,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
return {};
}
auto submap = submap_data_.at(submap_id).submap;
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())) {
if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose};
return {submap, optimized_submap_transforms_.at(submap_id).pose};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
@ -676,7 +654,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();
const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
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
// 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const std::vector<std::map<int, sparse_pose_graph::SubmapData>>&
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
submap_transforms,
int trajectory_id) const REQUIRES(mutex_);
@ -218,7 +218,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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_);
// List of all trimmers to consult when optimizations finish.

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

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

@ -27,6 +27,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
@ -84,13 +85,12 @@ class OptimizationProblem {
const std::set<int>& frozen_trajectories);
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:
struct TrajectoryData {
double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
int next_submap_index = 0;
int next_node_index = 0;
};
@ -99,7 +99,7 @@ class OptimizationProblem {
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::map<int, NodeData>> node_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<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
};