Introduce a GlobalSlamResultCallback (#1143)
parent
c2f54b8df2
commit
ce67d684ba
|
@ -109,7 +109,9 @@ class ClientServerTest : public ::testing::Test {
|
|||
|
||||
void InitializeRealServer() {
|
||||
auto map_builder = common::make_unique<MapBuilder>(
|
||||
map_builder_server_options_.map_builder_options());
|
||||
map_builder_server_options_.map_builder_options(),
|
||||
nullptr /* global_slam_optimization_callback */
|
||||
);
|
||||
server_ = common::make_unique<MapBuilderServer>(map_builder_server_options_,
|
||||
std::move(map_builder));
|
||||
EXPECT_TRUE(server_ != nullptr);
|
||||
|
@ -117,7 +119,8 @@ class ClientServerTest : public ::testing::Test {
|
|||
|
||||
void InitializeRealUploadingServer() {
|
||||
auto map_builder = common::make_unique<MapBuilder>(
|
||||
uploading_map_builder_server_options_.map_builder_options());
|
||||
uploading_map_builder_server_options_.map_builder_options(),
|
||||
nullptr /* global_slam_optimization_callback */);
|
||||
uploading_server_ = common::make_unique<MapBuilderServer>(
|
||||
uploading_map_builder_server_options_, std::move(map_builder));
|
||||
EXPECT_TRUE(uploading_server_ != nullptr);
|
||||
|
|
|
@ -55,7 +55,8 @@ void Run(const std::string& configuration_directory,
|
|||
map_builder_server_options.mutable_map_builder_options()
|
||||
->set_collate_by_trajectory(true);
|
||||
auto map_builder = common::make_unique<mapping::MapBuilder>(
|
||||
map_builder_server_options.map_builder_options());
|
||||
map_builder_server_options.map_builder_options(),
|
||||
nullptr /* global_slam_optimization_callback */);
|
||||
std::unique_ptr<MapBuilderServerInterface> map_builder_server =
|
||||
CreateMapBuilderServer(map_builder_server_options,
|
||||
std::move(map_builder));
|
||||
|
|
|
@ -41,9 +41,11 @@ namespace mapping {
|
|||
|
||||
PoseGraph2D::PoseGraph2D(
|
||||
const proto::PoseGraphOptions& options,
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback,
|
||||
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
|
||||
common::ThreadPool* thread_pool)
|
||||
: options_(options),
|
||||
global_slam_optimization_callback_(global_slam_optimization_callback),
|
||||
optimization_problem_(std::move(optimization_problem)),
|
||||
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
|
||||
|
||||
|
@ -301,7 +303,8 @@ void PoseGraph2D::DispatchOptimization() {
|
|||
// If there is a 'work_queue_' already, some other thread will take care.
|
||||
if (work_queue_ == nullptr) {
|
||||
work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
|
||||
HandleWorkQueue();
|
||||
constraint_builder_.WhenDone(
|
||||
std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
|
||||
}
|
||||
}
|
||||
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
|
||||
|
@ -326,15 +329,33 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
|
|||
time);
|
||||
}
|
||||
|
||||
void PoseGraph2D::HandleWorkQueue() {
|
||||
constraint_builder_.WhenDone(
|
||||
[this](const constraints::ConstraintBuilder2D::Result& result) {
|
||||
void PoseGraph2D::HandleWorkQueue(
|
||||
const constraints::ConstraintBuilder2D::Result& result) {
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
}
|
||||
RunOptimization();
|
||||
|
||||
if (global_slam_optimization_callback_) {
|
||||
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
|
||||
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
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()) {
|
||||
trajectory_id_to_last_optimized_node_id[trajectory_id] =
|
||||
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
|
||||
trajectory_id_to_last_optimized_submap_id[trajectory_id] =
|
||||
std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
|
||||
}
|
||||
}
|
||||
global_slam_optimization_callback_(
|
||||
trajectory_id_to_last_optimized_submap_id,
|
||||
trajectory_id_to_last_optimized_node_id);
|
||||
}
|
||||
|
||||
common::MutexLocker locker(&mutex_);
|
||||
for (const Constraint& constraint : result) {
|
||||
UpdateTrajectoryConnectivity(constraint);
|
||||
|
@ -362,8 +383,8 @@ void PoseGraph2D::HandleWorkQueue() {
|
|||
}
|
||||
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
|
||||
// We have to optimize again.
|
||||
HandleWorkQueue();
|
||||
});
|
||||
constraint_builder_.WhenDone(
|
||||
std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void PoseGraph2D::WaitForAllComputations() {
|
||||
|
|
|
@ -61,6 +61,7 @@ class PoseGraph2D : public PoseGraph {
|
|||
public:
|
||||
PoseGraph2D(
|
||||
const proto::PoseGraphOptions& options,
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback,
|
||||
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
|
||||
common::ThreadPool* thread_pool);
|
||||
~PoseGraph2D() override;
|
||||
|
@ -186,9 +187,9 @@ class PoseGraph2D : public PoseGraph {
|
|||
void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
|
||||
REQUIRES(mutex_);
|
||||
|
||||
// Registers the callback to run the optimization once all constraints have
|
||||
// been computed, that will also do all work that queue up in 'work_queue_'.
|
||||
void HandleWorkQueue() REQUIRES(mutex_);
|
||||
// Runs the optimization, executes the trimmers and processes the work queue.
|
||||
void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
|
||||
REQUIRES(mutex_);
|
||||
|
||||
// Waits until we caught up (i.e. nothing is waiting to be scheduled), and
|
||||
// all computations have finished.
|
||||
|
@ -215,6 +216,7 @@ class PoseGraph2D : public PoseGraph {
|
|||
REQUIRES(mutex_);
|
||||
|
||||
const proto::PoseGraphOptions options_;
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback_;
|
||||
common::Mutex mutex_;
|
||||
|
||||
// If it exists, further work items must be added to this queue, and will be
|
||||
|
|
|
@ -142,7 +142,7 @@ class PoseGraph2DTest : public ::testing::Test {
|
|||
})text");
|
||||
auto options = CreatePoseGraphOptions(parameter_dictionary.get());
|
||||
pose_graph_ = common::make_unique<PoseGraph2D>(
|
||||
options,
|
||||
options, nullptr /* global_slam_optimization_callback */,
|
||||
common::make_unique<optimization::OptimizationProblem2D>(
|
||||
options.optimization_problem_options()),
|
||||
&thread_pool_);
|
||||
|
|
|
@ -41,9 +41,11 @@ namespace mapping {
|
|||
|
||||
PoseGraph3D::PoseGraph3D(
|
||||
const proto::PoseGraphOptions& options,
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback,
|
||||
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
|
||||
common::ThreadPool* thread_pool)
|
||||
: options_(options),
|
||||
global_slam_optimization_callback_(global_slam_optimization_callback),
|
||||
optimization_problem_(std::move(optimization_problem)),
|
||||
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
|
||||
|
||||
|
@ -315,7 +317,8 @@ void PoseGraph3D::DispatchOptimization() {
|
|||
// If there is a 'work_queue_' already, some other thread will take care.
|
||||
if (work_queue_ == nullptr) {
|
||||
work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
|
||||
HandleWorkQueue();
|
||||
constraint_builder_.WhenDone(
|
||||
std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -341,15 +344,33 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
|
|||
time);
|
||||
}
|
||||
|
||||
void PoseGraph3D::HandleWorkQueue() {
|
||||
constraint_builder_.WhenDone(
|
||||
[this](const constraints::ConstraintBuilder3D::Result& result) {
|
||||
void PoseGraph3D::HandleWorkQueue(
|
||||
const constraints::ConstraintBuilder3D::Result& result) {
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
}
|
||||
RunOptimization();
|
||||
|
||||
if (global_slam_optimization_callback_) {
|
||||
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
|
||||
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
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()) {
|
||||
trajectory_id_to_last_optimized_node_id[trajectory_id] =
|
||||
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
|
||||
trajectory_id_to_last_optimized_submap_id[trajectory_id] =
|
||||
std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
|
||||
}
|
||||
}
|
||||
global_slam_optimization_callback_(
|
||||
trajectory_id_to_last_optimized_submap_id,
|
||||
trajectory_id_to_last_optimized_node_id);
|
||||
}
|
||||
|
||||
common::MutexLocker locker(&mutex_);
|
||||
for (const Constraint& constraint : result) {
|
||||
UpdateTrajectoryConnectivity(constraint);
|
||||
|
@ -377,8 +398,8 @@ void PoseGraph3D::HandleWorkQueue() {
|
|||
}
|
||||
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
|
||||
// We have to optimize again.
|
||||
HandleWorkQueue();
|
||||
});
|
||||
constraint_builder_.WhenDone(
|
||||
std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void PoseGraph3D::WaitForAllComputations() {
|
||||
|
|
|
@ -60,6 +60,7 @@ class PoseGraph3D : public PoseGraph {
|
|||
public:
|
||||
PoseGraph3D(
|
||||
const proto::PoseGraphOptions& options,
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback,
|
||||
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
|
||||
common::ThreadPool* thread_pool);
|
||||
~PoseGraph3D() override;
|
||||
|
@ -189,9 +190,9 @@ class PoseGraph3D : public PoseGraph {
|
|||
void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
|
||||
REQUIRES(mutex_);
|
||||
|
||||
// Registers the callback to run the optimization once all constraints have
|
||||
// been computed, that will also do all work that queue up in 'work_queue_'.
|
||||
void HandleWorkQueue() REQUIRES(mutex_);
|
||||
// Runs the optimization, executes the trimmers and processes the work queue.
|
||||
void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result)
|
||||
REQUIRES(mutex_);
|
||||
|
||||
// Runs the optimization. Callers have to make sure, that there is only one
|
||||
// optimization being run at a time.
|
||||
|
@ -219,6 +220,7 @@ class PoseGraph3D : public PoseGraph {
|
|||
REQUIRES(mutex_);
|
||||
|
||||
const proto::PoseGraphOptions options_;
|
||||
GlobalSlamOptimizationCallback global_slam_optimization_callback_;
|
||||
common::Mutex mutex_;
|
||||
|
||||
// If it exists, further work items must be added to this queue, and will be
|
||||
|
|
|
@ -48,7 +48,8 @@ class PoseGraph3DForTesting : public PoseGraph3D {
|
|||
const proto::PoseGraphOptions& options,
|
||||
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
|
||||
common::ThreadPool* thread_pool)
|
||||
: PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
|
||||
: PoseGraph3D(options, nullptr /* global_slam_optimization_callback */,
|
||||
std::move(optimization_problem), thread_pool) {}
|
||||
|
||||
void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
|
||||
};
|
||||
|
|
|
@ -61,20 +61,22 @@ std::vector<std::string> SelectRangeSensorIds(
|
|||
return range_sensor_ids;
|
||||
}
|
||||
|
||||
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
|
||||
MapBuilder::MapBuilder(
|
||||
const proto::MapBuilderOptions& options,
|
||||
PoseGraph::GlobalSlamOptimizationCallback global_slam_optimization_callback)
|
||||
: options_(options), thread_pool_(options.num_background_threads()) {
|
||||
CHECK(options.use_trajectory_builder_2d() ^
|
||||
options.use_trajectory_builder_3d());
|
||||
if (options.use_trajectory_builder_2d()) {
|
||||
pose_graph_ = common::make_unique<PoseGraph2D>(
|
||||
options_.pose_graph_options(),
|
||||
options_.pose_graph_options(), global_slam_optimization_callback,
|
||||
common::make_unique<optimization::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(),
|
||||
options_.pose_graph_options(), global_slam_optimization_callback,
|
||||
common::make_unique<optimization::OptimizationProblem3D>(
|
||||
options_.pose_graph_options().optimization_problem_options()),
|
||||
&thread_pool_);
|
||||
|
|
|
@ -36,7 +36,9 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
|
|||
// and a PoseGraph for loop closure.
|
||||
class MapBuilder : public MapBuilderInterface {
|
||||
public:
|
||||
explicit MapBuilder(const proto::MapBuilderOptions& options);
|
||||
MapBuilder(const proto::MapBuilderOptions& options,
|
||||
PoseGraph::GlobalSlamOptimizationCallback
|
||||
global_slam_optimization_callback);
|
||||
~MapBuilder() override {}
|
||||
|
||||
MapBuilder(const MapBuilder&) = delete;
|
||||
|
|
|
@ -58,7 +58,8 @@ class MapBuilderTest : public ::testing::Test {
|
|||
}
|
||||
|
||||
void BuildMapBuilder() {
|
||||
map_builder_ = common::make_unique<MapBuilder>(map_builder_options_);
|
||||
map_builder_ = common::make_unique<MapBuilder>(
|
||||
map_builder_options_, nullptr /* global_slam_optimization_callback */);
|
||||
}
|
||||
|
||||
void SetOptionsTo3D() {
|
||||
|
|
|
@ -52,6 +52,10 @@ class PoseGraph : public PoseGraphInterface {
|
|||
common::Time time;
|
||||
};
|
||||
|
||||
using GlobalSlamOptimizationCallback =
|
||||
std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
|
||||
const std::map<int /* trajectory_id */, NodeId>&)>;
|
||||
|
||||
PoseGraph() {}
|
||||
~PoseGraph() override {}
|
||||
|
||||
|
|
Loading…
Reference in New Issue