diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 62907fc..ccfbcbb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -194,7 +194,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .at(node_id.trajectory_id) .at(node_id.node_index - optimization_problem_.num_trimmed_nodes( node_id.trajectory_id)) - .point_cloud_pose; + .pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), @@ -444,8 +444,8 @@ void SparsePoseGraph::RunOptimization() { for (; node_data_index != static_cast(node_data[trajectory_id].size()); ++node_data_index, ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; - trajectory_nodes_.at(node_id).pose = transform::Embed3D( - node_data[trajectory_id][node_data_index].point_cloud_pose); + trajectory_nodes_.at(node_id).pose = + transform::Embed3D(node_data[trajectory_id][node_data_index].pose); } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 597828d..e43887d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -80,13 +80,11 @@ void OptimizationProblem::AddOdometerData( void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, - const transform::Rigid2d& initial_point_cloud_pose, - const transform::Rigid2d& point_cloud_pose) { + const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose) { CHECK_GE(trajectory_id, 0); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); - node_data_[trajectory_id].push_back( - NodeData{time, initial_point_cloud_pose, point_cloud_pose}); + node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose}); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); } @@ -174,7 +172,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, // stay valid. C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size()); for (const NodeData& node_data : node_data_[trajectory_id]) { - C_nodes[trajectory_id].push_back(FromPose(node_data.point_cloud_pose)); + C_nodes[trajectory_id].push_back(FromPose(node_data.pose)); problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3); if (frozen) { problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data()); @@ -223,9 +221,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, node_data_[trajectory_id][node_data_index].time) : transform::Embed3D( node_data_[trajectory_id][node_data_index - 1] - .initial_point_cloud_pose.inverse() * - node_data_[trajectory_id][node_data_index] - .initial_point_cloud_pose); + .initial_pose.inverse() * + node_data_[trajectory_id][node_data_index].initial_pose); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ @@ -260,7 +257,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (size_t node_data_index = 0; node_data_index != node_data_[trajectory_id].size(); ++node_data_index) { - node_data_[trajectory_id][node_data_index].point_cloud_pose = + node_data_[trajectory_id][node_data_index].pose = ToPose(C_nodes[trajectory_id][node_data_index]); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index e856871..0df075e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -39,8 +39,8 @@ namespace sparse_pose_graph { struct NodeData { common::Time time; - transform::Rigid2d initial_point_cloud_pose; - transform::Rigid2d point_cloud_pose; + transform::Rigid2d initial_pose; + transform::Rigid2d pose; }; struct SubmapData { @@ -64,8 +64,8 @@ class OptimizationProblem { void AddOdometerData(int trajectory_id, const sensor::OdometryData& odometry_data); void AddTrajectoryNode(int trajectory_id, common::Time time, - const transform::Rigid2d& initial_point_cloud_pose, - const transform::Rigid2d& point_cloud_pose); + const transform::Rigid2d& initial_pose, + const transform::Rigid2d& pose); void TrimTrajectoryNode(const mapping::NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 5f07d81..80c835d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -180,7 +180,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, inverse_submap_pose * optimization_problem_.node_data() .at(node_id.trajectory_id) .at(node_id.node_index) - .point_cloud_pose; + .pose; std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : @@ -265,8 +265,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( .size()) : 0}; const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; - optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, - scan_data->time, optimized_pose); + optimization_problem_.AddTrajectoryNode( + matching_id.trajectory_id, scan_data->time, pose, optimized_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 @@ -481,7 +481,7 @@ void SparsePoseGraph::RunOptimization() { ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = - node_data[trajectory_id][node_index].point_cloud_pose; + node_data[trajectory_id][node_index].pose; } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = ComputeLocalToGlobalTransform( diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index f5218c5..c4314cc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -73,11 +73,11 @@ void OptimizationProblem::AddFixedFramePoseData( void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, - const transform::Rigid3d& point_cloud_pose) { + const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { CHECK_GE(trajectory_id, 0); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); - node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose}); + node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -151,7 +151,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { C_nodes[trajectory_id].emplace_back( - node_data_[trajectory_id][node_index].point_cloud_pose, + node_data_[trajectory_id][node_index].pose, translation_parameterization(), common::make_unique(), &problem); if (frozen) { @@ -284,8 +284,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, if (!fixed_frame_pose_initialized) { const transform::Rigid3d fixed_frame_pose_in_map = - node_data[node_index].point_cloud_pose * - constraint_pose.zbar_ij.inverse(); + node_data[node_index].pose * constraint_pose.zbar_ij.inverse(); C_fixed_frames.emplace_back( transform::Rigid3d( fixed_frame_pose_in_map.translation(), @@ -347,7 +346,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, ++trajectory_id) { for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { - node_data_[trajectory_id][node_index].point_cloud_pose = + node_data_[trajectory_id][node_index].pose = C_nodes[trajectory_id][node_index].ToRigid(); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index dcb11a7..dfa48fd 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -39,7 +39,8 @@ namespace sparse_pose_graph { struct NodeData { common::Time time; - transform::Rigid3d point_cloud_pose; + transform::Rigid3d initial_pose; + transform::Rigid3d pose; }; struct SubmapData { @@ -67,7 +68,8 @@ class OptimizationProblem { int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); void AddTrajectoryNode(int trajectory_id, common::Time time, - const transform::Rigid3d& point_cloud_pose); + const transform::Rigid3d& initial_pose, + const transform::Rigid3d& pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void SetMaxNumIterations(int32 max_num_iterations); 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 df2e2ad..1ad245a 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -127,7 +127,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { optimization_problem_.AddImuData( kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::Zero()}); - optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose); + optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose); now += common::FromSeconds(0.01); } @@ -159,11 +159,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { const auto& node_data = optimization_problem_.node_data().at(0); for (int j = 0; j != kNumNodes; ++j) { translation_error_before += (test_data[j].ground_truth_pose.translation() - - node_data[j].point_cloud_pose.translation()) + node_data[j].pose.translation()) .norm(); - rotation_error_before += - transform::GetAngle(test_data[j].ground_truth_pose.inverse() * - node_data[j].point_cloud_pose); + rotation_error_before += transform::GetAngle( + test_data[j].ground_truth_pose.inverse() * node_data[j].pose); } optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); @@ -176,11 +175,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { double rotation_error_after = 0.; for (int j = 0; j != kNumNodes; ++j) { translation_error_after += (test_data[j].ground_truth_pose.translation() - - node_data[j].point_cloud_pose.translation()) + node_data[j].pose.translation()) .norm(); - rotation_error_after += - transform::GetAngle(test_data[j].ground_truth_pose.inverse() * - node_data[j].point_cloud_pose); + rotation_error_after += transform::GetAngle( + test_data[j].ground_truth_pose.inverse() * node_data[j].pose); } EXPECT_GT(0.8 * translation_error_before, translation_error_after);