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

Introduce a GlobalSlamResultCallback (#1143)

master
Christoph Schütte 2018-05-10 11:25:00 +02:00 committed by GitHub
parent c2f54b8df2
commit ce67d684ba
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 152 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/cloud/internal/client_server_test.cc
View File

@ -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);

3
cartographer/cloud/map_builder_server_main.cc
View File

@ -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));

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

@ -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,44 +329,62 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
time);
}
void PoseGraph2D::HandleWorkQueue() {
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);
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false;
while (!run_loop_closure_) {
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
work_queue_->front()();
work_queue_->pop_front();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
// We have to optimize again.
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false;
while (!run_loop_closure_) {
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
work_queue_->front()();
work_queue_->pop_front();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
// We have to optimize again.
HandleWorkQueue();
});
std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
}
void PoseGraph2D::WaitForAllComputations() {

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

@ -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

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

@ -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_);

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

@ -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,44 +344,62 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
time);
}
void PoseGraph3D::HandleWorkQueue() {
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);
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false;
while (!run_loop_closure_) {
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
work_queue_->front()();
work_queue_->pop_front();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
// We have to optimize again.
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder3D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
}
RunOptimization();
common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false;
while (!run_loop_closure_) {
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
work_queue_->front()();
work_queue_->pop_front();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
// We have to optimize again.
HandleWorkQueue();
});
std::bind(&PoseGraph3D::HandleWorkQueue, this, std::placeholders::_1));
}
void PoseGraph3D::WaitForAllComputations() {

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

@ -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

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

@ -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(); }
};

8
cartographer/mapping/map_builder.cc
View File

@ -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_);

4
cartographer/mapping/map_builder.h
View File

@ -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;

3
cartographer/mapping/map_builder_test.cc
View File

@ -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() {

4
cartographer/mapping/pose_graph.h
View File

@ -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 {}