Add SetGlobalSlamOptimizationCallback() (#1164)

to allow setting the GlobalSlamOptimizationCallback after MapBuilder and PoseGraph
creation. Also removes the GlobalSlamOptimizationCallback from the Constructor
since it is not used.

Prerequisite for implementing ReceiveGlobalSlamOptimizations() in gRPC MapBuilderInterface.
master
Christoph Schütte 2018-05-25 12:29:52 +02:00 committed by Wally B. Feed
parent 207979f209
commit ce18ec7295
17 changed files with 64 additions and 50 deletions

View File

@ -158,5 +158,10 @@ mapping::proto::PoseGraph PoseGraphStub::ToProto() const {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
void PoseGraphStub::SetGlobalSlamOptimizationCallback(
GlobalSlamOptimizationCallback callback) {
LOG(FATAL) << "Not implemented";
}
} // namespace cloud } // namespace cloud
} // namespace cartographer } // namespace cartographer

View File

@ -50,6 +50,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
const override; const override;
std::vector<Constraint> constraints() const override; std::vector<Constraint> constraints() const override;
mapping::proto::PoseGraph ToProto() const override; mapping::proto::PoseGraph ToProto() const override;
void SetGlobalSlamOptimizationCallback(
GlobalSlamOptimizationCallback callback) override;
private: private:
std::shared_ptr<::grpc::Channel> client_channel_; std::shared_ptr<::grpc::Channel> client_channel_;

View File

@ -109,9 +109,7 @@ 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);
@ -119,8 +117,7 @@ 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);

View File

@ -55,8 +55,7 @@ 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));

View File

@ -41,11 +41,9 @@ 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) {}
@ -928,5 +926,10 @@ PoseGraph2D::GetSubmapDataUnderLock() const {
return submaps; return submaps;
} }
void PoseGraph2D::SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) {
global_slam_optimization_callback_ = callback;
}
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -61,7 +61,6 @@ 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;
@ -141,6 +140,8 @@ class PoseGraph2D : public PoseGraph {
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_); int trajectory_id, const common::Time time) const REQUIRES(mutex_);

View File

@ -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, nullptr /* global_slam_optimization_callback */, options,
common::make_unique<optimization::OptimizationProblem2D>( common::make_unique<optimization::OptimizationProblem2D>(
options.optimization_problem_options()), options.optimization_problem_options()),
&thread_pool_); &thread_pool_);

View File

@ -41,11 +41,9 @@ 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) {}
@ -956,5 +954,10 @@ PoseGraph3D::GetSubmapDataUnderLock() const {
return submaps; return submaps;
} }
void PoseGraph3D::SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) {
global_slam_optimization_callback_ = callback;
}
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -60,7 +60,6 @@ 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;
@ -140,6 +139,8 @@ class PoseGraph3D : public PoseGraph {
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetGlobalSlamOptimizationCallback(
PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_); int trajectory_id, const common::Time time) const REQUIRES(mutex_);

View File

