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

Mockable optimization (#994)

master
gaschler 2018-03-15 14:13:59 +01:00 committed by Wally B. Feed
parent bd7d7202bf
commit 55065a2108
7 changed files with 92 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -39,10 +39,12 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions& options, PoseGraph2D::PoseGraph2D(
common::ThreadPool* thread_pool) const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options()), optimization_problem_(std::move(optimization_problem)),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {} constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph2D::~PoseGraph2D() { PoseGraph2D::~PoseGraph2D() {
@ -55,7 +57,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_->submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one. // If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
@ -64,7 +66,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
trajectory_id, trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
} }
optimization_problem_.AddSubmap( optimization_problem_->AddSubmap(
trajectory_id, trajectory_id,
transform::Project2D(ComputeLocalToGlobalTransform( transform::Project2D(ComputeLocalToGlobalTransform(
global_submap_poses_, trajectory_id) * global_submap_poses_, trajectory_id) *
@ -83,7 +85,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
// In this case, 'last_submap_id' is the ID of // In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new. // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap( optimization_problem_->AddSubmap(
trajectory_id, trajectory_id,
first_submap_pose * first_submap_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() * pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
@ -153,7 +155,7 @@ void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, imu_data); optimization_problem_->AddImuData(trajectory_id, imu_data);
}); });
} }
@ -161,7 +163,7 @@ void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometryData(trajectory_id, odometry_data); optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}); });
} }
@ -204,10 +206,10 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
// the submap's trajectory, it suffices to do a match constrained to a // the submap's trajectory, it suffices to do a match constrained to a
// local search window. // local search window.
const transform::Rigid2d initial_relative_pose = const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data() optimization_problem_->submap_data()
.at(submap_id) .at(submap_id)
.global_pose.inverse() * .global_pose.inverse() *
optimization_problem_.node_data().at(node_id).pose; optimization_problem_->node_data().at(node_id).pose;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), trajectory_nodes_.at(node_id).constant_data.get(),
@ -221,7 +223,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) { for (const auto& node_id_data : optimization_problem_->node_data()) {
const NodeId& node_id = node_id_data.id; const NodeId& node_id = node_id_data.id;
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
@ -242,10 +244,10 @@ void PoseGraph2D::ComputeConstraintsForNode(
constant_data->local_pose * constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_id).global_pose * optimization_problem_->submap_data().at(matching_id).global_pose *
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, matching_id.trajectory_id,
pose_graph::NodeData2D{constant_data->time, pose, optimized_pose, pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment}); constant_data->gravity_alignment});
@ -445,7 +447,7 @@ void PoseGraph2D::AddSubmapFromProto(
pose_graph::SubmapData2D{global_submap_pose_2d}); pose_graph::SubmapData2D{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);
}); });
} }
@ -464,7 +466,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
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, node_id,
pose_graph::NodeData2D{ pose_graph::NodeData2D{
constant_data->time, constant_data->time,
@ -532,12 +534,12 @@ void PoseGraph2D::RunFinalOptimization() {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
DispatchOptimization(); DispatchOptimization();
}); });
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options() options_.optimization_problem_options()
.ceres_solver_options() .ceres_solver_options()
.max_num_iterations()); .max_num_iterations());
@ -547,7 +549,7 @@ void PoseGraph2D::RunFinalOptimization() {
} }
void PoseGraph2D::RunOptimization() { void PoseGraph2D::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_->submap_data().empty()) {
return; return;
} }
@ -555,12 +557,12 @@ void PoseGraph2D::RunOptimization() {
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking // time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing. // foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_, optimization_problem_->Solve(constraints_, frozen_trajectories_,
landmark_nodes_); landmark_nodes_);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_id)) { for (const auto& node : node_data.trajectory(trajectory_id)) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node.id); auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
@ -589,7 +591,7 @@ void PoseGraph2D::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose; old_global_to_new_global * mutable_trajectory_node.global_pose;
} }
} }
for (const auto& landmark : optimization_problem_.landmark_data()) { for (const auto& landmark : optimization_problem_->landmark_data()) {
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second; landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
} }
global_submap_poses_ = submap_data; global_submap_poses_ = submap_data;
@ -625,12 +627,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() {
sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data(); return optimization_problem_->odometry_data();
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
@ -775,7 +777,7 @@ PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
: parent_(parent) {} : parent_(parent) {}
int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const { int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_.submap_data(); const auto& submap_data = parent_->optimization_problem_->submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id); return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
} }
@ -831,13 +833,13 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(submap_id); parent_->submap_data_.Trim(submap_id);
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id);
// Remove the 'nodes_to_remove' from the pose graph and the optimization // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const NodeId& node_id : nodes_to_remove) { for (const NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->trajectory_nodes_.Trim(node_id);
parent_->optimization_problem_.TrimTrajectoryNode(node_id); parent_->optimization_problem_->TrimTrajectoryNode(node_id);
} }
} }

