diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc index 90cde25..299a07f 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -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(); diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h index 30281a5..f15c111 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h @@ -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; }; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 5a40c9b..55cb8f2 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -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); }