OptimizationProblemInterface (#991)

Derive optimization problems from a new OptimizationProblemInterface.
Move fix_z into OptimizationProblemOptions.
Move trivial getters to header files.
This will allow mocking the optimization problems.
master
gaschler 2018-03-15 11:12:26 +01:00 committed by Wally B. Feed
parent e6c4ee4b8b
commit ed47f9d8f8
15 changed files with 271 additions and 236 deletions

View File

@ -40,9 +40,8 @@ class LandmarkCostFunction2D {
PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation,
const OptimizationProblem2D::NodeData& prev_node,
const OptimizationProblem2D::NodeData& next_node) {
const LandmarkObservation& observation, const NodeData2D& prev_node,
const NodeData2D& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction2D, 6 /* residuals */,
3 /* previous node pose variables */, 3 /* next node pose variables */,
@ -72,8 +71,8 @@ class LandmarkCostFunction2D {
private:
LandmarkCostFunction2D(const LandmarkObservation& observation,
const OptimizationProblem2D::NodeData& prev_node,
const OptimizationProblem2D::NodeData& next_node)
const NodeData2D& prev_node,
const NodeData2D& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
prev_node_gravity_alignment_(prev_node.gravity_alignment),

View File

@ -34,10 +34,10 @@ using LandmarkObservation =
PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) {
OptimizationProblem2D::NodeData prev_node;
NodeData2D prev_node;
prev_node.time = common::FromUniversal(0);
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
OptimizationProblem2D::NodeData next_node;
NodeData2D next_node;
next_node.time = common::FromUniversal(10);
next_node.gravity_alignment = Eigen::Quaterniond::Identity();

View File

@ -42,8 +42,8 @@ namespace {
using ::cartographer::mapping::pose_graph::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using NodeData = OptimizationProblem2D::NodeData;
using SubmapData = OptimizationProblem2D::SubmapData;
using NodeData = NodeData2D;
using SubmapData = SubmapData2D;
// Converts a pose into the 3 optimization variable format used for Ceres:
// translation in x and y, followed by the rotation angle representing the
@ -149,20 +149,14 @@ void OptimizationProblem2D::AddOdometryData(
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem2D::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
node_data_.Append(trajectory_id,
NodeData{time, initial_pose, pose, gravity_alignment});
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) {
node_data_.Append(trajectory_id, node_data);
}
void OptimizationProblem2D::InsertTrajectoryNode(
const NodeId& node_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
node_data_.Insert(node_id,
NodeData{time, initial_pose, pose, gravity_alignment});
void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) {
node_data_.Insert(node_id, node_data);
}
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
@ -301,30 +295,6 @@ void OptimizationProblem2D::Solve(
}
}
const MapById<NodeId, NodeData>& OptimizationProblem2D::node_data() const {
return node_data_;
}
const MapById<SubmapId, SubmapData>& OptimizationProblem2D::submap_data()
const {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>&
OptimizationProblem2D::landmark_data() const {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem2D::imu_data()
const {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem2D::odometry_data() const {
return odometry_data_;
}
std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
const int trajectory_id, const common::Time time) const {
const auto it = odometry_data_.lower_bound(trajectory_id, time);

View File

@ -28,6 +28,7 @@
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/imu_data.h"
@ -39,23 +40,21 @@ namespace cartographer {
namespace mapping {
namespace pose_graph {
// Implements the SPA loop closure method.
class OptimizationProblem2D {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
struct NodeData {
struct NodeData2D {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
};
};
struct SubmapData {
struct SubmapData2D {
transform::Rigid2d global_pose;
};
};
class OptimizationProblem2D
: public OptimizationProblemInterface<NodeData2D, SubmapData2D,
transform::Rigid2d> {
public:
explicit OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options);
~OptimizationProblem2D();
@ -63,48 +62,55 @@ class OptimizationProblem2D {
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void TrimTrajectoryNode(const NodeId& node_id);
const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id,
const NodeData2D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id,
const NodeData2D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose);
const transform::Rigid2d& global_submap_pose) override;
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid2d& global_submap_pose);
void TrimSubmap(const SubmapId& submap_id);
const transform::Rigid2d& global_submap_pose) override;
void TrimSubmap(const SubmapId& submap_id) override;
void SetMaxNumIterations(int32 max_num_iterations) override;
void SetMaxNumIterations(int32 max_num_iterations);
// Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints,
void Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes);
const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeData>& node_data() const;
const MapById<SubmapId, SubmapData>& submap_data() const;
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
const MapById<NodeId, NodeData2D>& node_data() const override {
return node_data_;
}
const MapById<SubmapId, SubmapData2D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
const override {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>& odometry_data()
const override {
return odometry_data_;
}
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(
int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const;
int trajectory_id, const NodeData2D& first_node_data,
const NodeData2D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData> node_data_;
MapById<SubmapId, SubmapData> submap_data_;
MapById<NodeId, NodeData2D> node_data_;
MapById<SubmapId, SubmapData2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;

View File

@ -246,8 +246,9 @@ void PoseGraph2D::ComputeConstraintsForNode(
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
pose;
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment);
matching_id.trajectory_id,
pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will
@ -440,9 +441,8 @@ void PoseGraph2D::AddSubmapFromProto(
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(
submap_id,
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
global_submap_poses_.Insert(submap_id,
pose_graph::SubmapData2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
@ -465,11 +465,13 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse());
optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time,
node_id,
pose_graph::NodeData2D{
constant_data->time,
transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse),
transform::Project2D(global_pose * gravity_alignment_inverse),
constant_data->gravity_alignment);
constant_data->gravity_alignment});
});
}
@ -728,8 +730,7 @@ PoseGraph2D::GetAllSubmapPoses() {
}
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
global_submap_poses,
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

View File

@ -191,8 +191,7 @@ class PoseGraph2D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
global_submap_poses,
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
MapById<SubmapId, pose_graph::SubmapData2D> global_submap_poses_
GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

View File

@ -39,9 +39,8 @@ class LandmarkCostFunction3D {
PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation,
const OptimizationProblem3D::NodeData& prev_node,
const OptimizationProblem3D::NodeData& next_node) {
const LandmarkObservation& observation, const NodeData3D& prev_node,
const NodeData3D& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction3D, 6 /* residuals */,
4 /* previous node rotation variables */,
@ -77,8 +76,8 @@ class LandmarkCostFunction3D {
private:
LandmarkCostFunction3D(const LandmarkObservation& observation,
const OptimizationProblem3D::NodeData& prev_node,
const OptimizationProblem3D::NodeData& next_node)
const NodeData3D& prev_node,
const NodeData3D& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
translation_weight_(observation.translation_weight),

View File

@ -34,9 +34,9 @@ using LandmarkObservation =
PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunction3DTest, SmokeTest) {
OptimizationProblem3D::NodeData prev_node;
NodeData3D prev_node;
prev_node.time = common::FromUniversal(0);
OptimizationProblem3D::NodeData next_node;
NodeData3D next_node;
next_node.time = common::FromUniversal(10);
std::unique_ptr<ceres::CostFunction> cost_function(

View File

@ -52,8 +52,8 @@ namespace {
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using TrajectoryData =
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
using NodeData = OptimizationProblem3D::NodeData;
using SubmapData = OptimizationProblem3D::SubmapData;
using NodeData = NodeData3D;
using SubmapData = SubmapData3D;
// For odometry.
std::unique_ptr<transform::Rigid3d> Interpolate(
@ -179,8 +179,8 @@ void AddLandmarkCostFunctions(
} // namespace
OptimizationProblem3D::OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z)
: options_(options), fix_z_(fix_z) {}
const pose_graph::proto::OptimizationProblemOptions& options)
: options_(options) {}
OptimizationProblem3D::~OptimizationProblem3D() {}
@ -200,11 +200,9 @@ void OptimizationProblem3D::AddFixedFramePoseData(
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
}
void OptimizationProblem3D::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) {
node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id];
}
@ -213,11 +211,9 @@ void OptimizationProblem3D::SetTrajectoryData(
trajectory_data_[trajectory_id] = trajectory_data;
}
void OptimizationProblem3D::InsertTrajectoryNode(
const NodeId& node_id, const common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) {
node_data_.Insert(node_id, node_data);
trajectory_data_[node_id.trajectory_id];
}
@ -265,7 +261,7 @@ void OptimizationProblem3D::Solve(
const auto translation_parameterization =
[this]() -> std::unique_ptr<ceres::LocalParameterization> {
return fix_z_ == FixZ::kYes
return options_.fix_z_in_3d()
? common::make_unique<ceres::SubsetParameterization>(
3, std::vector<int>{2})
: nullptr;
@ -342,7 +338,7 @@ void OptimizationProblem3D::Solve(
&C_nodes, &C_landmarks, &problem);
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
if (fix_z_ == FixZ::kNo) {
if (!options_.fix_z_in_3d()) {
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);
@ -429,7 +425,7 @@ void OptimizationProblem3D::Solve(
}
}
if (fix_z_ == FixZ::kYes) {
if (options_.fix_z_in_3d()) {
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
@ -565,40 +561,6 @@ void OptimizationProblem3D::Solve(
}
}
const MapById<NodeId, NodeData>& OptimizationProblem3D::node_data() const {
return node_data_;
}
const MapById<SubmapId, SubmapData>& OptimizationProblem3D::submap_data()
const {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>&
OptimizationProblem3D::landmark_data() const {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem3D::imu_data()
const {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem3D::odometry_data() const {
return odometry_data_;
}
const sensor::MapByTime<sensor::FixedFramePoseData>&
OptimizationProblem3D::fixed_frame_pose_data() const {
return fixed_frame_pose_data_;
}
const std::map<int, TrajectoryData>& OptimizationProblem3D::trajectory_data()
const {
return trajectory_data_;
}
transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {

View File

@ -28,6 +28,7 @@
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
@ -40,80 +41,89 @@ namespace cartographer {
namespace mapping {
namespace pose_graph {
// Implements the SPA loop closure method.
class OptimizationProblem3D {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
struct NodeData {
struct NodeData3D {
common::Time time;
transform::Rigid3d local_pose;
transform::Rigid3d global_pose;
};
};
struct SubmapData {
struct SubmapData3D {
transform::Rigid3d global_pose;
};
};
enum class FixZ { kYes, kNo };
OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z);
class OptimizationProblem3D
: public OptimizationProblemInterface<NodeData3D, SubmapData3D,
transform::Rigid3d> {
public:
explicit OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options);
~OptimizationProblem3D();
OptimizationProblem3D(const OptimizationProblem3D&) = delete;
OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data);
const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id,
const NodeData3D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id,
const NodeData3D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose) override;
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose) override;
void TrimSubmap(const SubmapId& submap_id) override;
void SetMaxNumIterations(int32 max_num_iterations) override;
void Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeData3D>& node_data() const override {
return node_data_;
}
const MapById<SubmapId, SubmapData3D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
const override {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>& odometry_data()
const override {
return odometry_data_;
}
void AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
void SetTrajectoryData(
int trajectory_id,
const PoseGraphInterface::TrajectoryData& trajectory_data);
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
void TrimTrajectoryNode(const NodeId& node_id);
void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose);
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid3d& global_submap_pose);
void TrimSubmap(const SubmapId& submap_id);
void SetMaxNumIterations(int32 max_num_iterations);
// Optimizes the global poses.
void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes);
const MapById<NodeId, NodeData>& node_data() const;
const MapById<SubmapId, SubmapData>& submap_data() const;
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
const;
const {
return fixed_frame_pose_data_;
}
const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
const;
const {
return trajectory_data_;
}
private:
// Uses odometry if available, otherwise the local SLAM results.
transform::Rigid3d ComputeRelativePose(
int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const;
int trajectory_id, const NodeData3D& first_node_data,
const NodeData3D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
MapById<NodeId, NodeData> node_data_;
MapById<SubmapId, SubmapData> submap_data_;
MapById<NodeId, NodeData3D> node_data_;
MapById<SubmapId, SubmapData3D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;

View File

@ -34,9 +34,7 @@ namespace {
class OptimizationProblem3DTest : public ::testing::Test {
protected:
OptimizationProblem3DTest()
: optimization_problem_(CreateOptions(),
OptimizationProblem3D::FixZ::kNo),
rng_(45387) {}
: optimization_problem_(CreateOptions()), rng_(45387) {}
pose_graph::proto::OptimizationProblemOptions CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text(
@ -127,7 +125,8 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
optimization_problem_.AddImuData(
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose);
optimization_problem_.AddTrajectoryNode(kTrajectoryId,
NodeData3D{now, pose, pose});
now += common::FromSeconds(0.01);
}
@ -170,7 +169,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
const std::set<int> kFrozen;
const std::set<int> kFrozen = {};
optimization_problem_.Solve(constraints, kFrozen, {});
double translation_error_after = 0.;

View File

@ -42,8 +42,7 @@ namespace mapping {
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options(),
pose_graph::OptimizationProblem3D::FixZ::kNo),
optimization_problem_(options_.optimization_problem_options()),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph3D::~PoseGraph3D() {
@ -264,7 +263,8 @@ void PoseGraph3D::ComputeConstraintsForNode(
optimization_problem_.submap_data().at(matching_id).global_pose *
insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, local_pose, global_pose);
matching_id.trajectory_id,
pose_graph::NodeData3D{constant_data->time, local_pose, global_pose});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will only
@ -456,9 +456,8 @@ void PoseGraph3D::AddSubmapFromProto(
submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(
submap_id,
pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
global_submap_poses_.Insert(submap_id,
pose_graph::SubmapData3D{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
@ -479,7 +478,9 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time, constant_data->local_pose, global_pose);
node_id,
pose_graph::NodeData3D{constant_data->time, constant_data->local_pose,
global_pose});
});
}
@ -762,8 +763,7 @@ PoseGraph3D::GetAllSubmapPoses() {
}
transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
global_submap_poses,
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

View File

@ -191,8 +191,7 @@ class PoseGraph3D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
global_submap_poses,
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>
global_submap_poses_ GUARDED_BY(mutex_);
MapById<SubmapId, pose_graph::SubmapData3D> global_submap_poses_
GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

View File

@ -0,0 +1,88 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
#include <map>
#include <set>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/time.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
// Implements the SPA loop closure method.
template <typename NodeDataType, typename SubmapDataType,
typename RigidTransformType>
class OptimizationProblemInterface {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
OptimizationProblemInterface() {}
virtual ~OptimizationProblemInterface() {}
OptimizationProblemInterface(const OptimizationProblemInterface&) = delete;
OptimizationProblemInterface& operator=(const OptimizationProblemInterface&) =
delete;
virtual void AddImuData(int trajectory_id,
const sensor::ImuData& imu_data) = 0;
virtual void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) = 0;
virtual void AddTrajectoryNode(int trajectory_id,
const NodeDataType& node_data) = 0;
virtual void InsertTrajectoryNode(const NodeId& node_id,
const NodeDataType& node_data) = 0;
virtual void TrimTrajectoryNode(const NodeId& node_id) = 0;
virtual void AddSubmap(int trajectory_id,
const RigidTransformType& global_submap_pose) = 0;
virtual void InsertSubmap(const SubmapId& submap_id,
const RigidTransformType& global_submap_pose) = 0;
virtual void TrimSubmap(const SubmapId& submap_id) = 0;
virtual void SetMaxNumIterations(int32 max_num_iterations) = 0;
// Optimizes the global poses.
virtual void Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) = 0;
virtual const MapById<NodeId, NodeDataType>& node_data() const = 0;
virtual const MapById<SubmapId, SubmapDataType>& submap_data() const = 0;
virtual const std::map<std::string, transform::Rigid3d>& landmark_data()
const = 0;
virtual const sensor::MapByTime<sensor::ImuData>& imu_data() const = 0;
virtual const sensor::MapByTime<sensor::OdometryData>& odometry_data()
const = 0;
};
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_

View File

@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto;
import "cartographer/common/proto/ceres_solver_options.proto";
// NEXT ID: 13
// NEXT ID: 14
message OptimizationProblemOptions {
// Scaling parameter for Huber loss function.
double huber_scale = 1;
@ -41,6 +41,9 @@ message OptimizationProblemOptions {
// Scaling parameter for the FixedFramePose rotation.
double fixed_frame_pose_rotation_weight = 12;
// 3D only: fix Z.
bool fix_z_in_3d = 13;
// If true, the Ceres solver summary will be logged for every optimization.
bool log_solver_summary = 5;