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

Use 'global_pose' terminology for submaps. (#645) 

Use 'global_pose' terminology for submaps.

This follows #624.
master
Wolfgang Hess 2017-11-09 16:40:21 +01:00 committed by Wally B. Feed
parent 6eaf0f344d
commit 418819a9ef
8 changed files with 82 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -83,7 +83,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
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 = submap_data.at(last_submap_id).pose; const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
trajectory_id, trajectory_id,
first_submap_pose * first_submap_pose *
@ -192,7 +192,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// submap's trajectory, it suffices to do a match constrained to a local // submap's trajectory, it suffices to do a match constrained to a local
// search window. // search window.
const transform::Rigid2d initial_relative_pose = const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data().at(submap_id).pose.inverse() * optimization_problem_.submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_.node_data().at(node_id).pose; optimization_problem_.node_data().at(node_id).pose;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -229,7 +231,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constant_data->local_pose * constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_id).pose * optimization_problem_.submap_data().at(matching_id).global_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() * .inverse() *
pose; pose;
@ -386,7 +388,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
}); });
} }
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) { const mapping::proto::Submap& submap) {
if (!submap.has_submap_2d()) { if (!submap.has_submap_2d()) {
return; return;
@ -396,19 +399,20 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
submap.submap_id().submap_index()}; submap.submap_id().submap_index()};
std::shared_ptr<const Submap> submap_ptr = std::shared_ptr<const Submap> submap_ptr =
std::make_shared<const Submap>(submap.submap_2d()); std::make_shared<const Submap>(submap.submap_2d());
const transform::Rigid2d global_pose_2d = transform::Project2D(global_pose); const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData()); submap_data_.Insert(submap_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 'global_submap_pose'.
optimized_submap_transforms_.Insert( optimized_submap_transforms_.Insert(
submap_id, sparse_pose_graph::SubmapData{global_pose_2d}); submap_id, sparse_pose_graph::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() 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;
optimization_problem_.InsertSubmap(submap_id, global_pose_2d); optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
}); });
} }
@ -643,7 +647,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// 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 transform::Embed3D( return transform::Embed3D(
submap_transforms.at(last_optimized_submap_id).pose) * submap_transforms.at(last_optimized_submap_id).global_pose) *
submap_data_.at(last_optimized_submap_id) submap_data_.at(last_optimized_submap_id)
.submap->local_pose() .submap->local_pose()
.inverse(); .inverse();
@ -658,8 +662,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
auto submap = it->data.submap; auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) { if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose. // We already have an optimized pose.
return {submap, transform::Embed3D( return {submap,
optimized_submap_transforms_.at(submap_id).pose)}; transform::Embed3D(
optimized_submap_transforms_.at(submap_id).global_pose)};
} }
// We have to extrapolate. // We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,

12
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -65,11 +65,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new node with 'constant_data' and a 'pose' that will later be // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// optimized. The 'pose' was determined by scan matching against // determined by scan matching against 'insertion_submaps.front()' and the
// 'insertion_submaps.front()' and the scan was inserted into the // scan was inserted into the 'insertion_submaps'. If
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// 'true', this submap was inserted into for the last time. // this submap for the last time.
mapping::NodeId AddScan( mapping::NodeId AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) override; const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override; const mapping::proto::Node& node) override;

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

@ -96,14 +96,15 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
node_data_.Trim(node_id); node_data_.Trim(node_id);
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(
const transform::Rigid2d& submap_pose) { const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{submap_pose}); submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
} }
void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, void OptimizationProblem::InsertSubmap(
const transform::Rigid2d& submap_pose) { const mapping::SubmapId& submap_id,
submap_data_.Insert(submap_id, SubmapData{submap_pose}); const transform::Rigid2d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
} }
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
@ -133,7 +134,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
const bool frozen = const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.pose)); C_submaps.Insert(submap_id_data.id,
FromPose(submap_id_data.data.global_pose));
problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3); problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
if (first_submap || frozen) { if (first_submap || frozen) {
first_submap = false; first_submap = false;
@ -221,7 +223,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Store the result. // Store the result.
for (const auto& C_submap_id_data : C_submaps) { for (const auto& C_submap_id_data : C_submaps) {
submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data); submap_data_.at(C_submap_id_data.id).global_pose =
ToPose(C_submap_id_data.data);
} }
for (const auto& C_node_id_data : C_nodes) { for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data); node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);

9
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -47,7 +47,7 @@ struct NodeData {
}; };
struct SubmapData { struct SubmapData {
transform::Rigid2d pose; transform::Rigid2d global_pose;
}; };
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
@ -75,14 +75,15 @@ class OptimizationProblem {
const transform::Rigid2d& pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment); const Eigen::Quaterniond& gravity_alignment);
void TrimTrajectoryNode(const mapping::NodeId& node_id); void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id, void InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid2d& submap_pose); const transform::Rigid2d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id); void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations); void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. // Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints, void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories); const std::set<int>& frozen_trajectories);

