Separate global optimization constraints for local SLAM and odometry (#1029)

Implements [RFC-0018](https://github.com/googlecartographer/rfcs/blob/master/text/0018-odometry-use-in-optimization-problem.md)
master
Michael Grupp 2018-04-10 14:44:43 +02:00 committed by Wally B. Feed
parent cf358e7640
commit 4351bdb3c8
10 changed files with 109 additions and 45 deletions

View File

@ -273,12 +273,28 @@ void OptimizationProblem2D::Solve(
continue; continue;
} }
const transform::Rigid3d relative_pose = // Add a relative pose constraint based on the odometry (if available).
ComputeRelativePose(trajectory_id, first_node_data, second_node_data); std::unique_ptr<transform::Rigid3d> relative_odometry =
CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
second_node_data);
if (relative_odometry != nullptr) {
problem.AddResidualBlock( problem.AddResidualBlock(
SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{ SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(), *relative_odometry, options_.odometry_translation_weight(),
options_.consecutive_node_rotation_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_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(), nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data()); C_nodes.at(second_node_id).data());
} }
@ -325,7 +341,8 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
.transform); .transform);
} }
transform::Rigid3d OptimizationProblem2D::ComputeRelativePose( std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data, const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const { const NodeData& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) { if (odometry_data_.HasTrajectory(trajectory_id)) {
@ -334,14 +351,15 @@ transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
const std::unique_ptr<transform::Rigid3d> second_node_odometry = const std::unique_ptr<transform::Rigid3d> second_node_odometry =
InterpolateOdometry(trajectory_id, second_node_data.time); InterpolateOdometry(trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) { if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
return transform::Rigid3d::Rotation(first_node_data.gravity_alignment) * transform::Rigid3d relative_odometry =
transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
first_node_odometry->inverse() * (*second_node_odometry) * first_node_odometry->inverse() * (*second_node_odometry) *
transform::Rigid3d::Rotation( transform::Rigid3d::Rotation(
second_node_data.gravity_alignment.inverse()); second_node_data.gravity_alignment.inverse());
return common::make_unique<transform::Rigid3d>(relative_odometry);
} }
} }
return transform::Embed3D(first_node_data.initial_pose.inverse() * return nullptr;
second_node_data.initial_pose);
} }
} // namespace pose_graph } // namespace pose_graph

View File

@ -103,8 +103,8 @@ class OptimizationProblem2D
private: private:
std::unique_ptr<transform::Rigid3d> InterpolateOdometry( std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
int trajectory_id, common::Time time) const; int trajectory_id, common::Time time) const;
// Uses odometry if available, otherwise the local SLAM results. // Computes the relative pose between two nodes based on odometry data.
transform::Rigid3d ComputeRelativePose( std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData2D& first_node_data, int trajectory_id, const NodeData2D& first_node_data,
const NodeData2D& second_node_data) const; const NodeData2D& second_node_data) const;

View File

@ -116,8 +116,10 @@ class PoseGraph2DTest : public ::testing::Test {
acceleration_weight = 1., acceleration_weight = 1.,
rotation_weight = 1e2, rotation_weight = 1e2,
huber_scale = 1., huber_scale = 1.,
consecutive_node_translation_weight = 0., local_slam_pose_translation_weight = 0.,
consecutive_node_rotation_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_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = true, log_solver_summary = true,

View File

@ -437,8 +437,8 @@ void OptimizationProblem3D::Solve(
} }
if (options_.fix_z_in_3d()) { if (options_.fix_z_in_3d()) {
// Add penalties for violating odometry or changes between consecutive nodes // Add penalties for violating odometry (if available) and changes between
// if odometry is not available. // consecutive nodes.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id; const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
@ -459,12 +459,29 @@ void OptimizationProblem3D::Solve(
continue; continue;
} }
const transform::Rigid3d relative_pose = ComputeRelativePose( // Add a relative pose constraint based on the odometry (if available).
trajectory_id, first_node_data, second_node_data); const std::unique_ptr<transform::Rigid3d> relative_odometry =
CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
second_node_data);
if (relative_odometry != nullptr) {
problem.AddResidualBlock( problem.AddResidualBlock(
SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{ SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(), *relative_odometry, options_.odometry_translation_weight(),
options_.consecutive_node_rotation_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_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(), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(first_node_id).translation(), C_nodes.at(first_node_id).translation(),
C_nodes.at(second_node_id).rotation(), C_nodes.at(second_node_id).rotation(),
@ -572,7 +589,8 @@ void OptimizationProblem3D::Solve(
} }
} }
transform::Rigid3d OptimizationProblem3D::ComputeRelativePose( std::unique_ptr<transform::Rigid3d>
OptimizationProblem3D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data, const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const { const NodeData& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) { if (odometry_data_.HasTrajectory(trajectory_id)) {
@ -581,10 +599,12 @@ transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
const std::unique_ptr<transform::Rigid3d> second_node_odometry = const std::unique_ptr<transform::Rigid3d> second_node_odometry =
Interpolate(odometry_data_, trajectory_id, second_node_data.time); Interpolate(odometry_data_, trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) { 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<transform::Rigid3d>(relative_odometry);
} }
} }
return first_node_data.local_pose.inverse() * second_node_data.local_pose; return nullptr;
} }
} // namespace pose_graph } // namespace pose_graph

