3d/sparse_pose_graph/constraint_builder.cc
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 
18 
19 #include <cmath>
20 #include <functional>
21 #include <iomanip>
22 #include <iostream>
23 #include <limits>
24 #include <memory>
25 #include <sstream>
26 #include <string>
27 
28 #include "Eigen/Eigenvalues"
33 #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
34 #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
36 #include "glog/logging.h"
37 
38 namespace cartographer {
39 namespace mapping_3d {
40 namespace sparse_pose_graph {
41 
43  const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options,
44  common::ThreadPool* const thread_pool)
45  : options_(options),
46  thread_pool_(thread_pool),
47  sampler_(options.sampling_ratio()),
48  adaptive_voxel_filter_(options.adaptive_voxel_filter_options()),
49  ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
50 
52  common::MutexLocker locker(&mutex_);
53  CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
54  CHECK_EQ(pending_computations_.size(), 0);
55  CHECK_EQ(submap_queued_work_items_.size(), 0);
56  CHECK(when_done_ == nullptr);
57 }
58 
60  const mapping::SubmapId& submap_id, const Submap* const submap,
61  const mapping::NodeId& node_id,
62  const sensor::CompressedPointCloud* const compressed_point_cloud,
63  const std::vector<mapping::TrajectoryNode>& submap_nodes,
64  const transform::Rigid3d& initial_pose) {
65  if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
66  return;
67  }
68  if (sampler_.Pulse()) {
69  common::MutexLocker locker(&mutex_);
70  constraints_.emplace_back();
71  auto* const constraint = &constraints_.back();
72  ++pending_computations_[current_computation_];
73  const int current_computation = current_computation_;
75  submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
76  [=]() EXCLUDES(mutex_) {
77  ComputeConstraint(submap_id, submap, node_id,
78  false, /* match_full_submap */
79  nullptr, /* trajectory_connectivity */
80  compressed_point_cloud, initial_pose, constraint);
81  FinishComputation(current_computation);
82  });
83  }
84 }
85 
87  const mapping::SubmapId& submap_id, const Submap* const submap,
88  const mapping::NodeId& node_id,
89  const sensor::CompressedPointCloud* const compressed_point_cloud,
90  const std::vector<mapping::TrajectoryNode>& submap_nodes,
91  const Eigen::Quaterniond& gravity_alignment,
92  mapping::TrajectoryConnectivity* const trajectory_connectivity) {
93  common::MutexLocker locker(&mutex_);
94  constraints_.emplace_back();
95  auto* const constraint = &constraints_.back();
96  ++pending_computations_[current_computation_];
97  const int current_computation = current_computation_;
99  submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
100  [=]() EXCLUDES(mutex_) {
102  submap_id, submap, node_id, true, /* match_full_submap */
103  trajectory_connectivity, compressed_point_cloud,
104  transform::Rigid3d::Rotation(gravity_alignment), constraint);
105  FinishComputation(current_computation);
106  });
107 }
108 
110  common::MutexLocker locker(&mutex_);
111  ++current_computation_;
112 }
113 
115  const std::function<void(const ConstraintBuilder::Result&)> callback) {
116  common::MutexLocker locker(&mutex_);
117  CHECK(when_done_ == nullptr);
118  when_done_ =
119  common::make_unique<std::function<void(const Result&)>>(callback);
120  ++pending_computations_[current_computation_];
121  const int current_computation = current_computation_;
123  [this, current_computation] { FinishComputation(current_computation); });
124 }
125 
127  const mapping::SubmapId& submap_id,
128  const std::vector<mapping::TrajectoryNode>& submap_nodes,
129  const HybridGrid* const submap, const std::function<void()> work_item) {
130  if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
131  nullptr) {
132  thread_pool_->Schedule(work_item);
133  } else {
134  submap_queued_work_items_[submap_id].push_back(work_item);
135  if (submap_queued_work_items_[submap_id].size() == 1) {
136  thread_pool_->Schedule([=]() {
137  ConstructSubmapScanMatcher(submap_id, submap_nodes, submap);
138  });
139  }
140  }
141 }
142 
144  const mapping::SubmapId& submap_id,
145  const std::vector<mapping::TrajectoryNode>& submap_nodes,
146  const HybridGrid* const submap) {
147  auto submap_scan_matcher =
148  common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
149  *submap, submap_nodes,
150  options_.fast_correlative_scan_matcher_options_3d());
151  common::MutexLocker locker(&mutex_);
152  submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
153  for (const std::function<void()>& work_item :
154  submap_queued_work_items_[submap_id]) {
155  thread_pool_->Schedule(work_item);
156  }
157  submap_queued_work_items_.erase(submap_id);
158 }
159 
162  common::MutexLocker locker(&mutex_);
163  const SubmapScanMatcher* submap_scan_matcher =
164  &submap_scan_matchers_[submap_id];
165  CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr);
166  return submap_scan_matcher;
167 }
168 
170  const mapping::SubmapId& submap_id, const Submap* const submap,
171  const mapping::NodeId& node_id, bool match_full_submap,
172  mapping::TrajectoryConnectivity* trajectory_connectivity,
173  const sensor::CompressedPointCloud* const compressed_point_cloud,
174  const transform::Rigid3d& initial_pose,
175  std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
176  const SubmapScanMatcher* const submap_scan_matcher =
177  GetSubmapScanMatcher(submap_id);
178  const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
179  const sensor::PointCloud filtered_point_cloud =
180  adaptive_voxel_filter_.Filter(point_cloud);
181 
182  // The 'constraint_transform' (submap 'i' <- scan 'j') is computed from the
183  // initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a
184  // 'filtered_point_cloud' in 'j'.
185  float score = 0.;
186  transform::Rigid3d pose_estimate;
187 
188  if (match_full_submap) {
189  if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
190  initial_pose.rotation(), filtered_point_cloud, point_cloud,
191  options_.global_localization_min_score(), &score, &pose_estimate)) {
192  CHECK_GT(score, options_.global_localization_min_score());
193  CHECK_GE(node_id.trajectory_id, 0);
194  CHECK_GE(submap_id.trajectory_id, 0);
195  trajectory_connectivity->Connect(node_id.trajectory_id,
196  submap_id.trajectory_id);
197  } else {
198  return;
199  }
200  } else {
201  if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
202  initial_pose, filtered_point_cloud, point_cloud,
203  options_.min_score(), &score, &pose_estimate)) {
204  // We've reported a successful local match.
205  CHECK_GT(score, options_.min_score());
206  } else {
207  return;
208  }
209  }
210  {
211  common::MutexLocker locker(&mutex_);
212  score_histogram_.Add(score);
213  }
214 
215  // Use the CSM estimate as both the initial and previous pose. This has the
216  // effect that, in the absence of better information, we prefer the original
217  // CSM estimate.
218  ceres::Solver::Summary unused_summary;
219  transform::Rigid3d constraint_transform;
221  pose_estimate, pose_estimate,
222  {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
223  &constraint_transform, &unused_summary);
224 
225  constraint->reset(new OptimizationProblem::Constraint{
226  submap_id,
227  node_id,
228  {constraint_transform, options_.loop_closure_translation_weight(),
229  options_.loop_closure_rotation_weight()},
231 
232  if (options_.log_matches()) {
233  std::ostringstream info;
234  info << "Node " << node_id << " with " << filtered_point_cloud.size()
235  << " points on submap " << submap_id << std::fixed;
236  if (match_full_submap) {
237  info << " matches";
238  } else {
239  const transform::Rigid3d difference =
240  initial_pose.inverse() * constraint_transform;
241  info << " differs by translation " << std::setprecision(2)
242  << difference.translation().norm() << " rotation "
243  << std::setprecision(3) << transform::GetAngle(difference);
244  }
245  info << " with score " << std::setprecision(1) << 100. * score << "%.";
246  LOG(INFO) << info.str();
247  }
248 }
249 
250 void ConstraintBuilder::FinishComputation(const int computation_index) {
251  Result result;
252  std::unique_ptr<std::function<void(const Result&)>> callback;
253  {
254  common::MutexLocker locker(&mutex_);
255  if (--pending_computations_[computation_index] == 0) {
256  pending_computations_.erase(computation_index);
257  }
258  if (pending_computations_.empty()) {
259  CHECK_EQ(submap_queued_work_items_.size(), 0);
260  if (when_done_ != nullptr) {
261  for (const std::unique_ptr<OptimizationProblem::Constraint>&
262  constraint : constraints_) {
263  if (constraint != nullptr) {
264  result.push_back(*constraint);
265  }
266  }
267  if (options_.log_matches()) {
268  LOG(INFO) << constraints_.size() << " computations resulted in "
269  << result.size() << " additional constraints.";
270  LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
271  }
272  constraints_.clear();
273  callback = std::move(when_done_);
274  when_done_.reset();
275  }
276  }
277  }
278  if (callback != nullptr) {
279  (*callback)(result);
280  }
281 }
282 
284  common::MutexLocker locker(&mutex_);
285  if (pending_computations_.empty()) {
286  return current_computation_;
287  }
288  return pending_computations_.begin()->first;
289 }
290 
291 } // namespace sparse_pose_graph
292 } // namespace mapping_3d
293 } // namespace cartographer
proto::RangeDataInserterOptions options_
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)
common::ThreadPool thread_pool_
const Quaternion & rotation() const
static Rigid3 Rotation(const AngleAxis &angle_axis)
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_)
#define EXCLUDES(...)
Definition: mutex.h:53
const HybridGrid & high_resolution_hybrid_grid() const
Definition: 3d/submaps.h:55
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_
void Match(const transform::Rigid3d &previous_pose, const transform::Rigid3d &initial_pose_estimate, const std::vector< PointCloudAndHybridGridPointers > &point_clouds_and_hybrid_grids, transform::Rigid3d *pose_estimate, ceres::Solver::Summary *summary)
PointCloud Filter(const PointCloud &point_cloud) const
void Schedule(std::function< void()> work_item)
Definition: thread_pool.cc:48
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)
void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_)
const Vector & translation() const
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
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)
FloatType GetAngle(const Rigid3< FloatType > &transform)
Definition: transform.h:34
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(const mapping::SubmapId &submap_id, const std::vector< mapping::TrajectoryNode > &submap_nodes, const HybridGrid *submap, std::function< void()> work_item) REQUIRES(mutex_)
Mutex::Locker MutexLocker
Definition: mutex.h:95
std::unique_ptr< CeresScanMatcher > ceres_scan_matcher_


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