Introduces mapping::MapById for submaps in 3D. (#584)
Wolfgang Hess
GitHub
parent
006db45910
commit
7e0cfc3f22
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
// Rotation quaternion as (w, x, y, z).
|
||||
std::array<double, 4> rotation_;
|
||||
struct Data {
|
||||
std::array<double, 3> translation;
|
||||
// Rotation quaternion as (w, x, y, z).
|
||||
std::array<double, 4> rotation;
|
||||
};
|
||||
std::shared_ptr<Data> data_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
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].at(submap_index).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(),
|
||||
common::make_unique<ceres::QuaternionParameterization>(),
|
||||
&problem));
|
||||
}
|
||||
if (frozen) {
|
||||
problem.SetParameterBlockConstant(
|
||||
C_submaps[trajectory_id].at(submap_index).rotation());
|
||||
problem.SetParameterBlockConstant(
|
||||
C_submaps[trajectory_id].at(submap_index).translation());
|
||||
}
|
||||
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.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.at(submap_id_data.id).translation());
|
||||
} else {
|
||||
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.at(submap_id_data.id).rotation());
|
||||
problem.SetParameterBlockConstant(
|
||||
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_;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue