diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index fce3fc9..68decca 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -37,9 +37,7 @@ class Data { public: Data(int milliseconds) : time_(CreateTime(milliseconds)) {} - const common::Time& time() const { - return time_; - } + const common::Time& time() const { return time_; } private: const common::Time time_; diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index a09d3c0..de07d57 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -94,7 +94,6 @@ void OptimizationProblem::InsertTrajectoryNode( NodeData{time, initial_pose, pose, gravity_alignment}); } - void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { node_data_.Trim(node_id); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index a23371b..86e00c3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -175,7 +175,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, optimization_problem_.submap_data().at(submap_id).pose.inverse(); const transform::Rigid3d initial_relative_pose = - inverse_submap_pose * optimization_problem_.node_data().at(node_id).pose; + inverse_submap_pose * + optimization_problem_.node_data().at(node_id).global_pose; std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : @@ -500,7 +501,7 @@ void SparsePoseGraph::RunOptimization() { const auto& node_data = optimization_problem_.node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { for (const auto& node : node_data.trajectory(trajectory_id)) { - trajectory_nodes_.at(node.id).global_pose = node.data.pose; + trajectory_nodes_.at(node.id).global_pose = node.data.global_pose; } // Extrapolate all point cloud poses that were not included in the diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 58814c5..f3f3d35 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -82,20 +82,22 @@ void OptimizationProblem::AddFixedFramePoseData( void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, - const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { + const transform::Rigid3d& local_pose, + const transform::Rigid3d& global_pose) { CHECK_GE(trajectory_id, 0); trajectory_data_.resize(std::max(trajectory_data_.size(), static_cast(trajectory_id) + 1)); - node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose}); + node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose}); } void OptimizationProblem::InsertTrajectoryNode( const mapping::NodeId& node_id, const common::Time time, - const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { + const transform::Rigid3d& local_pose, + const transform::Rigid3d& global_pose) { CHECK_GE(node_id.trajectory_id, 0); trajectory_data_.resize(std::max( trajectory_data_.size(), static_cast(node_id.trajectory_id) + 1)); - node_data_.Insert(node_id, NodeData{time, initial_pose, pose}); + node_data_.Insert(node_id, NodeData{time, local_pose, global_pose}); } void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { @@ -197,7 +199,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, frozen_trajectories.count(node_id_data.id.trajectory_id) != 0; C_nodes.Insert( node_id_data.id, - CeresPose(node_id_data.data.pose, translation_parameterization(), + CeresPose(node_id_data.data.global_pose, translation_parameterization(), common::make_unique(), &problem)); if (frozen) { @@ -335,8 +337,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, .inverse() * odometry_data_[trajectory_id].Lookup( second_node_data.time) - : first_node_data.initial_pose.inverse() * - second_node_data.initial_pose; + : first_node_data.local_pose.inverse() * + second_node_data.local_pose; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ @@ -376,7 +378,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, if (!fixed_frame_pose_initialized) { const transform::Rigid3d fixed_frame_pose_in_map = - node_data.pose * constraint_pose.zbar_ij.inverse(); + node_data.global_pose * constraint_pose.zbar_ij.inverse(); C_fixed_frames.emplace_back( transform::Rigid3d( fixed_frame_pose_in_map.translation(), @@ -428,7 +430,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); } for (const auto& C_node_id_data : C_nodes) { - node_data_.at(C_node_id_data.id).pose = C_node_id_data.data.ToRigid(); + node_data_.at(C_node_id_data.id).global_pose = + C_node_id_data.data.ToRigid(); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index c205ece..9e444b7 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -41,8 +41,8 @@ namespace sparse_pose_graph { struct NodeData { common::Time time; - transform::Rigid3d initial_pose; - transform::Rigid3d pose; + transform::Rigid3d local_pose; + transform::Rigid3d global_pose; }; struct SubmapData { @@ -72,11 +72,11 @@ class OptimizationProblem { int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); void AddTrajectoryNode(int trajectory_id, common::Time time, - const transform::Rigid3d& initial_pose, - const transform::Rigid3d& pose); + const transform::Rigid3d& local_pose, + const transform::Rigid3d& global_pose); void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, - const transform::Rigid3d& initial_pose, - const transform::Rigid3d& pose); + 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 InsertSubmap(const mapping::SubmapId& submap_id, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 9685c02..ed01c89 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -158,13 +158,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double rotation_error_before = 0.; const auto& node_data = optimization_problem_.node_data(); for (int j = 0; j != kNumNodes; ++j) { - translation_error_before += - (test_data[j].ground_truth_pose.translation() - - node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) - .norm(); + translation_error_before += (test_data[j].ground_truth_pose.translation() - + node_data.at(mapping::NodeId{kTrajectoryId, j}) + .global_pose.translation()) + .norm(); rotation_error_before += transform::GetAngle( test_data[j].ground_truth_pose.inverse() * - node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); + node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose); } optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); @@ -176,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double translation_error_after = 0.; double rotation_error_after = 0.; for (int j = 0; j != kNumNodes; ++j) { - translation_error_after += - (test_data[j].ground_truth_pose.translation() - - node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) - .norm(); + translation_error_after += (test_data[j].ground_truth_pose.translation() - + node_data.at(mapping::NodeId{kTrajectoryId, j}) + .global_pose.translation()) + .norm(); rotation_error_after += transform::GetAngle( test_data[j].ground_truth_pose.inverse() * - node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); + node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose); } EXPECT_GT(0.8 * translation_error_before, translation_error_after);