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

31
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()) {
// 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(last_submap_id).pose;
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap(
trajectory_id,
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
// search window.
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;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -229,7 +231,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
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())
.inverse() *
pose;
@ -386,8 +388,9 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) {
void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_2d()) {
return;
}
@ -396,19 +399,20 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
submap.submap_id().submap_index()};
std::shared_ptr<const Submap> submap_ptr =
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_);
AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData());
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(
submap_id, sparse_pose_graph::SubmapData{global_pose_2d});
AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) {
submap_id, sparse_pose_graph::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
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;
// Accessing 'local_pose' in Submap is okay, since the member is const.
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->local_pose()
.inverse();
@ -658,8 +662,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, transform::Embed3D(
optimized_submap_transforms_.at(submap_id).pose)};
return {submap,
transform::Embed3D(
optimized_submap_transforms_.at(submap_id).global_pose)};
}
// We have to extrapolate.
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& operator=(const SparsePoseGraph&) = delete;
// Adds a new node with 'constant_data' and a 'pose' that will later be
// optimized. The 'pose' was determined by scan matching against
// 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// 'true', this submap was inserted into for the last time.
// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the
// scan was inserted into the 'insertion_submaps'. If
// 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time.
mapping::NodeId AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id,
@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void FinishTrajectory(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;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
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);
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid2d& submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{submap_pose});
void OptimizationProblem::AddSubmap(
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
}
void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id,
const transform::Rigid2d& submap_pose) {
submap_data_.Insert(submap_id, SubmapData{submap_pose});
void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id,
const transform::Rigid2d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
}
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_) {
const bool frozen =
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);
if (first_submap || frozen) {
first_submap = false;
@ -221,7 +223,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Store the result.
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) {
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 {
transform::Rigid2d pose;
transform::Rigid2d global_pose;
};
// Implements the SPA loop closure method.
@ -75,14 +75,15 @@ class OptimizationProblem {
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
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,
const transform::Rigid2d& submap_pose);
const transform::Rigid2d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses.
// Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories);

35
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()) {
// 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(last_submap_id).pose;
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose *
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;
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 =
global_submap_pose.inverse();
@ -247,12 +247,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const transform::Rigid3d& pose = constant_data->local_pose;
const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data().at(matching_id).pose *
insertion_submaps.front()->local_pose().inverse() * pose;
const transform::Rigid3d& local_pose = constant_data->local_pose;
const transform::Rigid3d global_pose =
optimization_problem_.submap_data().at(matching_id).global_pose *
insertion_submaps.front()->local_pose().inverse() * local_pose;
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) {
const mapping::SubmapId submap_id = submap_ids[i];
// 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);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform =
insertion_submaps[i]->local_pose().inverse() * pose;
insertion_submaps[i]->local_pose().inverse() * local_pose;
constraints_.push_back(
Constraint{submap_id,
node_id,
@ -402,8 +402,9 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) {
void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_3d()) {
return;
}
@ -417,13 +418,13 @@ void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose,
AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData());
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(
submap_id, sparse_pose_graph::SubmapData{global_pose});
AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) {
submap_id, sparse_pose_graph::SubmapData{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
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 =
constraint.pose.zbar_ij;
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 =
optimized_submap_to_map.inverse() * optimized_node_to_map;
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;
// 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->local_pose()
.inverse();
@ -674,7 +675,7 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
// 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.
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& operator=(const SparsePoseGraph&) = delete;
// Adds a new node with 'constant_data' and a 'pose' that will later be
// optimized. The 'pose' was determined by scan matching against
// 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// 'true', this submap was inserted into for the last time.
// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the
// scan was inserted into the 'insertion_submaps'. If
// 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time.
mapping::NodeId AddScan(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id,
@ -86,7 +86,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void FinishTrajectory(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;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
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);
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid3d& submap_pose) {
void OptimizationProblem::AddSubmap(
const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
CHECK_GE(trajectory_id, 0);
trajectory_data_.resize(std::max(trajectory_data_.size(),
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,
const transform::Rigid3d& submap_pose) {
void OptimizationProblem::InsertSubmap(
const mapping::SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose) {
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_data_.resize(
std::max(trajectory_data_.size(),
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) {
@ -161,7 +162,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// gravity alignment.
C_submaps.Insert(
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<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem));
@ -170,7 +172,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
} else {
C_submaps.Insert(
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>(),
&problem));
}
@ -420,7 +423,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Store the result.
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) {
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 {
transform::Rigid3d pose;
transform::Rigid3d global_pose;
};
// Implements the SPA loop closure method.
@ -79,14 +79,15 @@ class OptimizationProblem {
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
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,
const transform::Rigid3d& submap_pose);
const transform::Rigid3d& global_submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses.
// Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories);