3d/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_3D_SPARSE_POSE_GRAPH_H_
18 #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
19 
20 #include <deque>
21 #include <functional>
22 #include <limits>
23 #include <map>
24 #include <unordered_map>
25 #include <vector>
26 
27 #include "Eigen/Core"
28 #include "Eigen/Geometry"
41 
42 namespace cartographer {
43 namespace mapping_3d {
44 
45 // Implements the loop closure method called Sparse Pose Adjustment (SPA) from
46 // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
47 // Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
48 // on (pp. 22--29). IEEE, 2010.
49 //
50 // It is extended for submapping in 3D:
51 // Each scan has been matched against one or more submaps (adding a constraint
52 // for each match), both poses of scans and of submaps are to be optimized.
53 // All constraints are between a submap i and a scan j.
55  public:
56  SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
57  common::ThreadPool* thread_pool);
58  ~SparsePoseGraph() override;
59 
60  SparsePoseGraph(const SparsePoseGraph&) = delete;
61  SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
62 
63  // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
64  // that will later be optimized. The 'pose' was determined by scan matching
65  // against the 'matching_submap' and the scan was inserted into the
66  // 'insertion_submaps'.
68  const sensor::RangeData& range_data_in_tracking,
69  const transform::Rigid3d& pose, int trajectory_id,
70  const Submap* matching_submap,
71  const std::vector<const Submap*>& insertion_submaps)
73 
74  // Adds new IMU data to be used in the optimization.
75  void AddImuData(int trajectory_id, common::Time time,
76  const Eigen::Vector3d& linear_acceleration,
77  const Eigen::Vector3d& angular_velocity);
78 
79  void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override {
80  LOG(FATAL) << "Not yet implemented for 3D.";
81  }
82  void RunFinalOptimization() override;
83  std::vector<std::vector<int>> GetConnectedTrajectories() override;
84  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
86  EXCLUDES(mutex_) override;
88  EXCLUDES(mutex_) override;
89  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
90  override EXCLUDES(mutex_);
91  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
92 
93  private:
94  // The current state of the submap in the background threads. When this
95  // transitions to kFinished, all scans are tried to match against this submap.
96  // Likewise, all new scans are matched against submaps which are finished.
98  struct SubmapData {
99  const Submap* submap = nullptr;
100 
101  // IDs of the scans that were inserted into this map together with
102  // constraints for them. They are not to be matched again when this submap
103  // becomes 'finished'.
104  std::set<mapping::NodeId> node_ids;
105 
107  };
108 
109  // Handles a new work item.
110  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
111 
112  mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
113  REQUIRES(mutex_) {
114  const auto iterator = submap_ids_.find(submap);
115  CHECK(iterator != submap_ids_.end());
116  return iterator->second;
117  }
118 
119  // Grows the optimization problem to have an entry for every element of
120  // 'insertion_submaps'.
122  const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
123 
124  // Adds constraints for a scan, and starts scan matching in the background.
125  void ComputeConstraintsForScan(const Submap* matching_submap,
126  std::vector<const Submap*> insertion_submaps,
127  const Submap* finished_submap,
128  const transform::Rigid3d& pose)
129  REQUIRES(mutex_);
130 
131  // Computes constraints for a scan and submap pair.
132  void ComputeConstraint(const mapping::NodeId& node_id,
133  const mapping::SubmapId& submap_id) REQUIRES(mutex_);
134 
135  // Adds constraints for older scans whenever a new submap is finished.
137 
138  // Registers the callback to run the optimization once all constraints have
139  // been computed, that will also do all work that queue up in 'scan_queue_'.
141 
142  // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
143  // all computations have finished.
145 
146  // Runs the optimization. Callers have to make sure, that there is only one
147  // optimization being run at a time.
149 
150  // Computes the local to global frame transform based on the given optimized
151  // 'submap_transforms'.
153  const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
154  submap_transforms,
155  int trajectory_id) const REQUIRES(mutex_);
156 
157  const mapping::proto::SparsePoseGraphOptions options_;
158  common::Mutex mutex_;
159 
160  // If it exists, further scans must be added to this queue, and will be
161  // considered later.
162  std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
163  GUARDED_BY(mutex_);
164 
165  // How our various trajectories are related.
166  mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
167 
168  // We globally localize a fraction of the scans from each trajectory.
169  std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
170  global_localization_samplers_ GUARDED_BY(mutex_);
171 
172  // Number of scans added since last loop closure.
173  int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;
174 
175  // Whether the optimization has to be run before more data is added.
176  bool run_loop_closure_ GUARDED_BY(mutex_) = false;
177 
178  // Current optimization problem.
180  sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
181  std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
182 
183  // Submaps get assigned an ID and state as soon as they are seen, even
184  // before they take part in the background computations.
185  std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
186  GUARDED_BY(mutex_);
188  GUARDED_BY(mutex_);
189 
190  // Connectivity structure of our trajectories by IDs.
191  std::vector<std::vector<int>> connected_components_;
192  // Trajectory ID to connected component ID.
193  std::map<int, size_t> reverse_connected_components_;
194 
195  // Data that are currently being shown.
197  trajectory_nodes_ GUARDED_BY(mutex_);
198  int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
199 
200  // Current submap transforms used for displaying data.
201  std::vector<std::vector<sparse_pose_graph::SubmapData>>
202  optimized_submap_transforms_ GUARDED_BY(mutex_);
203 };
204 
205 } // namespace mapping_3d
206 } // namespace cartographer
207 
208 #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
void GrowSubmapTransformsAsNeeded(const std::vector< const Submap * > &insertion_submaps) REQUIRES(mutex_)
const mapping::proto::SparsePoseGraphOptions options_
void AddWorkItem(std::function< void()> work_item) REQUIRES(mutex_)
void ComputeConstraintsForScan(const Submap *matching_submap, std::vector< const Submap * > insertion_submaps, const Submap *finished_submap, const transform::Rigid3d &pose) REQUIRES(mutex_)
std::vector< std::vector< mapping::TrajectoryNode > > GetTrajectoryNodes() override EXCLUDES(mutex_)
#define EXCLUDES(...)
Definition: mutex.h:53
std::vector< Constraint > constraints() override EXCLUDES(mutex_)
void ComputeConstraint(const mapping::NodeId &node_id, const mapping::SubmapId &submap_id) REQUIRES(mutex_)
SparsePoseGraph & operator=(const SparsePoseGraph &)=delete
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
std::vector< std::vector< int > > connected_components_
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
#define REQUIRES(...)
Definition: mutex.h:44
std::unique_ptr< std::deque< std::function< void()> > > scan_queue_ GUARDED_BY(mutex_)
transform::Rigid3d ComputeLocalToGlobalTransform(const std::vector< std::vector< sparse_pose_graph::SubmapData >> &submap_transforms, int trajectory_id) const REQUIRES(mutex_)
mapping::SubmapId GetSubmapId(const mapping::Submap *submap) const REQUIRES(mutex_)
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId &submap_id) EXCLUDES(mutex_) override
void ComputeConstraintsForOldScans(const Submap *submap) REQUIRES(mutex_)
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override
sparse_pose_graph::OptimizationProblem optimization_problem_
void AddTrimmer(std::unique_ptr< mapping::PoseGraphTrimmer > trimmer) override
void AddScan(common::Time time, const sensor::RangeData &range_data_in_tracking, const transform::Rigid3d &pose, int trajectory_id, const Submap *matching_submap, const std::vector< const Submap * > &insertion_submaps) EXCLUDES(mutex_)
std::vector< std::vector< int > > GetConnectedTrajectories() override


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