8
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -59,8 +59,10 @@ namespace mapping {
// All constraints are between a submap i and a node j. // All constraints are between a submap i and a node j.
class PoseGraph2D : public PoseGraph { class PoseGraph2D : public PoseGraph {
public: public:
PoseGraph2D(const proto::PoseGraphOptions& options, PoseGraph2D(
common::ThreadPool* thread_pool); const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph2D() override; ~PoseGraph2D() override;
PoseGraph2D(const PoseGraph2D&) = delete; PoseGraph2D(const PoseGraph2D&) = delete;
@ -230,7 +232,7 @@ class PoseGraph2D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_); void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
pose_graph::OptimizationProblem2D optimization_problem_; std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem_;
pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);

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

@ -132,8 +132,12 @@ class PoseGraph2DTest : public ::testing::Test {
log_residual_histograms = true, log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.0, global_constraint_search_after_n_seconds = 10.0,
})text"); })text");
auto options = CreatePoseGraphOptions(parameter_dictionary.get());
pose_graph_ = common::make_unique<PoseGraph2D>( pose_graph_ = common::make_unique<PoseGraph2D>(
CreatePoseGraphOptions(parameter_dictionary.get()), &thread_pool_); options,
common::make_unique<pose_graph::OptimizationProblem2D>(
options.optimization_problem_options()),
&thread_pool_);
} }
current_pose_ = transform::Rigid2d::Identity(); current_pose_ = transform::Rigid2d::Identity();

68
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -39,10 +39,12 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options, PoseGraph3D::PoseGraph3D(
common::ThreadPool* thread_pool) const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options()), optimization_problem_(std::move(optimization_problem)),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {} constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph3D::~PoseGraph3D() { PoseGraph3D::~PoseGraph3D() {
@ -55,7 +57,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_->submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one. // If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
@ -64,7 +66,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
trajectory_id, trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
} }
optimization_problem_.AddSubmap( optimization_problem_->AddSubmap(
trajectory_id, trajectory_id,
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) * ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *
insertion_submaps[0]->local_pose()); insertion_submaps[0]->local_pose());
@ -82,7 +84,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
// In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
// and 'insertions_submaps.back()' is new. // and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap( optimization_problem_->AddSubmap(
trajectory_id, first_submap_pose * trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose().inverse() * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose()); insertion_submaps[1]->local_pose());
@ -151,7 +153,7 @@ void PoseGraph3D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, imu_data); optimization_problem_->AddImuData(trajectory_id, imu_data);
}); });
} }
@ -159,7 +161,7 @@ void PoseGraph3D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometryData(trajectory_id, odometry_data); optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}); });
} }
@ -168,8 +170,8 @@ void PoseGraph3D::AddFixedFramePoseData(
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddFixedFramePoseData(trajectory_id, optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data); fixed_frame_pose_data);
}); });
} }
@ -193,10 +195,10 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const transform::Rigid3d global_node_pose = const transform::Rigid3d global_node_pose =
optimization_problem_.node_data().at(node_id).global_pose; optimization_problem_->node_data().at(node_id).global_pose;
const transform::Rigid3d global_submap_pose = const transform::Rigid3d global_submap_pose =
optimization_problem_.submap_data().at(submap_id).global_pose; optimization_problem_->submap_data().at(submap_id).global_pose;
const transform::Rigid3d global_submap_pose_inverse = const transform::Rigid3d global_submap_pose_inverse =
global_submap_pose.inverse(); global_submap_pose.inverse();
@ -241,7 +243,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) { for (const auto& node_id_data : optimization_problem_->node_data()) {
const NodeId& node_id = node_id_data.id; const NodeId& node_id = node_id_data.id;
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
@ -260,9 +262,9 @@ void PoseGraph3D::ComputeConstraintsForNode(
const SubmapId matching_id = submap_ids.front(); const SubmapId matching_id = submap_ids.front();
const transform::Rigid3d& local_pose = constant_data->local_pose; const transform::Rigid3d& local_pose = constant_data->local_pose;
const transform::Rigid3d global_pose = const transform::Rigid3d global_pose =
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, matching_id.trajectory_id,
pose_graph::NodeData3D{constant_data->time, local_pose, global_pose}); 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) {
@ -460,7 +462,7 @@ void PoseGraph3D::AddSubmapFromProto(
pose_graph::SubmapData3D{global_submap_pose}); pose_graph::SubmapData3D{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);
}); });
} }
@ -477,7 +479,7 @@ 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, node_id,
pose_graph::NodeData3D{constant_data->time, constant_data->local_pose, pose_graph::NodeData3D{constant_data->time, constant_data->local_pose,
global_pose}); global_pose});
@ -499,7 +501,7 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
const int trajectory_id = data.trajectory_id(); const int trajectory_id = data.trajectory_id();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id, trajectory_data]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id, trajectory_data]() REQUIRES(mutex_) {
optimization_problem_.SetTrajectoryData(trajectory_id, trajectory_data); optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
}); });
} }
@ -548,12 +550,12 @@ void PoseGraph3D::RunFinalOptimization() {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
DispatchOptimization(); DispatchOptimization();
}); });
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options() options_.optimization_problem_options()
.ceres_solver_options() .ceres_solver_options()
.max_num_iterations()); .max_num_iterations());
@ -589,7 +591,7 @@ void PoseGraph3D::LogResidualHistograms() {
} }
void PoseGraph3D::RunOptimization() { void PoseGraph3D::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_->submap_data().empty()) {
return; return;
} }
@ -597,12 +599,12 @@ void PoseGraph3D::RunOptimization() {
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking // time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing. // foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_, optimization_problem_->Solve(constraints_, frozen_trajectories_,
landmark_nodes_); landmark_nodes_);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_id)) { for (const auto& node : node_data.trajectory(trajectory_id)) {
trajectory_nodes_.at(node.id).global_pose = node.data.global_pose; trajectory_nodes_.at(node.id).global_pose = node.data.global_pose;
@ -627,7 +629,7 @@ void PoseGraph3D::RunOptimization() {
old_global_to_new_global * mutable_trajectory_node.global_pose; old_global_to_new_global * mutable_trajectory_node.global_pose;
} }
} }
for (const auto& landmark : optimization_problem_.landmark_data()) { for (const auto& landmark : optimization_problem_->landmark_data()) {
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second; landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
} }
global_submap_poses_ = submap_data; global_submap_poses_ = submap_data;
@ -668,24 +670,24 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() {
sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data(); return optimization_problem_->odometry_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph3D::GetFixedFramePoseData() { PoseGraph3D::GetFixedFramePoseData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.fixed_frame_pose_data(); return optimization_problem_->fixed_frame_pose_data();
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() { PoseGraph3D::GetTrajectoryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.trajectory_data(); return optimization_problem_->trajectory_data();
} }
std::vector<PoseGraph::Constraint> PoseGraph3D::constraints() { std::vector<PoseGraph::Constraint> PoseGraph3D::constraints() {
@ -806,7 +808,7 @@ PoseGraph3D::TrimmingHandle::TrimmingHandle(PoseGraph3D* const parent)
: parent_(parent) {} : parent_(parent) {}
int PoseGraph3D::TrimmingHandle::num_submaps(const int trajectory_id) const { int PoseGraph3D::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_.submap_data(); const auto& submap_data = parent_->optimization_problem_->submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id); return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
} }
@ -862,13 +864,13 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(submap_id); parent_->submap_data_.Trim(submap_id);
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id);
// Remove the 'nodes_to_remove' from the pose graph and the optimization // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const NodeId& node_id : nodes_to_remove) { for (const NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->trajectory_nodes_.Trim(node_id);
parent_->optimization_problem_.TrimTrajectoryNode(node_id); parent_->optimization_problem_->TrimTrajectoryNode(node_id);
} }
} }

