2d/sparse_pose_graph/constraint_builder.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_CONSTRAINT_BUILDER_H_
18 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
19 
20 #include <array>
21 #include <deque>
22 #include <functional>
23 #include <limits>
24 #include <vector>
25 
26 #include "Eigen/Core"
27 #include "Eigen/Geometry"
34 #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
38 
43 
44 namespace cartographer {
45 namespace mapping_2d {
46 namespace sparse_pose_graph {
47 
48 // Returns (map <- submap) where 'submap' is a coordinate system at the origin
49 // of the Submap.
50 transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
51 
52 // Asynchronously computes constraints.
53 //
54 // Intermingle an arbitrary number of calls to MaybeAddConstraint() or
55 // MaybeAddGlobalConstraint, then call WhenDone(). After all computations are
56 // done the 'callback' will be called with the result and another
57 // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow.
58 //
59 // This class is thread-safe.
61  public:
63  using Result = std::vector<Constraint>;
64 
66  const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions&
67  options,
68  common::ThreadPool* thread_pool);
70 
71  ConstraintBuilder(const ConstraintBuilder&) = delete;
73 
74  // Schedules exploring a new constraint between 'submap' identified by
75  // 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
76  // relative to the 'submap'.
77  //
78  // The pointees of 'submap' and 'point_cloud' must stay valid until all
79  // computations are finished.
80  void MaybeAddConstraint(const mapping::SubmapId& submap_id,
81  const mapping::Submap* submap,
82  const mapping::NodeId& node_id,
83  const sensor::PointCloud* point_cloud,
84  const transform::Rigid2d& initial_relative_pose);
85 
86  // Schedules exploring a new constraint between 'submap' identified by
87  // 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
88  // matching.
89  //
90  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
91  //
92  // The pointees of 'submap' and 'point_cloud' must stay valid until all
93  // computations are finished.
95  const mapping::SubmapId& submap_id, const mapping::Submap* submap,
96  const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
97  mapping::TrajectoryConnectivity* trajectory_connectivity);
98 
99  // Must be called after all computations related to one node have been added.
100  void NotifyEndOfScan();
101 
102  // Registers the 'callback' to be called with the results, after all
103  // computations triggered by MaybeAddConstraint() have finished.
104  void WhenDone(std::function<void(const Result&)> callback);
105 
106  // Returns the number of consecutive finished scans.
107  int GetNumFinishedScans();
108 
109  // Delete data related to 'submap_id'.
110  void DeleteScanMatcher(const mapping::SubmapId& submap_id);
111 
112  private:
115  std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
117  };
118 
119  // Either schedules the 'work_item', or if needed, schedules the scan matcher
120  // construction and queues the 'work_item'.
122  const mapping::SubmapId& submap_id, const ProbabilityGrid* submap,
123  std::function<void()> work_item) REQUIRES(mutex_);
124 
125  // Constructs the scan matcher for a 'submap', then schedules its work items.
126  void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,
127  const ProbabilityGrid* submap)
128  EXCLUDES(mutex_);
129 
130  // Returns the scan matcher for a submap, which has to exist.
132  const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
133 
134  // Runs in a background thread and does computations for an additional
135  // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
136  // If 'match_full_submap' is true, and global localization succeeds, will
137  // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
138  // 'trajectory_connectivity'.
139  // As output, it may create a new Constraint in 'constraint'.
140  void ComputeConstraint(
141  const mapping::SubmapId& submap_id, const mapping::Submap* submap,
142  const mapping::NodeId& node_id, bool match_full_submap,
143  mapping::TrajectoryConnectivity* trajectory_connectivity,
144  const sensor::PointCloud* point_cloud,
145  const transform::Rigid2d& initial_relative_pose,
146  std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
147 
148  // Decrements the 'pending_computations_' count. If all computations are done,
149  // runs the 'when_done_' callback and resets the state.
150  void FinishComputation(int computation_index) EXCLUDES(mutex_);
151 
152  const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_;
153  common::ThreadPool* thread_pool_;
154  common::Mutex mutex_;
155 
156  // 'callback' set by WhenDone().
157  std::unique_ptr<std::function<void(const Result&)>> when_done_
158  GUARDED_BY(mutex_);
159 
160  // Index of the scan in reaction to which computations are currently
161  // added. This is always the highest scan index seen so far, even when older
162  // scans are matched against a new submap.
163  int current_computation_ GUARDED_BY(mutex_) = 0;
164 
165  // For each added scan, maps to the number of pending computations that were
166  // added for it.
167  std::map<int, int> pending_computations_ GUARDED_BY(mutex_);
168 
169  // Constraints currently being computed in the background. A deque is used to
170  // keep pointers valid when adding more entries.
171  std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
172 
173  // Map of already constructed scan matchers by 'submap_id'.
174  std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
175  GUARDED_BY(mutex_);
176 
177  // Map by 'submap_id' of scan matchers under construction, and the work
178  // to do once construction is done.
179  std::map<mapping::SubmapId, std::vector<std::function<void()>>>
180  submap_queued_work_items_ GUARDED_BY(mutex_);
181 
182  common::FixedRatioSampler sampler_;
183  const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
184  scan_matching::CeresScanMatcher ceres_scan_matcher_;
185 
186  // Histogram of scan matcher scores.
187  common::Histogram score_histogram_ GUARDED_BY(mutex_);
188 };
189 
190 } // namespace sparse_pose_graph
191 } // namespace mapping_2d
192 } // namespace cartographer
193 
194 #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
void WhenDone(std::function< void(const Result &)> callback)
#define EXCLUDES(...)
Definition: mutex.h:53
void ConstructSubmapScanMatcher(const mapping::SubmapId &submap_id, const ProbabilityGrid *submap) EXCLUDES(mutex_)
transform::Rigid2d ComputeSubmapPose(const mapping::Submap &submap)
Rigid2< double > Rigid2d
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(const mapping::SubmapId &submap_id, const ProbabilityGrid *submap, std::function< void()> work_item) REQUIRES(mutex_)
std::unique_ptr< std::function< void(const Result &)> > when_done_ GUARDED_BY(mutex_)
ConstraintBuilder & operator=(const ConstraintBuilder &)=delete
#define REQUIRES(...)
Definition: mutex.h:44
ConstraintBuilder(const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions &options, common::ThreadPool *thread_pool)
void MaybeAddConstraint(const mapping::SubmapId &submap_id, const mapping::Submap *submap, const mapping::NodeId &node_id, const sensor::PointCloud *point_cloud, const transform::Rigid2d &initial_relative_pose)
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_
void MaybeAddGlobalConstraint(const mapping::SubmapId &submap_id, const mapping::Submap *submap, const mapping::NodeId &node_id, const sensor::PointCloud *point_cloud, mapping::TrajectoryConnectivity *trajectory_connectivity)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
const SubmapScanMatcher * GetSubmapScanMatcher(const mapping::SubmapId &submap_id) EXCLUDES(mutex_)
void ComputeConstraint(const mapping::SubmapId &submap_id, const mapping::Submap *submap, const mapping::NodeId &node_id, bool match_full_submap, mapping::TrajectoryConnectivity *trajectory_connectivity, const sensor::PointCloud *point_cloud, const transform::Rigid2d &initial_relative_pose, std::unique_ptr< Constraint > *constraint) EXCLUDES(mutex_)


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