From 96cdbde5bf8f27f26e399d8d70200b77190eca90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Fri, 8 Dec 2017 11:13:48 +0100 Subject: [PATCH] Introduce PoseGraphInterface. (#744) [RFC=0005](https://github.com/googlecartographer/rfcs/blob/master/text/0005-pose-graph-interface.md) --- cartographer/mapping/map_builder.cc | 2 +- cartographer/mapping/map_builder.h | 3 +- cartographer/mapping/map_builder_interface.h | 8 +- cartographer/mapping/pose_graph.h | 52 +----------- cartographer/mapping/pose_graph_interface.h | 87 ++++++++++++++++++++ 5 files changed, 97 insertions(+), 55 deletions(-) create mode 100644 cartographer/mapping/pose_graph_interface.h diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 9d0b484..4e756de 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -307,7 +307,7 @@ int MapBuilder::num_trajectory_builders() const { return trajectory_builders_.size(); } -PoseGraph* MapBuilder::pose_graph() { return pose_graph_; } +PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_; } } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 18bdba0..b47ce81 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -23,6 +23,7 @@ #include #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h" @@ -65,7 +66,7 @@ class MapBuilder : public MapBuilderInterface { int num_trajectory_builders() const override; - mapping::PoseGraph* pose_graph() override; + mapping::PoseGraphInterface* pose_graph() override; private: const proto::MapBuilderOptions options_; diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 4dfbf4c..93396b9 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -27,7 +27,7 @@ #include "cartographer/io/proto_stream.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" @@ -43,8 +43,8 @@ class MapBuilderInterface { using LocalSlamResultCallback = GlobalTrajectoryBuilderInterface::LocalSlamResultCallback; - MapBuilderInterface(){}; - virtual ~MapBuilderInterface(){}; + MapBuilderInterface() {} + virtual ~MapBuilderInterface() {} MapBuilderInterface(const MapBuilderInterface&) = delete; MapBuilderInterface& operator=(const MapBuilderInterface&) = delete; @@ -82,7 +82,7 @@ class MapBuilderInterface { virtual int num_trajectory_builders() const = 0; - virtual mapping::PoseGraph* pose_graph() = 0; + virtual mapping::PoseGraphInterface* pose_graph() = 0; }; } // namespace mapping diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 8d35b7a..2821a2b 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -25,6 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/pose_graph_options.pb.h" @@ -43,35 +44,8 @@ namespace mapping { proto::PoseGraphOptions CreatePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); -class PoseGraph { +class PoseGraph : public PoseGraphInterface { public: - // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse - // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), - // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. - struct Constraint { - struct Pose { - transform::Rigid3d zbar_ij; - double translation_weight; - double rotation_weight; - }; - - SubmapId submap_id; // 'i' in the paper. - NodeId node_id; // 'j' in the paper. - - // Pose of the node 'j' relative to submap 'i'. - Pose pose; - - // Differentiates between intra-submap (where node 'j' was inserted into - // submap 'i') and inter-submap constraints (where node 'j' was not inserted - // into submap 'i'). - enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; - }; - - struct SubmapData { - std::shared_ptr submap; - transform::Rigid3d pose; - }; - struct InitialTrajectoryPose { int to_trajectory_id; transform::Rigid3d relative_pose; @@ -79,7 +53,7 @@ class PoseGraph { }; PoseGraph() {} - virtual ~PoseGraph() {} + virtual ~PoseGraph() override {} PoseGraph(const PoseGraph&) = delete; PoseGraph& operator=(const PoseGraph&) = delete; @@ -100,9 +74,6 @@ class PoseGraph { // Finishes the given trajectory. virtual void FinishTrajectory(int trajectory_id) = 0; - // Checks if the given trajectory is finished. - virtual bool IsTrajectoryFinished(int trajectory_id) = 0; - // Freezes a trajectory. Poses in this trajectory will not be optimized. virtual void FreezeTrajectory(int trajectory_id) = 0; @@ -130,9 +101,6 @@ class PoseGraph { // included in the pose graph. virtual void AddTrimmer(std::unique_ptr trimmer) = 0; - // Computes optimized poses. - virtual void RunFinalOptimization() = 0; - // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() = 0; @@ -141,17 +109,6 @@ class PoseGraph { // not exist (anymore). virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0; - // Returns data for all submaps. - virtual MapById GetAllSubmapData() = 0; - - // Returns the transform converting data in the local map frame (i.e. the - // continuous, non-loop-closed frame) into the global map frame (i.e. the - // discontinuous, loop-closed frame). - virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0; - - // Returns the current optimized trajectories. - virtual MapById GetTrajectoryNodes() = 0; - // Serializes the constraints and trajectories. proto::PoseGraph ToProto(); @@ -165,9 +122,6 @@ class PoseGraph { virtual sensor::MapByTime GetFixedFramePoseData() = 0; - // Returns the collection of constraints. - virtual std::vector constraints() = 0; - // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with // respect to 'to_trajectory_id' at time 'time'. virtual void SetInitialTrajectoryPose(int from_trajectory_id, diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h new file mode 100644 index 0000000..c1625de --- /dev/null +++ b/cartographer/mapping/pose_graph_interface.h @@ -0,0 +1,87 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_ + +#include + +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/submaps.h" + +namespace cartographer { +namespace mapping { + +class PoseGraphInterface { + public: + // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse + // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), + // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. + struct Constraint { + struct Pose { + transform::Rigid3d zbar_ij; + double translation_weight; + double rotation_weight; + }; + + SubmapId submap_id; // 'i' in the paper. + NodeId node_id; // 'j' in the paper. + + // Pose of the node 'j' relative to submap 'i'. + Pose pose; + + // Differentiates between intra-submap (where node 'j' was inserted into + // submap 'i') and inter-submap constraints (where node 'j' was not inserted + // into submap 'i'). + enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; + }; + + struct SubmapData { + std::shared_ptr submap; + transform::Rigid3d pose; + }; + + PoseGraphInterface() {} + virtual ~PoseGraphInterface() {} + + PoseGraphInterface(const PoseGraphInterface&) = delete; + PoseGraphInterface& operator=(const PoseGraphInterface&) = delete; + + // Computes optimized poses. + virtual void RunFinalOptimization() = 0; + + // Returns data for all submaps. + virtual MapById GetAllSubmapData() = 0; + + // Returns the transform converting data in the local map frame (i.e. the + // continuous, non-loop-closed frame) into the global map frame (i.e. the + // discontinuous, loop-closed frame). + virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0; + + // Returns the current optimized trajectories. + virtual MapById GetTrajectoryNodes() = 0; + + // Checks if the given trajectory is finished. + virtual bool IsTrajectoryFinished(int trajectory_id) = 0; + + // Returns the collection of constraints. + virtual std::vector constraints() = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_