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

Improve pose terminology of 2D optimization problem. (#1053) 

Removes the confusing term `initial_pose` from global 2D optimization.
It makes sense in other areas like scan matching, but has a completely
different meaning there. Also matches the 3D equivalents better.

- `local_pose_2d` replaces `initial_pose` as the name for the
   non-gravity-aligned, local 2D pose in the submap
- `global_pose_2d` replaces `pose` as the name for the
   non-gravity-aligned, global 2D pose that is optimized

See the comment in the PR for how this improves readability.
master
Michael Grupp 2018-04-12 16:00:56 +02:00 committed by Wally B. Feed
parent 14465aa23e
commit d2f29d04b9
3 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
View File

@ -231,7 +231,7 @@ void OptimizationProblem2D::Solve(
for (const auto& node_id_data : node_data_) {
const bool frozen =
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.pose));
C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
if (frozen) {
problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
@ -288,8 +288,8 @@ void OptimizationProblem2D::Solve(
// Add a relative pose constraint based on consecutive local SLAM poses.
const transform::Rigid3d relative_local_slam_pose =
transform::Embed3D(first_node_data.initial_pose.inverse() *
second_node_data.initial_pose);
transform::Embed3D(first_node_data.local_pose_2d.inverse() *
second_node_data.local_pose_2d);
problem.AddResidualBlock(
SpaCostFunction2D::CreateAutoDiffCostFunction(
Constraint::Pose{relative_local_slam_pose,
@ -315,7 +315,8 @@ void OptimizationProblem2D::Solve(
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);
node_data_.at(C_node_id_data.id).global_pose_2d =
ToPose(C_node_id_data.data);
}
for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();

4
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
View File

@ -42,8 +42,8 @@ namespace pose_graph {
struct NodeData2D {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
transform::Rigid2d local_pose_2d;
transform::Rigid2d global_pose_2d;
Eigen::Quaterniond gravity_alignment;
};

15
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -209,7 +209,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
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).global_pose_2d;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
@ -240,16 +240,16 @@ void PoseGraph2D::ComputeConstraintsForNode(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const SubmapId matching_id = submap_ids.front();
const transform::Rigid2d pose = transform::Project2D(
const transform::Rigid2d local_pose_2d = transform::Project2D(
constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose =
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
pose;
local_pose_2d;
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
pose_graph::NodeData2D{constant_data->time, local_pose_2d, global_pose_2d,
constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
@ -258,7 +258,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
constraints_.push_back(Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
@ -567,7 +568,7 @@ void PoseGraph2D::RunOptimization() {
for (const auto& node : node_data.trajectory(trajectory_id)) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.pose) *
transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation(
mutable_trajectory_node.constant_data->gravity_alignment);
}