@ -38,18 +38,18 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
: OptimizationProblem3D(OptimizationProblemOptions{}) {} : OptimizationProblem3D(OptimizationProblemOptions{}) {}
~MockOptimizationProblem3D() override = default; ~MockOptimizationProblem3D() override = default;
MOCK_METHOD3(Solve, void(const std::vector<Constraint>&, const std::set<int>&, MOCK_METHOD3(Solve,
const std::map<std::string, LandmarkNode>&)); void(const std::vector<Constraint> &, const std::set<int> &,
const std::map<std::string, LandmarkNode> &));
}; };
class PoseGraph3DForTesting : public PoseGraph3D { class PoseGraph3DForTesting : public PoseGraph3D {
public: public:
PoseGraph3DForTesting( PoseGraph3DForTesting(
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, nullptr /* global_slam_optimization_callback */, : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
std::move(optimization_problem), thread_pool) {}
void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); } void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
}; };
@ -199,7 +199,7 @@ class EvenSubmapTrimmer : public PoseGraphTrimmer {
explicit EvenSubmapTrimmer(int trajectory_id) explicit EvenSubmapTrimmer(int trajectory_id)
: trajectory_id_(trajectory_id) {} : trajectory_id_(trajectory_id) {}
void Trim(Trimmable* pose_graph) override { void Trim(Trimmable *pose_graph) override {
auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
for (const auto submap_id : submap_ids) { for (const auto submap_id : submap_ids) {
if (submap_id.submap_index % 2 == 0) { if (submap_id.submap_index % 2 == 0) {

View File

@ -18,6 +18,7 @@
#define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_TESTING_MOCK_MAP_BUILDER_H_
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"

View File

@ -54,6 +54,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
std::map<int, mapping::PoseGraphInterface::TrajectoryData>()); std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
MOCK_CONST_METHOD0(constraints, std::vector<Constraint>()); MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph()); MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
MOCK_METHOD1(SetGlobalSlamOptimizationCallback,
void(GlobalSlamOptimizationCallback callback));
}; };
} // namespace testing } // namespace testing

View File

@ -61,22 +61,20 @@ std::vector<std::string> SelectRangeSensorIds(
return range_sensor_ids; return range_sensor_ids;
} }
MapBuilder::MapBuilder( MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
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(), global_slam_optimization_callback, options_.pose_graph_options(),
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(), global_slam_optimization_callback, options_.pose_graph_options(),
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_);

View File

@ -30,40 +30,38 @@ namespace cartographer {
namespace mapping { namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions( proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary); common::LuaParameterDictionary *const parameter_dictionary);
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure. // and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface { class MapBuilder : public MapBuilderInterface {
public: public:
MapBuilder(const proto::MapBuilderOptions& options, explicit MapBuilder(const proto::MapBuilderOptions &options);
PoseGraph::GlobalSlamOptimizationCallback
global_slam_optimization_callback);
~MapBuilder() override {} ~MapBuilder() override {}
MapBuilder(const MapBuilder&) = delete; MapBuilder(const MapBuilder &) = delete;
MapBuilder& operator=(const MapBuilder&) = delete; MapBuilder &operator=(const MapBuilder &) = delete;
int AddTrajectoryBuilder( int AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids, const std::set<SensorId> &expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options, const proto::TrajectoryBuilderOptions &trajectory_options,
LocalSlamResultCallback local_slam_result_callback) override; LocalSlamResultCallback local_slam_result_callback) override;
int AddTrajectoryForDeserialization( int AddTrajectoryForDeserialization(
const proto::TrajectoryBuilderOptionsWithSensorIds& const proto::TrajectoryBuilderOptionsWithSensorIds
options_with_sensor_ids_proto) override; &options_with_sensor_ids_proto) override;
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
std::string SubmapToProto(const SubmapId& submap_id, std::string SubmapToProto(const SubmapId &submap_id,
proto::SubmapQuery::Response* response) override; proto::SubmapQuery::Response *response) override;
void SerializeState(io::ProtoStreamWriterInterface* writer) override; void SerializeState(io::ProtoStreamWriterInterface *writer) override;
void LoadState(io::ProtoStreamReaderInterface* reader, void LoadState(io::ProtoStreamReaderInterface *reader,
bool load_frozen_state) override; bool load_frozen_state) override;
mapping::PoseGraphInterface* pose_graph() override { mapping::PoseGraphInterface *pose_graph() override {
return pose_graph_.get(); return pose_graph_.get();
} }
@ -71,13 +69,13 @@ class MapBuilder : public MapBuilderInterface {
return trajectory_builders_.size(); return trajectory_builders_.size();
} }
mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
int trajectory_id) const override { int trajectory_id) const override {
return trajectory_builders_.at(trajectory_id).get(); return trajectory_builders_.at(trajectory_id).get();
} }
const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>& const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
GetAllTrajectoryBuilderOptions() const override { &GetAllTrajectoryBuilderOptions() const override {
return all_trajectory_builder_options_; return all_trajectory_builder_options_;
} }

View File

@ -58,8 +58,7 @@ class MapBuilderTest : public ::testing::Test {
} }
void BuildMapBuilder() { void BuildMapBuilder() {
map_builder_ = common::make_unique<MapBuilder>( map_builder_ = common::make_unique<MapBuilder>(map_builder_options_);
map_builder_options_, nullptr /* global_slam_optimization_callback */);
} }
void SetOptionsTo3D() { void SetOptionsTo3D() {

View File

@ -52,10 +52,6 @@ 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 {}

View File

@ -79,6 +79,10 @@ class PoseGraphInterface {
common::optional<transform::Rigid3d> fixed_frame_origin_in_map; common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
}; };
using GlobalSlamOptimizationCallback =
std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
const std::map<int /* trajectory_id */, NodeId>&)>;
PoseGraphInterface() {} PoseGraphInterface() {}
virtual ~PoseGraphInterface() {} virtual ~PoseGraphInterface() {}
@ -129,6 +133,11 @@ class PoseGraphInterface {
// Serializes the constraints and trajectories. // Serializes the constraints and trajectories.
virtual proto::PoseGraph ToProto() const = 0; virtual proto::PoseGraph ToProto() const = 0;
// Sets the callback function that is invoked whenever the global optimization
// problem is solved.
virtual void SetGlobalSlamOptimizationCallback(
GlobalSlamOptimizationCallback callback) = 0;
}; };
} // namespace mapping } // namespace mapping