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 89544fa..90cde25 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -273,12 +273,28 @@ void OptimizationProblem2D::Solve( continue; } - const transform::Rigid3d relative_pose = - ComputeRelativePose(trajectory_id, first_node_data, second_node_data); + // Add a relative pose constraint based on the odometry (if available). + std::unique_ptr relative_odometry = + CalculateOdometryBetweenNodes(trajectory_id, first_node_data, + second_node_data); + if (relative_odometry != nullptr) { + problem.AddResidualBlock( + SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{ + *relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).data(), + C_nodes.at(second_node_id).data()); + } + + // 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); problem.AddResidualBlock( - SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{ - relative_pose, options_.consecutive_node_translation_weight(), - options_.consecutive_node_rotation_weight()}), + SpaCostFunction2D::CreateAutoDiffCostFunction( + Constraint::Pose{relative_local_slam_pose, + options_.local_slam_pose_translation_weight(), + options_.local_slam_pose_rotation_weight()}), nullptr /* loss function */, C_nodes.at(first_node_id).data(), C_nodes.at(second_node_id).data()); } @@ -325,7 +341,8 @@ std::unique_ptr OptimizationProblem2D::InterpolateOdometry( .transform); } -transform::Rigid3d OptimizationProblem2D::ComputeRelativePose( +std::unique_ptr +OptimizationProblem2D::CalculateOdometryBetweenNodes( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { if (odometry_data_.HasTrajectory(trajectory_id)) { @@ -334,14 +351,15 @@ transform::Rigid3d OptimizationProblem2D::ComputeRelativePose( const std::unique_ptr second_node_odometry = InterpolateOdometry(trajectory_id, second_node_data.time); if (first_node_odometry != nullptr && second_node_odometry != nullptr) { - return transform::Rigid3d::Rotation(first_node_data.gravity_alignment) * - first_node_odometry->inverse() * (*second_node_odometry) * - transform::Rigid3d::Rotation( - second_node_data.gravity_alignment.inverse()); + transform::Rigid3d relative_odometry = + transform::Rigid3d::Rotation(first_node_data.gravity_alignment) * + first_node_odometry->inverse() * (*second_node_odometry) * + transform::Rigid3d::Rotation( + second_node_data.gravity_alignment.inverse()); + return common::make_unique(relative_odometry); } } - return transform::Embed3D(first_node_data.initial_pose.inverse() * - second_node_data.initial_pose); + return nullptr; } } // namespace pose_graph 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 48d8f85..30281a5 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h @@ -103,8 +103,8 @@ class OptimizationProblem2D private: std::unique_ptr InterpolateOdometry( int trajectory_id, common::Time time) const; - // Uses odometry if available, otherwise the local SLAM results. - transform::Rigid3d ComputeRelativePose( + // Computes the relative pose between two nodes based on odometry data. + std::unique_ptr CalculateOdometryBetweenNodes( int trajectory_id, const NodeData2D& first_node_data, const NodeData2D& second_node_data) const; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 8b0a973..d6b5a56 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -116,8 +116,10 @@ class PoseGraph2DTest : public ::testing::Test { acceleration_weight = 1., rotation_weight = 1e2, huber_scale = 1., - consecutive_node_translation_weight = 0., - consecutive_node_rotation_weight = 0., + local_slam_pose_translation_weight = 0., + local_slam_pose_rotation_weight = 0., + odometry_translation_weight = 0., + odometry_rotation_weight = 0., fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = true, diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc index dd1e43e..6daf799 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc @@ -437,8 +437,8 @@ void OptimizationProblem3D::Solve( } if (options_.fix_z_in_3d()) { - // Add penalties for violating odometry or changes between consecutive nodes - // if odometry is not available. + // Add penalties for violating odometry (if available) and changes between + // consecutive nodes. for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); @@ -459,12 +459,29 @@ void OptimizationProblem3D::Solve( continue; } - const transform::Rigid3d relative_pose = ComputeRelativePose( - trajectory_id, first_node_data, second_node_data); + // Add a relative pose constraint based on the odometry (if available). + const std::unique_ptr relative_odometry = + CalculateOdometryBetweenNodes(trajectory_id, first_node_data, + second_node_data); + if (relative_odometry != nullptr) { + problem.AddResidualBlock( + SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{ + *relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), + C_nodes.at(first_node_id).translation(), + C_nodes.at(second_node_id).rotation(), + C_nodes.at(second_node_id).translation()); + } + + // Add a relative pose constraint based on consecutive local SLAM poses. + const transform::Rigid3d relative_local_slam_pose = + first_node_data.local_pose.inverse() * second_node_data.local_pose; problem.AddResidualBlock( - SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{ - relative_pose, options_.consecutive_node_translation_weight(), - options_.consecutive_node_rotation_weight()}), + SpaCostFunction3D::CreateAutoDiffCostFunction( + Constraint::Pose{relative_local_slam_pose, + options_.local_slam_pose_translation_weight(), + options_.local_slam_pose_rotation_weight()}), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), C_nodes.at(first_node_id).translation(), C_nodes.at(second_node_id).rotation(), @@ -572,7 +589,8 @@ void OptimizationProblem3D::Solve( } } -transform::Rigid3d OptimizationProblem3D::ComputeRelativePose( +std::unique_ptr +OptimizationProblem3D::CalculateOdometryBetweenNodes( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { if (odometry_data_.HasTrajectory(trajectory_id)) { @@ -581,10 +599,12 @@ transform::Rigid3d OptimizationProblem3D::ComputeRelativePose( const std::unique_ptr second_node_odometry = Interpolate(odometry_data_, trajectory_id, second_node_data.time); if (first_node_odometry != nullptr && second_node_odometry != nullptr) { - return first_node_odometry->inverse() * (*second_node_odometry); + const transform::Rigid3d relative_odometry = + first_node_odometry->inverse() * (*second_node_odometry); + return common::make_unique(relative_odometry); } } - return first_node_data.local_pose.inverse() * second_node_data.local_pose; + return nullptr; } } // namespace pose_graph diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h index e36f666..044c4c5 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h @@ -116,8 +116,8 @@ class OptimizationProblem3D } private: - // Uses odometry if available, otherwise the local SLAM results. - transform::Rigid3d ComputeRelativePose( + // Computes the relative pose between two nodes based on odometry data. + std::unique_ptr CalculateOdometryBetweenNodes( int trajectory_id, const NodeData3D& first_node_data, const NodeData3D& second_node_data) const; diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc index 7c96f2b..4de86c4 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc @@ -42,8 +42,10 @@ class OptimizationProblem3DTest : public ::testing::Test { acceleration_weight = 1e-4, rotation_weight = 1e-2, huber_scale = 1., - consecutive_node_translation_weight = 1e-2, - consecutive_node_rotation_weight = 1e-2, + local_slam_pose_translation_weight = 1e-2, + local_slam_pose_rotation_weight = 1e-2, + odometry_translation_weight = 1e-2, + odometry_rotation_weight = 1e-2, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = true, diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc b/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc index 0e57806..7417eb0 100644 --- a/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc +++ b/cartographer/mapping/internal/pose_graph/optimization_problem_options.cc @@ -30,10 +30,14 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( parameter_dictionary->GetDouble("acceleration_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); - options.set_consecutive_node_translation_weight( - parameter_dictionary->GetDouble("consecutive_node_translation_weight")); - options.set_consecutive_node_rotation_weight( - parameter_dictionary->GetDouble("consecutive_node_rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_local_slam_pose_translation_weight( + parameter_dictionary->GetDouble("local_slam_pose_translation_weight")); + options.set_local_slam_pose_rotation_weight( + parameter_dictionary->GetDouble("local_slam_pose_rotation_weight")); options.set_fixed_frame_pose_translation_weight( parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); options.set_fixed_frame_pose_rotation_weight( diff --git a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto index 9827465..f399735 100644 --- a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto @@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 14 +// NEXT ID: 18 message OptimizationProblemOptions { // Scaling parameter for Huber loss function. double huber_scale = 1; @@ -29,11 +29,21 @@ message OptimizationProblemOptions { // Scaling parameter for the IMU rotation term. double rotation_weight = 9; - // Scaling parameter for translation between consecutive nodes. - double consecutive_node_translation_weight = 2; + // Scaling parameter for translation between consecutive nodes based on the + // local SLAM pose. + double local_slam_pose_translation_weight = 14; - // Scaling parameter for rotation between consecutive nodes. - double consecutive_node_rotation_weight = 3; + // Scaling parameter for rotation between consecutive nodes based on the + // local SLAM pose. + double local_slam_pose_rotation_weight = 15; + + // Scaling parameter for translation between consecutive nodes based on the + // odometry. + double odometry_translation_weight = 16; + + // Scaling parameter for rotation between consecutive nodes based on the + // odometry. + double odometry_rotation_weight = 17; // Scaling parameter for the FixedFramePose translation. double fixed_frame_pose_translation_weight = 11; diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index d69d3c8..11a073a 100644 --- a/configuration_files/pose_graph.lua +++ b/configuration_files/pose_graph.lua @@ -65,8 +65,10 @@ POSE_GRAPH = { huber_scale = 1e1, acceleration_weight = 1e3, rotation_weight = 3e5, - consecutive_node_translation_weight = 1e5, - consecutive_node_rotation_weight = 1e5, + local_slam_pose_translation_weight = 1e5, + local_slam_pose_rotation_weight = 1e5, + odometry_translation_weight = 1e5, + odometry_rotation_weight = 1e5, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = false, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 9b6cc19..9fd79f6 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -86,11 +86,17 @@ double acceleration_weight double rotation_weight Scaling parameter for the IMU rotation term. -double consecutive_node_translation_weight - Scaling parameter for translation between consecutive nodes. +double local_slam_pose_translation_weight + Scaling parameter for translation between consecutive nodes based on the local SLAM pose. -double consecutive_node_rotation_weight - Scaling parameter for rotation between consecutive nodes. +double local_slam_pose_rotation_weight + Scaling parameter for rotation between consecutive nodes based on the local SLAM pose. + +double odometry_translation_weight + Scaling parameter for translation between consecutive nodes based on the odometry. + +double odometry_rotation_weight + Scaling parameter for rotation between consecutive nodes based on the odometry. double fixed_frame_pose_translation_weight Scaling parameter for the FixedFramePose translation.