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


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:38