12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
View File

@ -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<transform::Rigid3d> 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<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
.transform);
}
transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
std::unique_ptr<transform::Rigid3d>
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<transform::Rigid3d> 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<transform::Rigid3d>(relative_odometry);
}
}
return transform::Embed3D(first_node_data.initial_pose.inverse() *
second_node_data.initial_pose);
return nullptr;
}
} // namespace pose_graph

4
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
View File

@ -103,8 +103,8 @@ class OptimizationProblem2D
private:
std::unique_ptr<transform::Rigid3d> 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<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData2D& first_node_data,
const NodeData2D& second_node_data) const;

6
cartographer/mapping/internal/2d/pose_graph_2d_test.cc
View File

@ -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,

40
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
View File

@ -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<transform::Rigid3d> 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<transform::Rigid3d>
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<transform::Rigid3d> 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<transform::Rigid3d>(relative_odometry);
}
}
return first_node_data.local_pose.inverse() * second_node_data.local_pose;
return nullptr;
}
} // namespace pose_graph

4
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h
View File

@ -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<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData3D& first_node_data,
const NodeData3D& second_node_data) const;

6
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc
View File

@ -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,

12
cartographer/mapping/internal/pose_graph/optimization_problem_options.cc
View File

@ -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(

20
cartographer/mapping/pose_graph/proto/optimization_problem_options.proto
View File

@ -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;

6
configuration_files/pose_graph.lua
View File

@ -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,

14
docs/source/configuration.rst
View File

@ -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.