33
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -82,7 +82,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
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 = submap_data.at(last_submap_id).pose; const auto& first_submap_pose = submap_data.at(last_submap_id).global_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() *
@ -183,7 +183,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
optimization_problem_.node_data().at(node_id).global_pose; optimization_problem_.node_data().at(node_id).global_pose;
const transform::Rigid3d global_submap_pose = const transform::Rigid3d global_submap_pose =
optimization_problem_.submap_data().at(submap_id).pose; optimization_problem_.submap_data().at(submap_id).global_pose;
const transform::Rigid3d global_submap_pose_inverse = const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse(); global_submap_pose.inverse();
@ -247,12 +247,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
node_id.trajectory_id, constant_data->time, insertion_submaps); node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size()); CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front(); const mapping::SubmapId matching_id = submap_ids.front();
const transform::Rigid3d& pose = constant_data->local_pose; const transform::Rigid3d& local_pose = constant_data->local_pose;
const transform::Rigid3d optimized_pose = const transform::Rigid3d global_pose =
optimization_problem_.submap_data().at(matching_id).pose * optimization_problem_.submap_data().at(matching_id).global_pose *
insertion_submaps.front()->local_pose().inverse() * pose; insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose); matching_id.trajectory_id, constant_data->time, local_pose, global_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i]; const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only // Even if this was the last scan added to 'submap_id', the submap will only
@ -260,7 +260,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
insertion_submaps[i]->local_pose().inverse() * pose; insertion_submaps[i]->local_pose().inverse() * local_pose;
constraints_.push_back( constraints_.push_back(
Constraint{submap_id, Constraint{submap_id,
node_id, node_id,
@ -402,7 +402,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
}); });
} }
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) { const mapping::proto::Submap& submap) {
if (!submap.has_submap_3d()) { if (!submap.has_submap_3d()) {
return; return;
@ -417,13 +418,13 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData()); submap_data_.Insert(submap_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 'global_submap_pose'.
optimized_submap_transforms_.Insert( optimized_submap_transforms_.Insert(
submap_id, sparse_pose_graph::SubmapData{global_pose}); submap_id, sparse_pose_graph::SubmapData{global_submap_pose});
AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_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;
optimization_problem_.InsertSubmap(submap_id, global_pose); optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
}); });
} }
@ -506,7 +507,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).pose; optimized_submap_transforms_.at(constraint.submap_id).global_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 =
@ -659,7 +660,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
} }
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// 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(last_optimized_submap_id).pose * return submap_transforms.at(last_optimized_submap_id).global_pose *
submap_data_.at(last_optimized_submap_id) submap_data_.at(last_optimized_submap_id)
.submap->local_pose() .submap->local_pose()
.inverse(); .inverse();
@ -674,7 +675,7 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
auto submap = it->data.submap; auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) { if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose. // We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id).pose}; return {submap, optimized_submap_transforms_.at(submap_id).global_pose};
} }
// We have to extrapolate. // We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,

12
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -65,11 +65,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new node with 'constant_data' and a 'pose' that will later be // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// optimized. The 'pose' was determined by scan matching against // determined by scan matching against 'insertion_submaps.front()' and the
// 'insertion_submaps.front()' and the scan was inserted into the // scan was inserted into the 'insertion_submaps'. If
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// 'true', this submap was inserted into for the last time. // this submap for the last time.
mapping::NodeId AddScan( mapping::NodeId AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) override; const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override; const mapping::proto::Node& node) override;

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

@ -102,21 +102,22 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
node_data_.Trim(node_id); node_data_.Trim(node_id);
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(
const transform::Rigid3d& submap_pose) { const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
trajectory_data_.resize(std::max(trajectory_data_.size(), trajectory_data_.resize(std::max(trajectory_data_.size(),
static_cast<size_t>(trajectory_id) + 1)); static_cast<size_t>(trajectory_id) + 1));
submap_data_.Append(trajectory_id, SubmapData{submap_pose}); submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
} }
void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, void OptimizationProblem::InsertSubmap(
const transform::Rigid3d& submap_pose) { const mapping::SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose) {
CHECK_GE(submap_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0);
trajectory_data_.resize( trajectory_data_.resize(
std::max(trajectory_data_.size(), std::max(trajectory_data_.size(),
static_cast<size_t>(submap_id.trajectory_id) + 1)); static_cast<size_t>(submap_id.trajectory_id) + 1));
submap_data_.Insert(submap_id, SubmapData{submap_pose}); submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
} }
void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) {
@ -161,7 +162,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// gravity alignment. // gravity alignment.
C_submaps.Insert( C_submaps.Insert(
submap_id_data.id, submap_id_data.id,
CeresPose(submap_id_data.data.pose, translation_parameterization(), CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization< common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(), ConstantYawQuaternionPlus, 4, 2>>(),
&problem)); &problem));
@ -170,7 +172,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
} else { } else {
C_submaps.Insert( C_submaps.Insert(
submap_id_data.id, submap_id_data.id,
CeresPose(submap_id_data.data.pose, translation_parameterization(), CeresPose(submap_id_data.data.global_pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), common::make_unique<ceres::QuaternionParameterization>(),
&problem)); &problem));
} }
@ -420,7 +423,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Store the result. // Store the result.
for (const auto& C_submap_id_data : C_submaps) { for (const auto& C_submap_id_data : C_submaps) {
submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); submap_data_.at(C_submap_id_data.id).global_pose =
C_submap_id_data.data.ToRigid();
} }
for (const auto& C_node_id_data : C_nodes) { for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).global_pose = node_data_.at(C_node_id_data.id).global_pose =

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

@ -47,7 +47,7 @@ struct NodeData {
}; };
struct SubmapData { struct SubmapData {
transform::Rigid3d pose; transform::Rigid3d global_pose;
}; };
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
@ -79,14 +79,15 @@ class OptimizationProblem {
const transform::Rigid3d& local_pose, const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose); const transform::Rigid3d& global_pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id); void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id, void InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid3d& submap_pose); const transform::Rigid3d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id); void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations); void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. // Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints, void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories); const std::set<int>& frozen_trajectories);