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