8
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -58,8 +58,10 @@ namespace mapping {
// All constraints are between a submap i and a node j. // All constraints are between a submap i and a node j.
class PoseGraph3D : public PoseGraph { class PoseGraph3D : public PoseGraph {
public: public:
PoseGraph3D(const proto::PoseGraphOptions& options, PoseGraph3D(
common::ThreadPool* thread_pool); const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph3D() override; ~PoseGraph3D() override;
PoseGraph3D(const PoseGraph3D&) = delete; PoseGraph3D(const PoseGraph3D&) = delete;
@ -235,7 +237,7 @@ class PoseGraph3D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_); void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
pose_graph::OptimizationProblem3D optimization_problem_; std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem_;
pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);

5
cartographer/mapping/internal/3d/pose_graph_3d_test.cc
View File

@ -32,7 +32,10 @@ class PoseGraph3DForTesting : public PoseGraph3D {
public: public:
PoseGraph3DForTesting(const proto::PoseGraphOptions& options, PoseGraph3DForTesting(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: PoseGraph3D(options, thread_pool) {} : PoseGraph3D(options,
common::make_unique<pose_graph::OptimizationProblem3D>(
options.optimization_problem_options()),
thread_pool) {}
void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); } void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
}; };

10
cartographer/mapping/map_builder.cc
View File

@ -53,11 +53,17 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) { : options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
pose_graph_ = common::make_unique<PoseGraph2D>( pose_graph_ = common::make_unique<PoseGraph2D>(
options_.pose_graph_options(), &thread_pool_); options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
} }
if (options.use_trajectory_builder_3d()) { if (options.use_trajectory_builder_3d()) {
pose_graph_ = common::make_unique<PoseGraph3D>( pose_graph_ = common::make_unique<PoseGraph3D>(
options_.pose_graph_options(), &thread_pool_); options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
} }
if (options.collate_by_trajectory()) { if (options.collate_by_trajectory()) {
sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>(); sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>();