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 mapping {
PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool)
PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options()),
optimization_problem_(std::move(optimization_problem)),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph2D::~PoseGraph2D() {
@ -55,7 +57,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
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 we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
@ -64,7 +66,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
optimization_problem_->AddSubmap(
trajectory_id,
transform::Project2D(ComputeLocalToGlobalTransform(
global_submap_poses_, trajectory_id) *
@ -83,7 +85,7 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap(
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
@ -153,7 +155,7 @@ void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&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) {
common::MutexLocker locker(&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
// local search window.
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_.node_data().at(node_id).pose;
optimization_problem_->node_data().at(node_id).pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
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) {
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;
if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
@ -242,10 +244,10 @@ void PoseGraph2D::ComputeConstraintsForNode(
constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
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;
optimization_problem_.AddTrajectoryNode(
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment});
@ -445,7 +447,7 @@ void PoseGraph2D::AddSubmapFromProto(
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);
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 gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse());
optimization_problem_.InsertTrajectoryNode(
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeData2D{
constant_data->time,
@ -532,12 +534,12 @@ void PoseGraph2D::RunFinalOptimization() {
{
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations(
optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations());
DispatchOptimization();
});
AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations(
optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options()
.ceres_solver_options()
.max_num_iterations());
@ -547,7 +549,7 @@ void PoseGraph2D::RunFinalOptimization() {
}
void PoseGraph2D::RunOptimization() {
if (optimization_problem_.submap_data().empty()) {
if (optimization_problem_->submap_data().empty()) {
return;
}
@ -555,12 +557,12 @@ void PoseGraph2D::RunOptimization() {
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
optimization_problem_->Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data();
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_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;
}
}
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;
}
global_submap_poses_ = submap_data;
@ -625,12 +627,12 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() {
sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
return optimization_problem_->imu_data();
}
sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data();
return optimization_problem_->odometry_data();
}
std::map<int, PoseGraphInterface::TrajectoryData>
@ -775,7 +777,7 @@ PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
: parent_(parent) {}
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);
}
@ -831,13 +833,13 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(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
// problem.
for (const NodeId& node_id : nodes_to_remove) {
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.
class PoseGraph2D : public PoseGraph {
public:
PoseGraph2D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool);
PoseGraph2D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph2D() override;
PoseGraph2D(const PoseGraph2D&) = delete;
@ -230,7 +232,7 @@ class PoseGraph2D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem.
pose_graph::OptimizationProblem2D optimization_problem_;
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem_;
pose_graph::ConstraintBuilder2D constraint_builder_ 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,
global_constraint_search_after_n_seconds = 10.0,
})text");
auto options = CreatePoseGraphOptions(parameter_dictionary.get());
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();

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

@ -39,10 +39,12 @@
namespace cartographer {
namespace mapping {
PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool)
PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options()),
optimization_problem_(std::move(optimization_problem)),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph3D::~PoseGraph3D() {
@ -55,7 +57,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) {
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 we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
@ -64,7 +66,7 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
optimization_problem_->AddSubmap(
trajectory_id,
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *
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()'
// and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_.AddSubmap(
optimization_problem_->AddSubmap(
trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
@ -151,7 +153,7 @@ void PoseGraph3D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&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) {
common::MutexLocker locker(&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) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
});
}
@ -193,10 +195,10 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
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 =
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 =
global_submap_pose.inverse();
@ -241,7 +243,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& 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;
if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
@ -260,9 +262,9 @@ void PoseGraph3D::ComputeConstraintsForNode(
const SubmapId matching_id = submap_ids.front();
const transform::Rigid3d& local_pose = constant_data->local_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;
optimization_problem_.AddTrajectoryNode(
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
pose_graph::NodeData3D{constant_data->time, local_pose, global_pose});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
@ -460,7 +462,7 @@ void PoseGraph3D::AddSubmapFromProto(
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);
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_) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.InsertTrajectoryNode(
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeData3D{constant_data->time, constant_data->local_pose,
global_pose});
@ -499,7 +501,7 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
const int trajectory_id = data.trajectory_id();
common::MutexLocker locker(&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_);
AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations(
optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations());
DispatchOptimization();
});
AddWorkItem([this]() REQUIRES(mutex_) {
optimization_problem_.SetMaxNumIterations(
optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options()
.ceres_solver_options()
.max_num_iterations());
@ -589,7 +591,7 @@ void PoseGraph3D::LogResidualHistograms() {
}
void PoseGraph3D::RunOptimization() {
if (optimization_problem_.submap_data().empty()) {
if (optimization_problem_->submap_data().empty()) {
return;
}
@ -597,12 +599,12 @@ void PoseGraph3D::RunOptimization() {
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking
// foreground processing.
optimization_problem_.Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
optimization_problem_->Solve(constraints_, frozen_trajectories_,
landmark_nodes_);
common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data();
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_id)) {
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;
}
}
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;
}
global_submap_poses_ = submap_data;
@ -668,24 +670,24 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() {
sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
return optimization_problem_->imu_data();
}
sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data();
return optimization_problem_->odometry_data();
}
sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph3D::GetFixedFramePoseData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.fixed_frame_pose_data();
return optimization_problem_->fixed_frame_pose_data();
}
std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.trajectory_data();
return optimization_problem_->trajectory_data();
}
std::vector<PoseGraph::Constraint> PoseGraph3D::constraints() {
@ -806,7 +808,7 @@ PoseGraph3D::TrimmingHandle::TrimmingHandle(PoseGraph3D* const parent)
: parent_(parent) {}
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);
}
@ -862,13 +864,13 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(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
// problem.
for (const NodeId& node_id : nodes_to_remove) {
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.
class PoseGraph3D : public PoseGraph {
public:
PoseGraph3D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool);
PoseGraph3D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph3D() override;
PoseGraph3D(const PoseGraph3D&) = delete;
@ -235,7 +237,7 @@ class PoseGraph3D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem.
pose_graph::OptimizationProblem3D optimization_problem_;
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem_;
pose_graph::ConstraintBuilder3D constraint_builder_ 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:
PoseGraph3DForTesting(const proto::PoseGraphOptions& options,
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(); }
};

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()) {
if (options.use_trajectory_builder_2d()) {
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()) {
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()) {
sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>();