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


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