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

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

9
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h
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),

4
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc
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();

46
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
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);

96
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
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 {
struct NodeData2D {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
};
struct SubmapData2D {
transform::Rigid2d global_pose;
};
class OptimizationProblem2D
: public OptimizationProblemInterface<NodeData2D, SubmapData2D,
transform::Rigid2d> {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
struct NodeData {
common::Time time;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
};
struct SubmapData {
transform::Rigid2d global_pose;
};
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);
void Solve(
const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) override;
// 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 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_;

25
cartographer/mapping/internal/2d/pose_graph_2d.cc
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,
transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse),
transform::Project2D(global_pose * gravity_alignment_inverse),
constant_data->gravity_alignment);
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});
});
}
@ -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);

7
cartographer/mapping/internal/2d/pose_graph_2d.h
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>

9
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h
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),

4
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc
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(

64
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
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 {

116
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h
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 {
struct NodeData3D {
common::Time time;
transform::Rigid3d local_pose;
transform::Rigid3d global_pose;
};
struct SubmapData3D {
transform::Rigid3d global_pose;
};
class OptimizationProblem3D
: public OptimizationProblemInterface<NodeData3D, SubmapData3D,
transform::Rigid3d> {
public:
using Constraint = PoseGraphInterface::Constraint;
using LandmarkNode = PoseGraphInterface::LandmarkNode;
struct NodeData {
common::Time time;
transform::Rigid3d local_pose;
transform::Rigid3d global_pose;
};
struct SubmapData {
transform::Rigid3d global_pose;
};
enum class FixZ { kYes, kNo };
OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z);
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_;

9
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc
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.;

18
cartographer/mapping/internal/3d/pose_graph_3d.cc
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);

7
cartographer/mapping/internal/3d/pose_graph_3d.h
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>

88
cartographer/mapping/internal/pose_graph/optimization_problem_interface.h Normal file
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_

5
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: 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;