3d/sparse_pose_graph.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 <algorithm>
20 #include <cmath>
21 #include <cstdio>
22 #include <functional>
23 #include <iomanip>
24 #include <iostream>
25 #include <limits>
26 #include <memory>
27 #include <sstream>
28 #include <string>
29 
30 #include "Eigen/Eigenvalues"
33 #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
36 #include "glog/logging.h"
37 
38 namespace cartographer {
39 namespace mapping_3d {
40 
42  const mapping::proto::SparsePoseGraphOptions& options,
43  common::ThreadPool* thread_pool)
44  : options_(options),
45  optimization_problem_(options_.optimization_problem_options(),
46  sparse_pose_graph::OptimizationProblem::FixZ::kNo),
47  constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
48 
51  common::MutexLocker locker(&mutex_);
52  CHECK(scan_queue_ == nullptr);
53 }
54 
56  const std::vector<const Submap*>& insertion_submaps) {
57  CHECK(!insertion_submaps.empty());
58  const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
59  const int trajectory_id = first_submap_id.trajectory_id;
60  CHECK_GE(trajectory_id, 0);
61  const auto& submap_data = optimization_problem_.submap_data();
62  if (insertion_submaps.size() == 1) {
63  // If we don't already have an entry for the first submap, add one.
64  CHECK_EQ(first_submap_id.submap_index, 0);
65  if (static_cast<size_t>(trajectory_id) >= submap_data.size() ||
66  submap_data[trajectory_id].empty()) {
67  optimization_problem_.AddSubmap(trajectory_id,
69  }
70  return;
71  }
72  CHECK_EQ(2, insertion_submaps.size());
73  const int next_submap_index = submap_data.at(trajectory_id).size();
74  // CHECK that we have a index for the second submap.
75  const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]);
76  CHECK_EQ(second_submap_id.trajectory_id, trajectory_id);
77  CHECK_LE(second_submap_id.submap_index, next_submap_index);
78  // Extrapolate if necessary.
79  if (second_submap_id.submap_index == next_submap_index) {
80  const auto& first_submap_pose =
81  submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
83  trajectory_id, first_submap_pose *
84  insertion_submaps[0]->local_pose().inverse() *
85  insertion_submaps[1]->local_pose());
86  }
87 }
88 
90  common::Time time, const sensor::RangeData& range_data_in_tracking,
91  const transform::Rigid3d& pose, const int trajectory_id,
92  const Submap* const matching_submap,
93  const std::vector<const Submap*>& insertion_submaps) {
94  const transform::Rigid3d optimized_pose(
95  GetLocalToGlobalTransform(trajectory_id) * pose);
96 
97  common::MutexLocker locker(&mutex_);
98  trajectory_nodes_.Append(
99  trajectory_id,
101  std::make_shared<const mapping::TrajectoryNode::Data>(
103  time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
104  sensor::Compress(range_data_in_tracking),
106  optimized_pose});
107  ++num_trajectory_nodes_;
108  trajectory_connectivity_.Add(trajectory_id);
109 
110  if (submap_ids_.count(insertion_submaps.back()) == 0) {
111  const mapping::SubmapId submap_id =
112  submap_data_.Append(trajectory_id, SubmapData());
113  submap_ids_.emplace(insertion_submaps.back(), submap_id);
114  submap_data_.at(submap_id).submap = insertion_submaps.back();
115  }
116  const Submap* const finished_submap = insertion_submaps.front()->finished()
117  ? insertion_submaps.front()
118  : nullptr;
119 
120  // Make sure we have a sampler for this trajectory.
121  if (!global_localization_samplers_[trajectory_id]) {
122  global_localization_samplers_[trajectory_id] =
123  common::make_unique<common::FixedRatioSampler>(
124  options_.global_sampling_ratio());
125  }
126 
127  AddWorkItem([=]() REQUIRES(mutex_) {
128  ComputeConstraintsForScan(matching_submap, insertion_submaps,
129  finished_submap, pose);
130  });
131 }
132 
133 void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
134  if (scan_queue_ == nullptr) {
135  work_item();
136  } else {
137  scan_queue_->push_back(work_item);
138  }
139 }
140 
141 void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
142  const Eigen::Vector3d& linear_acceleration,
143  const Eigen::Vector3d& angular_velocity) {
144  common::MutexLocker locker(&mutex_);
145  AddWorkItem([=]() REQUIRES(mutex_) {
146  optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
147  angular_velocity);
148  });
149 }
150 
152  const mapping::SubmapId& submap_id) {
153  CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
154 
155  const transform::Rigid3d inverse_submap_pose =
157  .at(submap_id.trajectory_id)
158  .at(submap_id.submap_index)
159  .pose.inverse();
160 
161  const transform::Rigid3d initial_relative_pose =
162  inverse_submap_pose * optimization_problem_.node_data()
163  .at(node_id.trajectory_id)
164  .at(node_id.node_index)
165  .point_cloud_pose;
166 
167  std::vector<mapping::TrajectoryNode> submap_nodes;
168  for (const mapping::NodeId& submap_node_id :
169  submap_data_.at(submap_id).node_ids) {
170  submap_nodes.push_back(mapping::TrajectoryNode{
171  trajectory_nodes_.at(submap_node_id).constant_data,
172  inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose});
173  }
174 
175  // Only globally match against submaps not in this trajectory.
176  if (node_id.trajectory_id != submap_id.trajectory_id &&
177  global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
178  // In this situation, 'initial_relative_pose' is:
179  //
180  // submap <- global map 2 <- global map 1 <- tracking
181  // (agreeing on gravity)
182  //
183  // Since they possibly came from two disconnected trajectories, the only
184  // guaranteed connection between the tracking and the submap frames is
185  // an agreement on the direction of gravity. Therefore, excluding yaw,
186  // 'initial_relative_pose.rotation()' is a good estimate of the relative
187  // orientation of the point cloud in the submap frame. Finding the correct
188  // yaw component will be handled by the matching procedure in the
189  // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
190  constraint_builder_.MaybeAddGlobalConstraint(
191  submap_id, submap_data_.at(submap_id).submap, node_id,
192  &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
193  submap_nodes, initial_relative_pose.rotation(),
194  &trajectory_connectivity_);
195  } else {
196  const bool scan_and_submap_trajectories_connected =
197  reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
198  reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
201  if (node_id.trajectory_id == submap_id.trajectory_id ||
202  scan_and_submap_trajectories_connected) {
203  constraint_builder_.MaybeAddConstraint(
204  submap_id, submap_data_.at(submap_id).submap, node_id,
205  &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
206  submap_nodes, initial_relative_pose);
207  }
208  }
209 }
210 
212  const auto submap_id = GetSubmapId(submap);
213  const auto& submap_data = submap_data_.at(submap_id);
214 
215  const auto& node_data = optimization_problem_.node_data();
216  for (size_t trajectory_id = 0; trajectory_id != node_data.size();
217  ++trajectory_id) {
218  for (size_t node_index = 0; node_index != node_data[trajectory_id].size();
219  ++node_index) {
220  const mapping::NodeId node_id{static_cast<int>(trajectory_id),
221  static_cast<int>(node_index)};
222  if (submap_data.node_ids.count(node_id) == 0) {
223  ComputeConstraint(node_id, submap_id);
224  }
225  }
226  }
227 }
228 
230  const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
231  const Submap* finished_submap, const transform::Rigid3d& pose) {
232  GrowSubmapTransformsAsNeeded(insertion_submaps);
233  const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
234  const transform::Rigid3d optimized_pose =
236  .at(matching_id.trajectory_id)
237  .at(matching_id.submap_index)
238  .pose *
239  matching_submap->local_pose().inverse() * pose;
240  const mapping::NodeId node_id{
241  matching_id.trajectory_id,
242  static_cast<size_t>(matching_id.trajectory_id) <
244  ? static_cast<int>(optimization_problem_.node_data()
245  .at(matching_id.trajectory_id)
246  .size())
247  : 0};
248  const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
250  scan_data->time, optimized_pose);
251  for (const Submap* submap : insertion_submaps) {
252  const mapping::SubmapId submap_id = GetSubmapId(submap);
253  CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
254  submap_data_.at(submap_id).node_ids.emplace(node_id);
255  const transform::Rigid3d constraint_transform =
256  submap->local_pose().inverse() * pose;
257  constraints_.push_back(
258  Constraint{submap_id,
259  node_id,
260  {constraint_transform, options_.matcher_translation_weight(),
261  options_.matcher_rotation_weight()},
263  }
264 
265  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
266  ++trajectory_id) {
267  for (int submap_index = 0;
268  submap_index < submap_data_.num_indices(trajectory_id);
269  ++submap_index) {
270  const mapping::SubmapId submap_id{trajectory_id, submap_index};
271  if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
272  CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
273  ComputeConstraint(node_id, submap_id);
274  }
275  }
276  }
277 
278  if (finished_submap != nullptr) {
279  const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
280  SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
281  CHECK(finished_submap_data.state == SubmapState::kActive);
282  finished_submap_data.state = SubmapState::kFinished;
283  // We have a new completed submap, so we look into adding constraints for
284  // old scans.
285  ComputeConstraintsForOldScans(finished_submap);
286  }
287  constraint_builder_.NotifyEndOfScan();
288  ++num_scans_since_last_loop_closure_;
289  if (options_.optimize_every_n_scans() > 0 &&
290  num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
291  CHECK(!run_loop_closure_);
292  run_loop_closure_ = true;
293  // If there is a 'scan_queue_' already, some other thread will take care.
294  if (scan_queue_ == nullptr) {
295  scan_queue_ = common::make_unique<std::deque<std::function<void()>>>();
296  HandleScanQueue();
297  }
298  }
299 }
300 
302  constraint_builder_.WhenDone(
303  [this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
304  {
305  common::MutexLocker locker(&mutex_);
306  constraints_.insert(constraints_.end(), result.begin(), result.end());
307  }
308  RunOptimization();
309 
310  common::MutexLocker locker(&mutex_);
311  num_scans_since_last_loop_closure_ = 0;
312  run_loop_closure_ = false;
313  while (!run_loop_closure_) {
314  if (scan_queue_->empty()) {
315  LOG(INFO) << "We caught up. Hooray!";
316  scan_queue_.reset();
317  return;
318  }
319  scan_queue_->front()();
320  scan_queue_->pop_front();
321  }
322  // We have to optimize again.
323  HandleScanQueue();
324  });
325 }
326 
328  bool notification = false;
329  common::MutexLocker locker(&mutex_);
330  const int num_finished_scans_at_start =
331  constraint_builder_.GetNumFinishedScans();
332  while (!locker.AwaitWithTimeout(
333  [this]() REQUIRES(mutex_) {
334  return constraint_builder_.GetNumFinishedScans() ==
335  num_trajectory_nodes_;
336  },
337  common::FromSeconds(1.))) {
338  std::ostringstream progress_info;
339  progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
340  << 100. *
341  (constraint_builder_.GetNumFinishedScans() -
342  num_finished_scans_at_start) /
343  (num_trajectory_nodes_ - num_finished_scans_at_start)
344  << "%...";
345  std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
346  }
347  std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
348  constraint_builder_.WhenDone(
349  [this, &notification](
351  common::MutexLocker locker(&mutex_);
352  constraints_.insert(constraints_.end(), result.begin(), result.end());
353  notification = true;
354  });
355  locker.Await([&notification]() { return notification; });
356 }
357 
361  options_.max_num_final_iterations());
362  RunOptimization();
364  options_.optimization_problem_options()
365  .ceres_solver_options()
366  .max_num_iterations());
367 }
368 
370  if (optimization_problem_.submap_data().empty()) {
371  return;
372  }
373  optimization_problem_.Solve(constraints_);
374  common::MutexLocker locker(&mutex_);
375 
376  const auto& node_data = optimization_problem_.node_data();
377  for (int trajectory_id = 0;
378  trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
379  int node_index = 0;
380  const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
381  for (; node_index != static_cast<int>(node_data[trajectory_id].size());
382  ++node_index) {
383  const mapping::NodeId node_id{trajectory_id, node_index};
384  trajectory_nodes_.at(node_id).pose =
385  node_data[trajectory_id][node_index].point_cloud_pose;
386  }
387  // Extrapolate all point cloud poses that were added later.
388  const auto local_to_new_global = ComputeLocalToGlobalTransform(
389  optimization_problem_.submap_data(), trajectory_id);
390  const auto local_to_old_global = ComputeLocalToGlobalTransform(
391  optimized_submap_transforms_, trajectory_id);
392  const transform::Rigid3d old_global_to_new_global =
393  local_to_new_global * local_to_old_global.inverse();
394  for (; node_index < num_nodes; ++node_index) {
395  const mapping::NodeId node_id{trajectory_id, node_index};
396  trajectory_nodes_.at(node_id).pose =
397  old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
398  }
399  }
400  optimized_submap_transforms_ = optimization_problem_.submap_data();
401  connected_components_ = trajectory_connectivity_.ConnectedComponents();
403  for (size_t i = 0; i != connected_components_.size(); ++i) {
404  for (const int trajectory_id : connected_components_[i]) {
405  reverse_connected_components_.emplace(trajectory_id, i);
406  }
407  }
408 }
409 
410 std::vector<std::vector<mapping::TrajectoryNode>>
412  common::MutexLocker locker(&mutex_);
413  return trajectory_nodes_.data();
414 }
415 
416 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
417  common::MutexLocker locker(&mutex_);
418  return constraints_;
419 }
420 
422  const int trajectory_id) {
423  common::MutexLocker locker(&mutex_);
424  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
425  trajectory_id);
426 }
427 
428 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
429  common::MutexLocker locker(&mutex_);
430  return connected_components_;
431 }
432 
433 int SparsePoseGraph::num_submaps(const int trajectory_id) {
434  common::MutexLocker locker(&mutex_);
435  if (trajectory_id >= submap_data_.num_trajectories()) {
436  return 0;
437  }
438  return submap_data_.num_indices(trajectory_id);
439 }
440 
442  const mapping::SubmapId& submap_id) {
443  common::MutexLocker locker(&mutex_);
444  // We already have an optimized pose.
445  if (submap_id.trajectory_id <
446  static_cast<int>(optimized_submap_transforms_.size()) &&
447  submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
448  .at(submap_id.trajectory_id)
449  .size())) {
450  return optimized_submap_transforms_.at(submap_id.trajectory_id)
451  .at(submap_id.submap_index)
452  .pose;
453  }
454  // We have to extrapolate.
455  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
456  submap_id.trajectory_id) *
457  submap_data_.at(submap_id).submap->local_pose();
458 }
459 
461  const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
462  submap_transforms,
463  const int trajectory_id) const {
464  if (trajectory_id >= static_cast<int>(submap_transforms.size()) ||
465  submap_transforms.at(trajectory_id).empty()) {
467  }
468  const mapping::SubmapId last_optimized_submap_id{
469  trajectory_id,
470  static_cast<int>(submap_transforms.at(trajectory_id).size() - 1)};
471  // Accessing 'local_pose' in Submap is okay, since the member is const.
472  return submap_transforms.at(trajectory_id).back().pose *
473  submap_data_.at(last_optimized_submap_id)
474  .submap->local_pose()
475  .inverse();
476 }
477 
478 } // namespace mapping_3d
479 } // namespace cartographer
void GrowSubmapTransformsAsNeeded(const std::vector< const Submap * > &insertion_submaps) REQUIRES(mutex_)
proto::RangeDataInserterOptions options_
const Quaternion & rotation() const
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_)
std::shared_ptr< const Data > constant_data
std::vector< Constraint > constraints() override EXCLUDES(mutex_)
transform::Rigid3d local_pose() const
Definition: submaps.h:65
void ComputeConstraint(const mapping::NodeId &node_id, const mapping::SubmapId &submap_id) REQUIRES(mutex_)
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d pose
std::vector< std::vector< int > > connected_components_
Duration FromSeconds(const double seconds)
Definition: time.cc:24
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override
void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid3d &point_cloud_pose)
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
#define REQUIRES(...)
Definition: mutex.h:44
OptimizationProblem optimization_problem_
CompressedRangeData Compress(const RangeData &range_data)
Definition: range_data.cc:56
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
const bool finished() const
Definition: 3d/submaps.h:61
Mutex::Locker MutexLocker
Definition: mutex.h:95
void ComputeConstraintsForOldScans(const Submap *submap) REQUIRES(mutex_)
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override
sparse_pose_graph::OptimizationProblem optimization_problem_
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:58