pose_graph_2d.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_
00018 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_
00019 
00020 #include <deque>
00021 #include <functional>
00022 #include <limits>
00023 #include <map>
00024 #include <memory>
00025 #include <set>
00026 #include <vector>
00027 
00028 #include "Eigen/Core"
00029 #include "Eigen/Geometry"
00030 #include "absl/container/flat_hash_map.h"
00031 #include "absl/synchronization/mutex.h"
00032 #include "cartographer/common/fixed_ratio_sampler.h"
00033 #include "cartographer/common/thread_pool.h"
00034 #include "cartographer/common/time.h"
00035 #include "cartographer/mapping/2d/submap_2d.h"
00036 #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
00037 #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
00038 #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
00039 #include "cartographer/mapping/internal/work_queue.h"
00040 #include "cartographer/mapping/pose_graph.h"
00041 #include "cartographer/mapping/pose_graph_data.h"
00042 #include "cartographer/mapping/pose_graph_trimmer.h"
00043 #include "cartographer/mapping/value_conversion_tables.h"
00044 #include "cartographer/metrics/family_factory.h"
00045 #include "cartographer/sensor/fixed_frame_pose_data.h"
00046 #include "cartographer/sensor/landmark_data.h"
00047 #include "cartographer/sensor/odometry_data.h"
00048 #include "cartographer/sensor/point_cloud.h"
00049 #include "cartographer/transform/rigid_transform.h"
00050 #include "cartographer/transform/transform.h"
00051 
00052 namespace cartographer {
00053 namespace mapping {
00054 
00055 // Implements the loop closure method called Sparse Pose Adjustment (SPA) from
00056 // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
00057 // Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
00058 // on (pp. 22--29). IEEE, 2010.
00059 //
00060 // It is extended for submapping:
00061 // Each node has been matched against one or more submaps (adding a constraint
00062 // for each match), both poses of nodes and of submaps are to be optimized.
00063 // All constraints are between a submap i and a node j.
00064 class PoseGraph2D : public PoseGraph {
00065  public:
00066   PoseGraph2D(
00067       const proto::PoseGraphOptions& options,
00068       std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
00069       common::ThreadPool* thread_pool);
00070   ~PoseGraph2D() override;
00071 
00072   PoseGraph2D(const PoseGraph2D&) = delete;
00073   PoseGraph2D& operator=(const PoseGraph2D&) = delete;
00074 
00075   // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
00076   // determined by scan matching against 'insertion_submaps.front()' and the
00077   // node data was inserted into the 'insertion_submaps'. If
00078   // 'insertion_submaps.front().finished()' is 'true', data was inserted into
00079   // this submap for the last time.
00080   NodeId AddNode(
00081       std::shared_ptr<const TrajectoryNode::Data> constant_data,
00082       int trajectory_id,
00083       const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
00084       LOCKS_EXCLUDED(mutex_);
00085 
00086   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
00087       LOCKS_EXCLUDED(mutex_);
00088   void AddOdometryData(int trajectory_id,
00089                        const sensor::OdometryData& odometry_data) override
00090       LOCKS_EXCLUDED(mutex_);
00091   void AddFixedFramePoseData(
00092       int trajectory_id,
00093       const sensor::FixedFramePoseData& fixed_frame_pose_data) override
00094       LOCKS_EXCLUDED(mutex_);
00095   void AddLandmarkData(int trajectory_id,
00096                        const sensor::LandmarkData& landmark_data) override
00097       LOCKS_EXCLUDED(mutex_);
00098 
00099   void DeleteTrajectory(int trajectory_id) override;
00100   void FinishTrajectory(int trajectory_id) override;
00101   bool IsTrajectoryFinished(int trajectory_id) const override
00102       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00103   void FreezeTrajectory(int trajectory_id) override;
00104   bool IsTrajectoryFrozen(int trajectory_id) const override
00105       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00106   void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
00107                           const proto::Submap& submap) override;
00108   void AddNodeFromProto(const transform::Rigid3d& global_pose,
00109                         const proto::Node& node) override;
00110   void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override;
00111   void AddNodeToSubmap(const NodeId& node_id,
00112                        const SubmapId& submap_id) override;
00113   void AddSerializedConstraints(
00114       const std::vector<Constraint>& constraints) override;
00115   void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
00116   void RunFinalOptimization() override;
00117   std::vector<std::vector<int>> GetConnectedTrajectories() const override
00118       LOCKS_EXCLUDED(mutex_);
00119   PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const
00120       LOCKS_EXCLUDED(mutex_) override;
00121   MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
00122       LOCKS_EXCLUDED(mutex_) override;
00123   MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
00124       LOCKS_EXCLUDED(mutex_) override;
00125   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
00126       LOCKS_EXCLUDED(mutex_) override;
00127   MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
00128       LOCKS_EXCLUDED(mutex_);
00129   MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
00130       LOCKS_EXCLUDED(mutex_);
00131   std::map<int, TrajectoryState> GetTrajectoryStates() const override
00132       LOCKS_EXCLUDED(mutex_);
00133   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
00134       LOCKS_EXCLUDED(mutex_);
00135   void SetLandmarkPose(const std::string& landmark_id,
00136                        const transform::Rigid3d& global_pose,
00137                        const bool frozen = false) override
00138       LOCKS_EXCLUDED(mutex_);
00139   sensor::MapByTime<sensor::ImuData> GetImuData() const override
00140       LOCKS_EXCLUDED(mutex_);
00141   sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
00142       LOCKS_EXCLUDED(mutex_);
00143   sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
00144       const override LOCKS_EXCLUDED(mutex_);
00145   std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
00146   GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_);
00147   std::map<int, TrajectoryData> GetTrajectoryData() const override
00148       LOCKS_EXCLUDED(mutex_);
00149   std::vector<Constraint> constraints() const override LOCKS_EXCLUDED(mutex_);
00150   void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
00151                                 const transform::Rigid3d& pose,
00152                                 const common::Time time) override
00153       LOCKS_EXCLUDED(mutex_);
00154   void SetGlobalSlamOptimizationCallback(
00155       PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
00156   transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
00157       int trajectory_id, const common::Time time) const
00158       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00159 
00160   static void RegisterMetrics(metrics::FamilyFactory* family_factory);
00161 
00162  private:
00163   MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
00164       const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00165 
00166   // Handles a new work item.
00167   void AddWorkItem(const std::function<WorkItem::Result()>& work_item)
00168       LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);
00169 
00170   // Adds connectivity and sampler for a trajectory if it does not exist.
00171   void AddTrajectoryIfNeeded(int trajectory_id)
00172       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00173 
00174   // Appends the new node and submap (if needed) to the internal data
00175   // structures.
00176   NodeId AppendNode(
00177       std::shared_ptr<const TrajectoryNode::Data> constant_data,
00178       int trajectory_id,
00179       const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
00180       const transform::Rigid3d& optimized_pose) LOCKS_EXCLUDED(mutex_);
00181 
00182   // Grows the optimization problem to have an entry for every element of
00183   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
00184   std::vector<SubmapId> InitializeGlobalSubmapPoses(
00185       int trajectory_id, const common::Time time,
00186       const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
00187       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00188 
00189   // Adds constraints for a node, and starts scan matching in the background.
00190   WorkItem::Result ComputeConstraintsForNode(
00191       const NodeId& node_id,
00192       std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
00193       bool newly_finished_submap) LOCKS_EXCLUDED(mutex_);
00194 
00195   // Computes constraints for a node and submap pair.
00196   void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
00197       LOCKS_EXCLUDED(mutex_);
00198 
00199   // Deletes trajectories waiting for deletion. Must not be called during
00200   // constraint search.
00201   void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00202 
00203   // Runs the optimization, executes the trimmers and processes the work queue.
00204   void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
00205       LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);
00206 
00207   // Process pending tasks in the work queue on the calling thread, until the
00208   // queue is either empty or an optimization is required.
00209   void DrainWorkQueue() LOCKS_EXCLUDED(mutex_)
00210       LOCKS_EXCLUDED(work_queue_mutex_);
00211 
00212   // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
00213   // all computations have finished.
00214   void WaitForAllComputations() LOCKS_EXCLUDED(mutex_)
00215       LOCKS_EXCLUDED(work_queue_mutex_);
00216 
00217   // Runs the optimization. Callers have to make sure, that there is only one
00218   // optimization being run at a time.
00219   void RunOptimization() LOCKS_EXCLUDED(mutex_);
00220 
00221   bool CanAddWorkItemModifying(int trajectory_id)
00222       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00223 
00224   // Computes the local to global map frame transform based on the given
00225   // 'global_submap_poses'.
00226   transform::Rigid3d ComputeLocalToGlobalTransform(
00227       const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
00228       int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00229 
00230   SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
00231       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00232 
00233   common::Time GetLatestNodeTime(const NodeId& node_id,
00234                                  const SubmapId& submap_id) const
00235       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00236 
00237   // Updates the trajectory connectivity structure with a new constraint.
00238   void UpdateTrajectoryConnectivity(const Constraint& constraint)
00239       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
00240 
00241   const proto::PoseGraphOptions options_;
00242   GlobalSlamOptimizationCallback global_slam_optimization_callback_;
00243   mutable absl::Mutex mutex_;
00244   absl::Mutex work_queue_mutex_;
00245 
00246   // If it exists, further work items must be added to this queue, and will be
00247   // considered later.
00248   std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(work_queue_mutex_);
00249 
00250   // We globally localize a fraction of the nodes from each trajectory.
00251   absl::flat_hash_map<int, std::unique_ptr<common::FixedRatioSampler>>
00252       global_localization_samplers_ GUARDED_BY(mutex_);
00253 
00254   // Number of nodes added since last loop closure.
00255   int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;
00256 
00257   // Current optimization problem.
00258   std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
00259   constraints::ConstraintBuilder2D constraint_builder_;
00260 
00261   // Thread pool used for handling the work queue.
00262   common::ThreadPool* const thread_pool_;
00263 
00264   // List of all trimmers to consult when optimizations finish.
00265   std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
00266 
00267   PoseGraphData data_ GUARDED_BY(mutex_);
00268 
00269   ValueConversionTables conversion_tables_;
00270 
00271   // Allows querying and manipulating the pose graph by the 'trimmers_'. The
00272   // 'mutex_' of the pose graph is held while this class is used.
00273   class TrimmingHandle : public Trimmable {
00274    public:
00275     TrimmingHandle(PoseGraph2D* parent);
00276     ~TrimmingHandle() override {}
00277 
00278     int num_submaps(int trajectory_id) const override;
00279     std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
00280     MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
00281         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
00282     const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
00283         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
00284     const std::vector<Constraint>& GetConstraints() const override
00285         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
00286     void TrimSubmap(const SubmapId& submap_id)
00287         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
00288     bool IsFinished(int trajectory_id) const override
00289         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
00290     void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
00291         EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
00292 
00293    private:
00294     PoseGraph2D* const parent_;
00295   };
00296 };
00297 
00298 }  // namespace mapping
00299 }  // namespace cartographer
00300 
00301 #endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35