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
parent
e6c4ee4b8b
commit
ed47f9d8f8
|
@ -40,9 +40,8 @@ class LandmarkCostFunction2D {
|
||||||
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const LandmarkObservation& observation,
|
const LandmarkObservation& observation, const NodeData2D& prev_node,
|
||||||
const OptimizationProblem2D::NodeData& prev_node,
|
const NodeData2D& next_node) {
|
||||||
const OptimizationProblem2D::NodeData& next_node) {
|
|
||||||
return new ceres::AutoDiffCostFunction<
|
return new ceres::AutoDiffCostFunction<
|
||||||
LandmarkCostFunction2D, 6 /* residuals */,
|
LandmarkCostFunction2D, 6 /* residuals */,
|
||||||
3 /* previous node pose variables */, 3 /* next node pose variables */,
|
3 /* previous node pose variables */, 3 /* next node pose variables */,
|
||||||
|
@ -72,8 +71,8 @@ class LandmarkCostFunction2D {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
LandmarkCostFunction2D(const LandmarkObservation& observation,
|
LandmarkCostFunction2D(const LandmarkObservation& observation,
|
||||||
const OptimizationProblem2D::NodeData& prev_node,
|
const NodeData2D& prev_node,
|
||||||
const OptimizationProblem2D::NodeData& next_node)
|
const NodeData2D& next_node)
|
||||||
: landmark_to_tracking_transform_(
|
: landmark_to_tracking_transform_(
|
||||||
observation.landmark_to_tracking_transform),
|
observation.landmark_to_tracking_transform),
|
||||||
prev_node_gravity_alignment_(prev_node.gravity_alignment),
|
prev_node_gravity_alignment_(prev_node.gravity_alignment),
|
||||||
|
|
|
@ -34,10 +34,10 @@ using LandmarkObservation =
|
||||||
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
TEST(LandmarkCostFunctionTest, SmokeTest) {
|
TEST(LandmarkCostFunctionTest, SmokeTest) {
|
||||||
OptimizationProblem2D::NodeData prev_node;
|
NodeData2D prev_node;
|
||||||
prev_node.time = common::FromUniversal(0);
|
prev_node.time = common::FromUniversal(0);
|
||||||
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
||||||
OptimizationProblem2D::NodeData next_node;
|
NodeData2D next_node;
|
||||||
next_node.time = common::FromUniversal(10);
|
next_node.time = common::FromUniversal(10);
|
||||||
next_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
next_node.gravity_alignment = Eigen::Quaterniond::Identity();
|
||||||
|
|
||||||
|
|
|
@ -42,8 +42,8 @@ namespace {
|
||||||
|
|
||||||
using ::cartographer::mapping::pose_graph::CeresPose;
|
using ::cartographer::mapping::pose_graph::CeresPose;
|
||||||
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
||||||
using NodeData = OptimizationProblem2D::NodeData;
|
using NodeData = NodeData2D;
|
||||||
using SubmapData = OptimizationProblem2D::SubmapData;
|
using SubmapData = SubmapData2D;
|
||||||
|
|
||||||
// Converts a pose into the 3 optimization variable format used for Ceres:
|
// Converts a pose into the 3 optimization variable format used for Ceres:
|
||||||
// translation in x and y, followed by the rotation angle representing the
|
// 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);
|
odometry_data_.Append(trajectory_id, odometry_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem2D::AddTrajectoryNode(
|
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
|
||||||
const int trajectory_id, const common::Time time,
|
const NodeData& node_data) {
|
||||||
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
node_data_.Append(trajectory_id, node_data);
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
|
||||||
node_data_.Append(trajectory_id,
|
|
||||||
NodeData{time, initial_pose, pose, gravity_alignment});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem2D::InsertTrajectoryNode(
|
void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
|
||||||
const NodeId& node_id, const common::Time time,
|
const NodeData& node_data) {
|
||||||
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
|
node_data_.Insert(node_id, node_data);
|
||||||
const Eigen::Quaterniond& gravity_alignment) {
|
|
||||||
node_data_.Insert(node_id,
|
|
||||||
NodeData{time, initial_pose, pose, gravity_alignment});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
|
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(
|
std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
|
||||||
const int trajectory_id, const common::Time time) const {
|
const int trajectory_id, const common::Time time) const {
|
||||||
const auto it = odometry_data_.lower_bound(trajectory_id, time);
|
const auto it = odometry_data_.lower_bound(trajectory_id, time);
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping/id.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/proto/optimization_problem_options.pb.h"
|
||||||
#include "cartographer/mapping/pose_graph_interface.h"
|
#include "cartographer/mapping/pose_graph_interface.h"
|
||||||
#include "cartographer/sensor/imu_data.h"
|
#include "cartographer/sensor/imu_data.h"
|
||||||
|
@ -39,23 +40,21 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
// Implements the SPA loop closure method.
|
struct NodeData2D {
|
||||||
class OptimizationProblem2D {
|
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:
|
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(
|
explicit OptimizationProblem2D(
|
||||||
const pose_graph::proto::OptimizationProblemOptions& options);
|
const pose_graph::proto::OptimizationProblemOptions& options);
|
||||||
~OptimizationProblem2D();
|
~OptimizationProblem2D();
|
||||||
|
@ -63,48 +62,55 @@ class OptimizationProblem2D {
|
||||||
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
|
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
|
||||||
OptimizationProblem2D& operator=(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,
|
void AddOdometryData(int trajectory_id,
|
||||||
const sensor::OdometryData& odometry_data);
|
const sensor::OdometryData& odometry_data) override;
|
||||||
void AddTrajectoryNode(int trajectory_id, common::Time time,
|
void AddTrajectoryNode(int trajectory_id,
|
||||||
const transform::Rigid2d& initial_pose,
|
const NodeData2D& node_data) override;
|
||||||
const transform::Rigid2d& pose,
|
void InsertTrajectoryNode(const NodeId& node_id,
|
||||||
const Eigen::Quaterniond& gravity_alignment);
|
const NodeData2D& node_data) override;
|
||||||
void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
|
void TrimTrajectoryNode(const NodeId& node_id) override;
|
||||||
const transform::Rigid2d& initial_pose,
|
|
||||||
const transform::Rigid2d& pose,
|
|
||||||
const Eigen::Quaterniond& gravity_alignment);
|
|
||||||
void TrimTrajectoryNode(const NodeId& node_id);
|
|
||||||
void AddSubmap(int trajectory_id,
|
void AddSubmap(int trajectory_id,
|
||||||
const transform::Rigid2d& global_submap_pose);
|
const transform::Rigid2d& global_submap_pose) override;
|
||||||
void InsertSubmap(const SubmapId& submap_id,
|
void InsertSubmap(const SubmapId& submap_id,
|
||||||
const transform::Rigid2d& global_submap_pose);
|
const transform::Rigid2d& global_submap_pose) override;
|
||||||
void TrimSubmap(const SubmapId& submap_id);
|
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.
|
const MapById<NodeId, NodeData2D>& node_data() const override {
|
||||||
void Solve(const std::vector<Constraint>& constraints,
|
return node_data_;
|
||||||
const std::set<int>& frozen_trajectories,
|
}
|
||||||
const std::map<std::string, LandmarkNode>& landmark_nodes);
|
const MapById<SubmapId, SubmapData2D>& submap_data() const override {
|
||||||
|
return submap_data_;
|
||||||
const MapById<NodeId, NodeData>& node_data() const;
|
}
|
||||||
const MapById<SubmapId, SubmapData>& submap_data() const;
|
const std::map<std::string, transform::Rigid3d>& landmark_data()
|
||||||
const std::map<std::string, transform::Rigid3d>& landmark_data() const;
|
const override {
|
||||||
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
return landmark_data_;
|
||||||
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
|
}
|
||||||
|
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:
|
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.
|
// Uses odometry if available, otherwise the local SLAM results.
|
||||||
transform::Rigid3d ComputeRelativePose(
|
transform::Rigid3d ComputeRelativePose(
|
||||||
int trajectory_id, const NodeData& first_node_data,
|
int trajectory_id, const NodeData2D& first_node_data,
|
||||||
const NodeData& second_node_data) const;
|
const NodeData2D& second_node_data) const;
|
||||||
|
|
||||||
pose_graph::proto::OptimizationProblemOptions options_;
|
pose_graph::proto::OptimizationProblemOptions options_;
|
||||||
MapById<NodeId, NodeData> node_data_;
|
MapById<NodeId, NodeData2D> node_data_;
|
||||||
MapById<SubmapId, SubmapData> submap_data_;
|
MapById<SubmapId, SubmapData2D> submap_data_;
|
||||||
std::map<std::string, transform::Rigid3d> landmark_data_;
|
std::map<std::string, transform::Rigid3d> landmark_data_;
|
||||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||||
|
|
|
@ -246,8 +246,9 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
||||||
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
|
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
|
||||||
pose;
|
pose;
|
||||||
optimization_problem_.AddTrajectoryNode(
|
optimization_problem_.AddTrajectoryNode(
|
||||||
matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
|
matching_id.trajectory_id,
|
||||||
constant_data->gravity_alignment);
|
pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
|
||||||
|
constant_data->gravity_alignment});
|
||||||
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
||||||
const SubmapId submap_id = submap_ids[i];
|
const SubmapId submap_id = submap_ids[i];
|
||||||
// Even if this was the last node added to 'submap_id', the submap will
|
// 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_.Insert(submap_id, SubmapData());
|
||||||
submap_data_.at(submap_id).submap = submap_ptr;
|
submap_data_.at(submap_id).submap = submap_ptr;
|
||||||
// Immediately show the submap at the 'global_submap_pose'.
|
// Immediately show the submap at the 'global_submap_pose'.
|
||||||
global_submap_poses_.Insert(
|
global_submap_poses_.Insert(submap_id,
|
||||||
submap_id,
|
pose_graph::SubmapData2D{global_submap_pose_2d});
|
||||||
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
|
|
||||||
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
|
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
|
||||||
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
||||||
optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
|
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(
|
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
|
||||||
constant_data->gravity_alignment.inverse());
|
constant_data->gravity_alignment.inverse());
|
||||||
optimization_problem_.InsertTrajectoryNode(
|
optimization_problem_.InsertTrajectoryNode(
|
||||||
node_id, constant_data->time,
|
node_id,
|
||||||
transform::Project2D(constant_data->local_pose *
|
pose_graph::NodeData2D{
|
||||||
gravity_alignment_inverse),
|
constant_data->time,
|
||||||
transform::Project2D(global_pose * gravity_alignment_inverse),
|
transform::Project2D(constant_data->local_pose *
|
||||||
constant_data->gravity_alignment);
|
gravity_alignment_inverse),
|
||||||
|
transform::Project2D(global_pose * gravity_alignment_inverse),
|
||||||
|
constant_data->gravity_alignment});
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -728,8 +730,7 @@ PoseGraph2D::GetAllSubmapPoses() {
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
|
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
|
||||||
global_submap_poses,
|
|
||||||
const int trajectory_id) const {
|
const int trajectory_id) const {
|
||||||
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
||||||
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
|
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
|
||||||
|
|
|
@ -191,8 +191,7 @@ class PoseGraph2D : public PoseGraph {
|
||||||
// Computes the local to global map frame transform based on the given
|
// Computes the local to global map frame transform based on the given
|
||||||
// 'global_submap_poses'.
|
// 'global_submap_poses'.
|
||||||
transform::Rigid3d ComputeLocalToGlobalTransform(
|
transform::Rigid3d ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
|
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
|
||||||
global_submap_poses,
|
|
||||||
int trajectory_id) const REQUIRES(mutex_);
|
int trajectory_id) const REQUIRES(mutex_);
|
||||||
|
|
||||||
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
|
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
|
||||||
|
@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph {
|
||||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||||
|
|
||||||
// Global submap poses currently used for displaying data.
|
// Global submap poses currently used for displaying data.
|
||||||
MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>
|
MapById<SubmapId, pose_graph::SubmapData2D> global_submap_poses_
|
||||||
global_submap_poses_ GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Global landmark poses with all observations.
|
// Global landmark poses with all observations.
|
||||||
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
||||||
|
|
|
@ -39,9 +39,8 @@ class LandmarkCostFunction3D {
|
||||||
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
static ceres::CostFunction* CreateAutoDiffCostFunction(
|
||||||
const LandmarkObservation& observation,
|
const LandmarkObservation& observation, const NodeData3D& prev_node,
|
||||||
const OptimizationProblem3D::NodeData& prev_node,
|
const NodeData3D& next_node) {
|
||||||
const OptimizationProblem3D::NodeData& next_node) {
|
|
||||||
return new ceres::AutoDiffCostFunction<
|
return new ceres::AutoDiffCostFunction<
|
||||||
LandmarkCostFunction3D, 6 /* residuals */,
|
LandmarkCostFunction3D, 6 /* residuals */,
|
||||||
4 /* previous node rotation variables */,
|
4 /* previous node rotation variables */,
|
||||||
|
@ -77,8 +76,8 @@ class LandmarkCostFunction3D {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
LandmarkCostFunction3D(const LandmarkObservation& observation,
|
LandmarkCostFunction3D(const LandmarkObservation& observation,
|
||||||
const OptimizationProblem3D::NodeData& prev_node,
|
const NodeData3D& prev_node,
|
||||||
const OptimizationProblem3D::NodeData& next_node)
|
const NodeData3D& next_node)
|
||||||
: landmark_to_tracking_transform_(
|
: landmark_to_tracking_transform_(
|
||||||
observation.landmark_to_tracking_transform),
|
observation.landmark_to_tracking_transform),
|
||||||
translation_weight_(observation.translation_weight),
|
translation_weight_(observation.translation_weight),
|
||||||
|
|
|
@ -34,9 +34,9 @@ using LandmarkObservation =
|
||||||
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
PoseGraphInterface::LandmarkNode::LandmarkObservation;
|
||||||
|
|
||||||
TEST(LandmarkCostFunction3DTest, SmokeTest) {
|
TEST(LandmarkCostFunction3DTest, SmokeTest) {
|
||||||
OptimizationProblem3D::NodeData prev_node;
|
NodeData3D prev_node;
|
||||||
prev_node.time = common::FromUniversal(0);
|
prev_node.time = common::FromUniversal(0);
|
||||||
OptimizationProblem3D::NodeData next_node;
|
NodeData3D next_node;
|
||||||
next_node.time = common::FromUniversal(10);
|
next_node.time = common::FromUniversal(10);
|
||||||
|
|
||||||
std::unique_ptr<ceres::CostFunction> cost_function(
|
std::unique_ptr<ceres::CostFunction> cost_function(
|
||||||
|
|
|
@ -52,8 +52,8 @@ namespace {
|
||||||
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
|
||||||
using TrajectoryData =
|
using TrajectoryData =
|
||||||
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
|
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
|
||||||
using NodeData = OptimizationProblem3D::NodeData;
|
using NodeData = NodeData3D;
|
||||||
using SubmapData = OptimizationProblem3D::SubmapData;
|
using SubmapData = SubmapData3D;
|
||||||
|
|
||||||
// For odometry.
|
// For odometry.
|
||||||
std::unique_ptr<transform::Rigid3d> Interpolate(
|
std::unique_ptr<transform::Rigid3d> Interpolate(
|
||||||
|
@ -179,8 +179,8 @@ void AddLandmarkCostFunctions(
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
OptimizationProblem3D::OptimizationProblem3D(
|
OptimizationProblem3D::OptimizationProblem3D(
|
||||||
const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z)
|
const pose_graph::proto::OptimizationProblemOptions& options)
|
||||||
: options_(options), fix_z_(fix_z) {}
|
: options_(options) {}
|
||||||
|
|
||||||
OptimizationProblem3D::~OptimizationProblem3D() {}
|
OptimizationProblem3D::~OptimizationProblem3D() {}
|
||||||
|
|
||||||
|
@ -200,11 +200,9 @@ void OptimizationProblem3D::AddFixedFramePoseData(
|
||||||
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
|
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem3D::AddTrajectoryNode(
|
void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id,
|
||||||
const int trajectory_id, const common::Time time,
|
const NodeData& node_data) {
|
||||||
const transform::Rigid3d& local_pose,
|
node_data_.Append(trajectory_id, node_data);
|
||||||
const transform::Rigid3d& global_pose) {
|
|
||||||
node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
|
|
||||||
trajectory_data_[trajectory_id];
|
trajectory_data_[trajectory_id];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -213,11 +211,9 @@ void OptimizationProblem3D::SetTrajectoryData(
|
||||||
trajectory_data_[trajectory_id] = trajectory_data;
|
trajectory_data_[trajectory_id] = trajectory_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem3D::InsertTrajectoryNode(
|
void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id,
|
||||||
const NodeId& node_id, const common::Time time,
|
const NodeData& node_data) {
|
||||||
const transform::Rigid3d& local_pose,
|
node_data_.Insert(node_id, node_data);
|
||||||
const transform::Rigid3d& global_pose) {
|
|
||||||
node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
|
|
||||||
trajectory_data_[node_id.trajectory_id];
|
trajectory_data_[node_id.trajectory_id];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -265,7 +261,7 @@ void OptimizationProblem3D::Solve(
|
||||||
|
|
||||||
const auto translation_parameterization =
|
const auto translation_parameterization =
|
||||||
[this]() -> std::unique_ptr<ceres::LocalParameterization> {
|
[this]() -> std::unique_ptr<ceres::LocalParameterization> {
|
||||||
return fix_z_ == FixZ::kYes
|
return options_.fix_z_in_3d()
|
||||||
? common::make_unique<ceres::SubsetParameterization>(
|
? common::make_unique<ceres::SubsetParameterization>(
|
||||||
3, std::vector<int>{2})
|
3, std::vector<int>{2})
|
||||||
: nullptr;
|
: nullptr;
|
||||||
|
@ -342,7 +338,7 @@ void OptimizationProblem3D::Solve(
|
||||||
&C_nodes, &C_landmarks, &problem);
|
&C_nodes, &C_landmarks, &problem);
|
||||||
// Add constraints based on IMU observations of angular velocities and
|
// Add constraints based on IMU observations of angular velocities and
|
||||||
// linear acceleration.
|
// 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();) {
|
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);
|
||||||
|
@ -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
|
// Add penalties for violating odometry or changes between consecutive nodes
|
||||||
// if odometry is not available.
|
// if odometry is not available.
|
||||||
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
|
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(
|
transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
|
||||||
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 {
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/common/time.h"
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping/id.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/proto/optimization_problem_options.pb.h"
|
||||||
#include "cartographer/mapping/pose_graph_interface.h"
|
#include "cartographer/mapping/pose_graph_interface.h"
|
||||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||||
|
@ -40,80 +41,89 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
// Implements the SPA loop closure method.
|
struct NodeData3D {
|
||||||
class OptimizationProblem3D {
|
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:
|
public:
|
||||||
using Constraint = PoseGraphInterface::Constraint;
|
explicit OptimizationProblem3D(
|
||||||
using LandmarkNode = PoseGraphInterface::LandmarkNode;
|
const pose_graph::proto::OptimizationProblemOptions& options);
|
||||||
|
|
||||||
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);
|
|
||||||
~OptimizationProblem3D();
|
~OptimizationProblem3D();
|
||||||
|
|
||||||
OptimizationProblem3D(const OptimizationProblem3D&) = delete;
|
OptimizationProblem3D(const OptimizationProblem3D&) = delete;
|
||||||
OptimizationProblem3D& operator=(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,
|
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(
|
void AddFixedFramePoseData(
|
||||||
int trajectory_id,
|
int trajectory_id,
|
||||||
const sensor::FixedFramePoseData& fixed_frame_pose_data);
|
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(
|
void SetTrajectoryData(
|
||||||
int trajectory_id,
|
int trajectory_id,
|
||||||
const PoseGraphInterface::TrajectoryData& trajectory_data);
|
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 sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
|
||||||
const;
|
const {
|
||||||
|
return fixed_frame_pose_data_;
|
||||||
|
}
|
||||||
const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
|
const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
|
||||||
const;
|
const {
|
||||||
|
return trajectory_data_;
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Uses odometry if available, otherwise the local SLAM results.
|
// Uses odometry if available, otherwise the local SLAM results.
|
||||||
transform::Rigid3d ComputeRelativePose(
|
transform::Rigid3d ComputeRelativePose(
|
||||||
int trajectory_id, const NodeData& first_node_data,
|
int trajectory_id, const NodeData3D& first_node_data,
|
||||||
const NodeData& second_node_data) const;
|
const NodeData3D& second_node_data) const;
|
||||||
|
|
||||||
pose_graph::proto::OptimizationProblemOptions options_;
|
pose_graph::proto::OptimizationProblemOptions options_;
|
||||||
FixZ fix_z_;
|
MapById<NodeId, NodeData3D> node_data_;
|
||||||
MapById<NodeId, NodeData> node_data_;
|
MapById<SubmapId, SubmapData3D> submap_data_;
|
||||||
MapById<SubmapId, SubmapData> submap_data_;
|
|
||||||
std::map<std::string, transform::Rigid3d> landmark_data_;
|
std::map<std::string, transform::Rigid3d> landmark_data_;
|
||||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||||
|
|
|
@ -34,9 +34,7 @@ namespace {
|
||||||
class OptimizationProblem3DTest : public ::testing::Test {
|
class OptimizationProblem3DTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
OptimizationProblem3DTest()
|
OptimizationProblem3DTest()
|
||||||
: optimization_problem_(CreateOptions(),
|
: optimization_problem_(CreateOptions()), rng_(45387) {}
|
||||||
OptimizationProblem3D::FixZ::kNo),
|
|
||||||
rng_(45387) {}
|
|
||||||
|
|
||||||
pose_graph::proto::OptimizationProblemOptions CreateOptions() {
|
pose_graph::proto::OptimizationProblemOptions CreateOptions() {
|
||||||
auto parameter_dictionary = common::MakeDictionary(R"text(
|
auto parameter_dictionary = common::MakeDictionary(R"text(
|
||||||
|
@ -127,7 +125,8 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
|
||||||
optimization_problem_.AddImuData(
|
optimization_problem_.AddImuData(
|
||||||
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
|
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
|
||||||
Eigen::Vector3d::Zero()});
|
Eigen::Vector3d::Zero()});
|
||||||
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose);
|
optimization_problem_.AddTrajectoryNode(kTrajectoryId,
|
||||||
|
NodeData3D{now, pose, pose});
|
||||||
now += common::FromSeconds(0.01);
|
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, kSubmap0Transform);
|
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
|
||||||
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
|
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
|
||||||
const std::set<int> kFrozen;
|
const std::set<int> kFrozen = {};
|
||||||
optimization_problem_.Solve(constraints, kFrozen, {});
|
optimization_problem_.Solve(constraints, kFrozen, {});
|
||||||
|
|
||||||
double translation_error_after = 0.;
|
double translation_error_after = 0.;
|
||||||
|
|
|
@ -42,8 +42,7 @@ namespace mapping {
|
||||||
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
|
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
|
||||||
common::ThreadPool* thread_pool)
|
common::ThreadPool* thread_pool)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
optimization_problem_(options_.optimization_problem_options(),
|
optimization_problem_(options_.optimization_problem_options()),
|
||||||
pose_graph::OptimizationProblem3D::FixZ::kNo),
|
|
||||||
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
|
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
|
||||||
|
|
||||||
PoseGraph3D::~PoseGraph3D() {
|
PoseGraph3D::~PoseGraph3D() {
|
||||||
|
@ -264,7 +263,8 @@ void PoseGraph3D::ComputeConstraintsForNode(
|
||||||
optimization_problem_.submap_data().at(matching_id).global_pose *
|
optimization_problem_.submap_data().at(matching_id).global_pose *
|
||||||
insertion_submaps.front()->local_pose().inverse() * local_pose;
|
insertion_submaps.front()->local_pose().inverse() * local_pose;
|
||||||
optimization_problem_.AddTrajectoryNode(
|
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) {
|
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
||||||
const SubmapId submap_id = submap_ids[i];
|
const SubmapId submap_id = submap_ids[i];
|
||||||
// Even if this was the last node added to 'submap_id', the submap will only
|
// 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_.Insert(submap_id, SubmapData());
|
||||||
submap_data_.at(submap_id).submap = submap_ptr;
|
submap_data_.at(submap_id).submap = submap_ptr;
|
||||||
// Immediately show the submap at the 'global_submap_pose'.
|
// Immediately show the submap at the 'global_submap_pose'.
|
||||||
global_submap_poses_.Insert(
|
global_submap_poses_.Insert(submap_id,
|
||||||
submap_id,
|
pose_graph::SubmapData3D{global_submap_pose});
|
||||||
pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
|
|
||||||
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
|
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
|
||||||
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
||||||
optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
|
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_) {
|
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
|
||||||
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
|
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
optimization_problem_.InsertTrajectoryNode(
|
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(
|
transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
|
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
|
||||||
global_submap_poses,
|
|
||||||
const int trajectory_id) const {
|
const int trajectory_id) const {
|
||||||
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
||||||
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
|
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
|
||||||
|
|
|
@ -191,8 +191,7 @@ class PoseGraph3D : public PoseGraph {
|
||||||
// Computes the local to global map frame transform based on the given
|
// Computes the local to global map frame transform based on the given
|
||||||
// 'global_submap_poses'.
|
// 'global_submap_poses'.
|
||||||
transform::Rigid3d ComputeLocalToGlobalTransform(
|
transform::Rigid3d ComputeLocalToGlobalTransform(
|
||||||
const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
|
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
|
||||||
global_submap_poses,
|
|
||||||
int trajectory_id) const REQUIRES(mutex_);
|
int trajectory_id) const REQUIRES(mutex_);
|
||||||
|
|
||||||
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
|
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
|
||||||
|
@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph {
|
||||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||||
|
|
||||||
// Global submap poses currently used for displaying data.
|
// Global submap poses currently used for displaying data.
|
||||||
MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>
|
MapById<SubmapId, pose_graph::SubmapData3D> global_submap_poses_
|
||||||
global_submap_poses_ GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Global landmark poses with all observations.
|
// Global landmark poses with all observations.
|
||||||
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
|
||||||
|
|
|
@ -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_
|
|
@ -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: 13
|
// NEXT ID: 14
|
||||||
message OptimizationProblemOptions {
|
message OptimizationProblemOptions {
|
||||||
// Scaling parameter for Huber loss function.
|
// Scaling parameter for Huber loss function.
|
||||||
double huber_scale = 1;
|
double huber_scale = 1;
|
||||||
|
@ -41,6 +41,9 @@ message OptimizationProblemOptions {
|
||||||
// Scaling parameter for the FixedFramePose rotation.
|
// Scaling parameter for the FixedFramePose rotation.
|
||||||
double fixed_frame_pose_rotation_weight = 12;
|
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.
|
// If true, the Ceres solver summary will be logged for every optimization.
|
||||||
bool log_solver_summary = 5;
|
bool log_solver_summary = 5;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue