From 55065a2108c882934963cf71cefe119c0628e0e8 Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 15 Mar 2018 14:13:59 +0100 Subject: [PATCH] Mockable optimization (#994) --- .../mapping/internal/2d/pose_graph_2d.cc | 58 ++++++++-------- .../mapping/internal/2d/pose_graph_2d.h | 8 ++- .../mapping/internal/2d/pose_graph_2d_test.cc | 6 +- .../mapping/internal/3d/pose_graph_3d.cc | 68 ++++++++++--------- .../mapping/internal/3d/pose_graph_3d.h | 8 ++- .../mapping/internal/3d/pose_graph_3d_test.cc | 5 +- cartographer/mapping/map_builder.cc | 10 ++- 7 files changed, 92 insertions(+), 71 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index e413436..994f89c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -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 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 PoseGraph2D::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, const std::vector>& 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 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 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 PoseGraph2D::GetLandmarkPoses() { sensor::MapByTime PoseGraph2D::GetImuData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.imu_data(); + return optimization_problem_->imu_data(); } sensor::MapByTime PoseGraph2D::GetOdometryData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.odometry_data(); + return optimization_problem_->odometry_data(); } std::map @@ -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); } } diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 942a13f..c1e3138 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -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 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 optimization_problem_; pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index cdf5c33..8b0a973 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -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( - CreatePoseGraphOptions(parameter_dictionary.get()), &thread_pool_); + options, + common::make_unique( + options.optimization_problem_options()), + &thread_pool_); } current_pose_ = transform::Rigid2d::Identity(); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 4c51b94..d1db849 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -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 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 PoseGraph3D::InitializeGlobalSubmapPoses( const int trajectory_id, const common::Time time, const std::vector>& 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 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 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 PoseGraph3D::GetLandmarkPoses() { sensor::MapByTime PoseGraph3D::GetImuData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.imu_data(); + return optimization_problem_->imu_data(); } sensor::MapByTime PoseGraph3D::GetOdometryData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.odometry_data(); + return optimization_problem_->odometry_data(); } sensor::MapByTime PoseGraph3D::GetFixedFramePoseData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.fixed_frame_pose_data(); + return optimization_problem_->fixed_frame_pose_data(); } std::map PoseGraph3D::GetTrajectoryData() { common::MutexLocker locker(&mutex_); - return optimization_problem_.trajectory_data(); + return optimization_problem_->trajectory_data(); } std::vector 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); } } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index c9986e4..5eaea65 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -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 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 optimization_problem_; pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index b529f1e..560d9a3 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -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( + options.optimization_problem_options()), + thread_pool) {} void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); } }; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 2208510..0f5d52e 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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( - options_.pose_graph_options(), &thread_pool_); + options_.pose_graph_options(), + common::make_unique( + options_.pose_graph_options().optimization_problem_options()), + &thread_pool_); } if (options.use_trajectory_builder_3d()) { pose_graph_ = common::make_unique( - options_.pose_graph_options(), &thread_pool_); + options_.pose_graph_options(), + common::make_unique( + options_.pose_graph_options().optimization_problem_options()), + &thread_pool_); } if (options.collate_by_trajectory()) { sensor_collator_ = common::make_unique();