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

Rename 'submap_transforms' to follow terminology. (#688) 

https://google-cartographer.readthedocs.io/en/latest/terminology.html

Related to #602.
master
Wolfgang Hess 2017-11-17 13:13:45 +01:00 committed by Wally B. Feed
parent b91ff8fe44
commit aba4575d93
4 changed files with 42 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cartographer/mapping_2d/pose_graph.cc
View File

@ -65,10 +65,10 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
insertion_submaps[0]->local_pose()));
trajectory_id,
transform::Project2D(ComputeLocalToGlobalTransform(
global_submap_poses_, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0};
@ -400,8 +400,8 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
optimized_submap_transforms_.Insert(
submap_id, pose_graph::SubmapData{global_submap_pose_2d});
global_submap_poses_.Insert(submap_id,
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;
@ -521,8 +521,8 @@ void PoseGraph::RunOptimization() {
// 'optimization_problem_' yet.
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id);
const auto local_to_old_global =
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
@ -536,7 +536,7 @@ void PoseGraph::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
optimized_submap_transforms_ = submap_data;
global_submap_poses_ = submap_data;
}
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
@ -604,8 +604,7 @@ transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
}
std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
@ -631,10 +630,10 @@ PoseGraph::GetAllSubmapData() {
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
global_submap_poses,
const int trajectory_id) const {
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
const auto it = initial_trajectory_poses_.find(trajectory_id);
if (it != initial_trajectory_poses_.end()) {
@ -648,7 +647,7 @@ transform::Rigid3d PoseGraph::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).global_pose) *
global_submap_poses.at(last_optimized_submap_id).global_pose) *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
@ -661,14 +660,13 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
return {};
}
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
if (global_submap_poses_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap,
transform::Embed3D(
optimized_submap_transforms_.at(submap_id).global_pose)};
transform::Embed3D(global_submap_poses_.at(submap_id).global_pose)};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
return {submap, ComputeLocalToGlobalTransform(global_submap_poses_,
submap_id.trajectory_id) *
submap->local_pose()};
}

8
cartographer/mapping_2d/pose_graph.h
View File

@ -173,10 +173,10 @@ class PoseGraph : public mapping::PoseGraph {
void RunOptimization() EXCLUDES(mutex_);
// Computes the local to global map frame transform based on the given
// optimized 'submap_transforms'.
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
mapping::PoseGraph::SubmapData GetSubmapDataUnderLock(
@ -226,9 +226,9 @@ class PoseGraph : public mapping::PoseGraph {
GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
// Global submap poses currently used for displaying data.
mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
optimized_submap_transforms_ GUARDED_BY(mutex_);
global_submap_poses_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_

35
cartographer/mapping_3d/pose_graph.cc
View File

@ -65,9 +65,9 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
trajectory_id, ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
insertion_submaps[0]->local_pose());
trajectory_id,
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *
insertion_submaps[0]->local_pose());
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0};
@ -415,8 +415,8 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
optimized_submap_transforms_.Insert(
submap_id, pose_graph::SubmapData{global_submap_pose});
global_submap_poses_.Insert(submap_id,
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;
@ -507,7 +507,7 @@ void PoseGraph::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).global_pose;
global_submap_poses_.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 =
@ -545,8 +545,8 @@ void PoseGraph::RunOptimization() {
// 'optimization_problem_' yet.
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id);
const auto local_to_old_global =
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
@ -560,7 +560,7 @@ void PoseGraph::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
optimized_submap_transforms_ = submap_data;
global_submap_poses_ = submap_data;
// Log the histograms for the pose residuals.
if (options_.log_residual_histograms()) {
@ -621,8 +621,7 @@ transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
}
std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
@ -648,10 +647,10 @@ PoseGraph::GetAllSubmapData() {
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
global_submap_poses,
const int trajectory_id) const {
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
const auto it = initial_trajectory_poses_.find(trajectory_id);
if (it != initial_trajectory_poses_.end()) {
@ -664,7 +663,7 @@ transform::Rigid3d PoseGraph::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).global_pose *
return global_submap_poses.at(last_optimized_submap_id).global_pose *
submap_data_.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
@ -677,12 +676,12 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
return {};
}
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
if (global_submap_poses_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id).global_pose};
return {submap, global_submap_poses_.at(submap_id).global_pose};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
return {submap, ComputeLocalToGlobalTransform(global_submap_poses_,
submap_id.trajectory_id) *
submap->local_pose()};
}

8
cartographer/mapping_3d/pose_graph.h
View File

@ -173,10 +173,10 @@ class PoseGraph : public mapping::PoseGraph {
void RunOptimization() EXCLUDES(mutex_);
// Computes the local to global map frame transform based on the given
// optimized 'submap_transforms'.
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
mapping::PoseGraph::SubmapData GetSubmapDataUnderLock(
@ -230,9 +230,9 @@ class PoseGraph : public mapping::PoseGraph {
GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data.
// Global submap poses currently used for displaying data.
mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
optimized_submap_transforms_ GUARDED_BY(mutex_);
global_submap_poses_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_