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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

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

2
cartographer/cloud/internal/client/pose_graph_stub.h
View File

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

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

@ -109,9 +109,7 @@ class ClientServerTest : public ::testing::Test {
void InitializeRealServer() {
auto map_builder = common::make_unique<MapBuilder>(
map_builder_server_options_.map_builder_options(),
nullptr /* global_slam_optimization_callback */
);
map_builder_server_options_.map_builder_options());
server_ = common::make_unique<MapBuilderServer>(map_builder_server_options_,
std::move(map_builder));
EXPECT_TRUE(server_ != nullptr);
@ -119,8 +117,7 @@ class ClientServerTest : public ::testing::Test {
void InitializeRealUploadingServer() {
auto map_builder = common::make_unique<MapBuilder>(
uploading_map_builder_server_options_.map_builder_options(),
nullptr /* global_slam_optimization_callback */);
uploading_map_builder_server_options_.map_builder_options());
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,8 +55,7 @@ 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(),
nullptr /* global_slam_optimization_callback */);
map_builder_server_options.map_builder_options());
std::unique_ptr<MapBuilderServerInterface> map_builder_server =
CreateMapBuilderServer(map_builder_server_options,
std::move(map_builder));

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

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

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

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

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

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

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

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

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

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

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

1
cartographer/mapping/internal/testing/mock_map_builder.h
View File

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

2
cartographer/mapping/internal/testing/mock_pose_graph.h
View File

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

8
cartographer/mapping/map_builder.cc
View File

@ -61,22 +61,20 @@ std::vector<std::string> SelectRangeSensorIds(
return range_sensor_ids;
}
MapBuilder::MapBuilder(
const proto::MapBuilderOptions& options,
PoseGraph::GlobalSlamOptimizationCallback global_slam_optimization_callback)
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: 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(), global_slam_optimization_callback,
options_.pose_graph_options(),
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(), global_slam_optimization_callback,
options_.pose_graph_options(),
common::make_unique<optimization::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);

34
cartographer/mapping/map_builder.h
View File

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

3
cartographer/mapping/map_builder_test.cc
View File

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

4
cartographer/mapping/pose_graph.h
View File

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

9
cartographer/mapping/pose_graph_interface.h
View File

@ -79,6 +79,10 @@ class PoseGraphInterface {
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() {}
virtual ~PoseGraphInterface() {}
@ -129,6 +133,11 @@ class PoseGraphInterface {
// Serializes the constraints and trajectories.
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