2d/sparse_pose_graph.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
18 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
19 
20 #include <deque>
21 #include <functional>
22 #include <limits>
23 #include <map>
24 #include <memory>
25 #include <unordered_map>
26 #include <vector>
27 
28 #include "Eigen/Core"
29 #include "Eigen/Geometry"
43 
44 namespace cartographer {
45 namespace mapping_2d {
46 
47 // Implements the loop closure method called Sparse Pose Adjustment (SPA) from
48 // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
49 // Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
50 // on (pp. 22--29). IEEE, 2010.
51 //
52 // It is extended for submapping:
53 // Each scan has been matched against one or more submaps (adding a constraint
54 // for each match), both poses of scans and of submaps are to be optimized.
55 // All constraints are between a submap i and a scan j.
57  public:
58  SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
59  common::ThreadPool* thread_pool);
60  ~SparsePoseGraph() override;
61 
62  SparsePoseGraph(const SparsePoseGraph&) = delete;
63  SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
64 
65  // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose'
66  // that will later be optimized. The 'tracking_to_pose' is remembered so
67  // that the optimized pose can be embedded into 3D. The 'pose' was determined
68  // by scan matching against the 'matching_submap' and the scan was inserted
69  // into the 'insertion_submaps'.
70  void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
71  const sensor::RangeData& range_data_in_pose,
72  const transform::Rigid2d& pose, int trajectory_id,
73  const mapping::Submap* matching_submap,
74  const std::vector<const mapping::Submap*>& insertion_submaps)
76 
77  // Adds new IMU data to be used in the optimization.
78  void AddImuData(int trajectory_id, common::Time time,
79  const Eigen::Vector3d& linear_acceleration,
80  const Eigen::Vector3d& angular_velocity);
81 
82  void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
83  void RunFinalOptimization() override;
84  std::vector<std::vector<int>> GetConnectedTrajectories() override;
85  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
87  EXCLUDES(mutex_) override;
89  EXCLUDES(mutex_) override;
90  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
91  override EXCLUDES(mutex_);
92  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
93 
94  private:
95  // The current state of the submap in the background threads. When this
96  // transitions to kFinished, all scans are tried to match against this submap.
97  // Likewise, all new scans are matched against submaps which are finished.
99  struct SubmapData {
100  const mapping::Submap* submap = nullptr;
101 
102  // IDs of the scans that were inserted into this map together with
103  // constraints for them. They are not to be matched again when this submap
104  // becomes 'finished'.
105  std::set<mapping::NodeId> node_ids;
106 
108  };
109 
110  // Handles a new work item.
111  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
112 
113  mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
114  REQUIRES(mutex_) {
115  const auto iterator = submap_ids_.find(submap);
116  CHECK(iterator != submap_ids_.end());
117  return iterator->second;
118  }
119 
120  // Grows the optimization problem to have an entry for every element of
121  // 'insertion_submaps'.
123  const std::vector<const mapping::Submap*>& insertion_submaps)
124  REQUIRES(mutex_);
125 
126  // Adds constraints for a scan, and starts scan matching in the background.
128  const mapping::Submap* matching_submap,
129  std::vector<const mapping::Submap*> insertion_submaps,
130  const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
131  REQUIRES(mutex_);
132 
133  // Computes constraints for a scan and submap pair.
134  void ComputeConstraint(const mapping::NodeId& node_id,
135  const mapping::SubmapId& submap_id) REQUIRES(mutex_);
136 
137  // Adds constraints for older scans whenever a new submap is finished.
139  REQUIRES(mutex_);
140 
141  // Registers the callback to run the optimization once all constraints have
142  // been computed, that will also do all work that queue up in 'scan_queue_'.
144 
145  // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
146  // all computations have finished.
148 
149  // Runs the optimization. Callers have to make sure, that there is only one
150  // optimization being run at a time.
152 
153  // Computes the local to global frame transform based on the given optimized
154  // 'submap_transforms'.
156  const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
157  submap_transforms,
158  int trajectory_id) const REQUIRES(mutex_);
159 
160  const mapping::proto::SparsePoseGraphOptions options_;
161  common::Mutex mutex_;
162 
163  // If it exists, further scans must be added to this queue, and will be
164  // considered later.
165  std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
166  GUARDED_BY(mutex_);
167 
168  // How our various trajectories are related.
169  mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
170 
171  // We globally localize a fraction of the scans from each trajectory.
172  std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
173  global_localization_samplers_ GUARDED_BY(mutex_);
174 
175  // Number of scans added since last loop closure.
176  int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;
177 
178  // Whether the optimization has to be run before more data is added.
179  bool run_loop_closure_ GUARDED_BY(mutex_) = false;
180 
181  // Current optimization problem.
183  sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
184  std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
185 
186  // Submaps get assigned an ID and state as soon as they are seen, even
187  // before they take part in the background computations.
188  std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
189  GUARDED_BY(mutex_);
191  GUARDED_BY(mutex_);
192 
193  // Connectivity structure of our trajectories by IDs.
194  std::vector<std::vector<int>> connected_components_;
195  // Trajectory ID to connected component ID.
196  std::map<int, size_t> reverse_connected_components_;
197 
198  // Data that are currently being shown.
200  trajectory_nodes_ GUARDED_BY(mutex_);
201  int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
202 
203  // Current submap transforms used for displaying data.
204  std::vector<std::vector<sparse_pose_graph::SubmapData>>
205  optimized_submap_transforms_ GUARDED_BY(mutex_);
206 
207  // List of all trimmers to consult when optimizations finish.
208  std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
209  GUARDED_BY(mutex_);
210 
211  // Allows querying and manipulating the pose graph by the 'trimmers_'. The
212  // 'mutex_' of the pose graph is held while this class is used.
214  public:
216  ~TrimmingHandle() override {}
217 
218  int num_submaps(int trajectory_id) const override;
219  void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) override;
220 
221  private:
223  };
224 };
225 
226 } // namespace mapping_2d
227 } // namespace cartographer
228 
229 #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
void AddTrimmer(std::unique_ptr< mapping::PoseGraphTrimmer > trimmer) override
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId &submap_id) EXCLUDES(mutex_) override
#define EXCLUDES(...)
Definition: mutex.h:53
std::unique_ptr< std::deque< std::function< void()> > > scan_queue_ GUARDED_BY(mutex_)
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
void AddScan(common::Time time, const transform::Rigid3d &tracking_to_pose, const sensor::RangeData &range_data_in_pose, const transform::Rigid2d &pose, int trajectory_id, const mapping::Submap *matching_submap, const std::vector< const mapping::Submap * > &insertion_submaps) EXCLUDES(mutex_)
void GrowSubmapTransformsAsNeeded(const std::vector< const mapping::Submap * > &insertion_submaps) REQUIRES(mutex_)
SparsePoseGraph & operator=(const SparsePoseGraph &)=delete
#define REQUIRES(...)
Definition: mutex.h:44
transform::Rigid3d ComputeLocalToGlobalTransform(const std::vector< std::vector< sparse_pose_graph::SubmapData >> &submap_transforms, int trajectory_id) const REQUIRES(mutex_)
void ComputeConstraint(const mapping::NodeId &node_id, const mapping::SubmapId &submap_id) REQUIRES(mutex_)
std::vector< std::vector< int > > GetConnectedTrajectories() override
const mapping::proto::SparsePoseGraphOptions options_
mapping::SubmapId GetSubmapId(const mapping::Submap *submap) const REQUIRES(mutex_)
void AddWorkItem(std::function< void()> work_item) REQUIRES(mutex_)
std::vector< std::vector< mapping::TrajectoryNode > > GetTrajectoryNodes() override EXCLUDES(mutex_)
void ComputeConstraintsForScan(const mapping::Submap *matching_submap, std::vector< const mapping::Submap * > insertion_submaps, const mapping::Submap *finished_submap, const transform::Rigid2d &pose) REQUIRES(mutex_)
std::vector< Constraint > constraints() override EXCLUDES(mutex_)
std::vector< std::vector< int > > connected_components_
sparse_pose_graph::OptimizationProblem optimization_problem_
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override
void ComputeConstraintsForOldScans(const mapping::Submap *submap) REQUIRES(mutex_)
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:59