View File

@ -116,8 +116,8 @@ class OptimizationProblem3D
} }
private: private:
// Uses odometry if available, otherwise the local SLAM results. // Computes the relative pose between two nodes based on odometry data.
transform::Rigid3d ComputeRelativePose( std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData3D& first_node_data, int trajectory_id, const NodeData3D& first_node_data,
const NodeData3D& second_node_data) const; const NodeData3D& second_node_data) const;

View File

@ -42,8 +42,10 @@ class OptimizationProblem3DTest : public ::testing::Test {
acceleration_weight = 1e-4, acceleration_weight = 1e-4,
rotation_weight = 1e-2, rotation_weight = 1e-2,
huber_scale = 1., huber_scale = 1.,
consecutive_node_translation_weight = 1e-2, local_slam_pose_translation_weight = 1e-2,
consecutive_node_rotation_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_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = true, log_solver_summary = true,

View File

@ -30,10 +30,14 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
parameter_dictionary->GetDouble("acceleration_weight")); parameter_dictionary->GetDouble("acceleration_weight"));
options.set_rotation_weight( options.set_rotation_weight(
parameter_dictionary->GetDouble("rotation_weight")); parameter_dictionary->GetDouble("rotation_weight"));
options.set_consecutive_node_translation_weight( options.set_odometry_translation_weight(
parameter_dictionary->GetDouble("consecutive_node_translation_weight")); parameter_dictionary->GetDouble("odometry_translation_weight"));
options.set_consecutive_node_rotation_weight( options.set_odometry_rotation_weight(
parameter_dictionary->GetDouble("consecutive_node_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( options.set_fixed_frame_pose_translation_weight(
parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight"));
options.set_fixed_frame_pose_rotation_weight( options.set_fixed_frame_pose_rotation_weight(

View File

@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto;
import "cartographer/common/proto/ceres_solver_options.proto"; import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 14 // NEXT ID: 18
message OptimizationProblemOptions { message OptimizationProblemOptions {
// Scaling parameter for Huber loss function. // Scaling parameter for Huber loss function.
double huber_scale = 1; double huber_scale = 1;
@ -29,11 +29,21 @@ message OptimizationProblemOptions {
// Scaling parameter for the IMU rotation term. // Scaling parameter for the IMU rotation term.
double rotation_weight = 9; double rotation_weight = 9;
// Scaling parameter for translation between consecutive nodes. // Scaling parameter for translation between consecutive nodes based on the
double consecutive_node_translation_weight = 2; // local SLAM pose.
double local_slam_pose_translation_weight = 14;
// Scaling parameter for rotation between consecutive nodes. // Scaling parameter for rotation between consecutive nodes based on the
double consecutive_node_rotation_weight = 3; // 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. // Scaling parameter for the FixedFramePose translation.
double fixed_frame_pose_translation_weight = 11; double fixed_frame_pose_translation_weight = 11;

View File

@ -65,8 +65,10 @@ POSE_GRAPH = {
huber_scale = 1e1, huber_scale = 1e1,
acceleration_weight = 1e3, acceleration_weight = 1e3,
rotation_weight = 3e5, rotation_weight = 3e5,
consecutive_node_translation_weight = 1e5, local_slam_pose_translation_weight = 1e5,
consecutive_node_rotation_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_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = false, log_solver_summary = false,

View File

@ -86,11 +86,17 @@ double acceleration_weight
double rotation_weight double rotation_weight
Scaling parameter for the IMU rotation term. Scaling parameter for the IMU rotation term.
double consecutive_node_translation_weight double local_slam_pose_translation_weight
Scaling parameter for translation between consecutive nodes. Scaling parameter for translation between consecutive nodes based on the local SLAM pose.
double consecutive_node_rotation_weight double local_slam_pose_rotation_weight
Scaling parameter for rotation between consecutive nodes. 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 double fixed_frame_pose_translation_weight
Scaling parameter for the FixedFramePose translation. Scaling parameter for the FixedFramePose translation.