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
parent
207979f209
commit
ce18ec7295
|
@ -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
|
||||
|
|
|
@ -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_;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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_);
|
||||
|
||||
|
|
|
@ -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_);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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_);
|
||||
|
||||
|
|
|
@ -38,7 +38,8 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
|
|||
: OptimizationProblem3D(OptimizationProblemOptions{}) {}
|
||||
~MockOptimizationProblem3D() override = default;
|
||||
|
||||
MOCK_METHOD3(Solve, void(const std::vector<Constraint>&, const std::set<int>&,
|
||||
MOCK_METHOD3(Solve,
|
||||
void(const std::vector<Constraint> &, const std::set<int> &,
|
||||
const std::map<std::string, LandmarkNode> &));
|
||||
};
|
||||
|
||||
|
@ -48,8 +49,7 @@ class PoseGraph3DForTesting : public PoseGraph3D {
|
|||
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) {}
|
||||
: PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
|
||||
|
||||
void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
|
||||
};
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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_);
|
||||
|
|
|
@ -36,9 +36,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
|
|||
// 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;
|
||||
|
@ -50,8 +48,8 @@ class MapBuilder : public MapBuilderInterface {
|
|||
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;
|
||||
|
||||
|
@ -76,8 +74,8 @@ class MapBuilder : public MapBuilderInterface {
|
|||
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_;
|
||||
}
|
||||
|
||||
|
|
|
@ -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() {
|
||||
|
|
|
@ -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 {}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue