optimizing_local_trajectory_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 
22 #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
25 #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
30 #include "glog/logging.h"
31 
32 namespace cartographer {
33 namespace mapping_3d {
34 
35 namespace {
36 
37 // Computes the cost of differences between two velocities. For the constant
38 // velocity model the residuals are just the vector difference.
39 class VelocityDeltaCostFunctor {
40  public:
41  explicit VelocityDeltaCostFunctor(const double scaling_factor)
42  : scaling_factor_(scaling_factor) {}
43 
44  VelocityDeltaCostFunctor(const VelocityDeltaCostFunctor&) = delete;
45  VelocityDeltaCostFunctor& operator=(const VelocityDeltaCostFunctor&) = delete;
46 
47  template <typename T>
48  bool operator()(const T* const old_velocity, const T* const new_velocity,
49  T* residual) const {
50  residual[0] = scaling_factor_ * (new_velocity[0] - old_velocity[0]);
51  residual[1] = scaling_factor_ * (new_velocity[1] - old_velocity[1]);
52  residual[2] = scaling_factor_ * (new_velocity[2] - old_velocity[2]);
53  return true;
54  }
55 
56  private:
57  const double scaling_factor_;
58 };
59 
60 class RelativeTranslationAndYawCostFunction {
61  public:
62  RelativeTranslationAndYawCostFunction(const double translation_scaling_factor,
63  const double rotation_scaling_factor,
64  const transform::Rigid3d& delta)
65  : translation_scaling_factor_(translation_scaling_factor),
66  rotation_scaling_factor_(rotation_scaling_factor),
67  delta_(delta) {}
68 
69  RelativeTranslationAndYawCostFunction(
70  const RelativeTranslationAndYawCostFunction&) = delete;
71  RelativeTranslationAndYawCostFunction& operator=(
72  const RelativeTranslationAndYawCostFunction&) = delete;
73 
74  template <typename T>
75  bool operator()(const T* const start_translation,
76  const T* const start_rotation, const T* const end_translation,
77  const T* const end_rotation, T* residual) const {
78  const transform::Rigid3<T> start(
79  Eigen::Map<const Eigen::Matrix<T, 3, 1>>(start_translation),
80  Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
81  start_rotation[2], start_rotation[3]));
82  const transform::Rigid3<T> end(
83  Eigen::Map<const Eigen::Matrix<T, 3, 1>>(end_translation),
84  Eigen::Quaternion<T>(end_rotation[0], end_rotation[1], end_rotation[2],
85  end_rotation[3]));
86 
87  const transform::Rigid3<T> delta = end.inverse() * start;
88  const transform::Rigid3<T> error = delta.inverse() * delta_.cast<T>();
89  residual[0] = translation_scaling_factor_ * error.translation().x();
90  residual[1] = translation_scaling_factor_ * error.translation().y();
91  residual[2] = translation_scaling_factor_ * error.translation().z();
92  residual[3] = rotation_scaling_factor_ * transform::GetYaw(error);
93  return true;
94  }
95 
96  private:
100 };
101 
102 } // namespace
103 
105  const proto::LocalTrajectoryBuilderOptions& options)
106  : options_(options),
107  ceres_solver_options_(common::CreateCeresSolverOptions(
108  options.ceres_scan_matcher_options().ceres_solver_options())),
109  submaps_(common::make_unique<Submaps>(options.submaps_options())),
110  num_accumulated_(0),
111  motion_filter_(options.motion_filter_options()) {}
112 
114 
116  return submaps_.get();
117 }
118 
120  const common::Time time, const Eigen::Vector3d& linear_acceleration,
121  const Eigen::Vector3d& angular_velocity) {
122  imu_data_.push_back(ImuData{
123  time, linear_acceleration, angular_velocity,
124  });
126 }
127 
129  const common::Time time, const transform::Rigid3d& pose) {
130  odometer_data_.push_back(OdometerData{time, pose});
132 }
133 
134 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
136  const common::Time time, const Eigen::Vector3f& origin,
137  const sensor::PointCloud& ranges) {
138  CHECK_GT(ranges.size(), 0);
139 
140  // TODO(hrapp): Handle misses.
141  // TODO(hrapp): Where are NaNs in range_data_in_tracking coming from?
142  sensor::PointCloud point_cloud;
143  for (const Eigen::Vector3f& hit : ranges) {
144  const Eigen::Vector3f delta = hit - origin;
145  const float range = delta.norm();
146  if (range >= options_.min_range()) {
147  if (range <= options_.max_range()) {
148  point_cloud.push_back(hit);
149  }
150  }
151  }
152 
153  auto high_resolution_options =
154  options_.high_resolution_adaptive_voxel_filter_options();
155  high_resolution_options.set_min_num_points(
156  high_resolution_options.min_num_points() /
157  options_.scans_per_accumulation());
158  sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter(
159  high_resolution_options);
160  const sensor::PointCloud high_resolution_filtered_points =
161  high_resolution_adaptive_voxel_filter.Filter(point_cloud);
162 
163  auto low_resolution_options =
164  options_.low_resolution_adaptive_voxel_filter_options();
165  low_resolution_options.set_min_num_points(
166  low_resolution_options.min_num_points() /
167  options_.scans_per_accumulation());
168  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
169  low_resolution_options);
170  const sensor::PointCloud low_resolution_filtered_points =
171  low_resolution_adaptive_voxel_filter.Filter(point_cloud);
172 
173  if (batches_.empty()) {
174  // First rangefinder data ever. Initialize to the origin.
175  batches_.push_back(
176  Batch{time, point_cloud, high_resolution_filtered_points,
177  low_resolution_filtered_points,
178  State(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity(),
179  Eigen::Vector3d::Zero())});
180  } else {
181  const Batch& last_batch = batches_.back();
182  batches_.push_back(Batch{
183  time, point_cloud, high_resolution_filtered_points,
184  low_resolution_filtered_points,
185  PredictState(last_batch.state, last_batch.time, time),
186  });
187  }
189 
191  return MaybeOptimize(time);
192 }
193 
195  if (imu_data_.empty()) {
196  batches_.clear();
197  return;
198  }
199 
200  while (batches_.size() >
201  static_cast<size_t>(options_.scans_per_accumulation())) {
202  batches_.pop_front();
203  }
204 
205  while (imu_data_.size() > 1 &&
206  (batches_.empty() || imu_data_[1].time <= batches_.front().time)) {
207  imu_data_.pop_front();
208  }
209 
210  while (
211  odometer_data_.size() > 1 &&
212  (batches_.empty() || odometer_data_[1].time <= batches_.front().time)) {
213  odometer_data_.pop_front();
214  }
215 }
216 
218  const transform::Rigid3d& transform) {
219  for (Batch& batch : batches_) {
220  const transform::Rigid3d new_pose = transform * batch.state.ToRigid();
221  const auto& velocity = batch.state.velocity;
222  const Eigen::Vector3d new_velocity =
223  transform.rotation() *
224  Eigen::Vector3d(velocity[0], velocity[1], velocity[2]);
225  batch.state =
226  State(new_pose.translation(), new_pose.rotation(), new_velocity);
227  }
228 }
229 
230 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
232  // TODO(hrapp): Make the number of optimizations configurable.
233  if (num_accumulated_ < options_.scans_per_accumulation() &&
234  num_accumulated_ % 10 != 0) {
235  return nullptr;
236  }
237 
238  ceres::Problem problem;
239  const Submap* const matching_submap =
240  submaps_->Get(submaps_->matching_index());
241  // We transform the states in 'batches_' in place to be in the submap frame as
242  // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
243  // the optimization problem.
244  TransformStates(matching_submap->local_pose().inverse());
245  for (size_t i = 0; i < batches_.size(); ++i) {
246  Batch& batch = batches_[i];
247  problem.AddResidualBlock(
248  new ceres::AutoDiffCostFunction<scan_matching::OccupiedSpaceCostFunctor,
249  ceres::DYNAMIC, 3, 4>(
251  options_.optimizing_local_trajectory_builder_options()
252  .high_resolution_grid_weight() /
253  std::sqrt(static_cast<double>(
254  batch.high_resolution_filtered_points.size())),
256  matching_submap->high_resolution_hybrid_grid()),
257  batch.high_resolution_filtered_points.size()),
258  nullptr, batch.state.translation.data(), batch.state.rotation.data());
259  problem.AddResidualBlock(
260  new ceres::AutoDiffCostFunction<scan_matching::OccupiedSpaceCostFunctor,
261  ceres::DYNAMIC, 3, 4>(
263  options_.optimizing_local_trajectory_builder_options()
264  .low_resolution_grid_weight() /
265  std::sqrt(static_cast<double>(
266  batch.low_resolution_filtered_points.size())),
268  matching_submap->low_resolution_hybrid_grid()),
269  batch.low_resolution_filtered_points.size()),
270  nullptr, batch.state.translation.data(), batch.state.rotation.data());
271 
272  if (i == 0) {
273  problem.SetParameterBlockConstant(batch.state.translation.data());
274  problem.SetParameterBlockConstant(batch.state.rotation.data());
275  problem.AddParameterBlock(batch.state.velocity.data(), 3);
276  problem.SetParameterBlockConstant(batch.state.velocity.data());
277  } else {
278  problem.SetParameterization(batch.state.rotation.data(),
279  new ceres::QuaternionParameterization());
280  }
281  }
282 
283  auto it = imu_data_.cbegin();
284  for (size_t i = 1; i < batches_.size(); ++i) {
285  problem.AddResidualBlock(
286  new ceres::AutoDiffCostFunction<VelocityDeltaCostFunctor, 3, 3, 3>(
287  new VelocityDeltaCostFunctor(
288  options_.optimizing_local_trajectory_builder_options()
289  .velocity_weight())),
290  nullptr, batches_[i - 1].state.velocity.data(),
291  batches_[i].state.velocity.data());
292 
293  problem.AddResidualBlock(
294  new ceres::AutoDiffCostFunction<TranslationCostFunction, 3, 3, 3, 3>(
296  options_.optimizing_local_trajectory_builder_options()
297  .translation_weight(),
298  common::ToSeconds(batches_[i].time - batches_[i - 1].time))),
299  nullptr, batches_[i - 1].state.translation.data(),
300  batches_[i].state.translation.data(),
301  batches_[i - 1].state.velocity.data());
302 
303  const IntegrateImuResult<double> result =
304  IntegrateImu(imu_data_, batches_[i - 1].time, batches_[i].time, &it);
305  problem.AddResidualBlock(
306  new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
308  options_.optimizing_local_trajectory_builder_options()
309  .rotation_weight(),
310  result.delta_rotation)),
311  nullptr, batches_[i - 1].state.rotation.data(),
312  batches_[i].state.rotation.data());
313  }
314 
315  if (odometer_data_.size() > 1) {
316  transform::TransformInterpolationBuffer interpolation_buffer;
317  for (const auto& odometer_data : odometer_data_) {
318  interpolation_buffer.Push(odometer_data.time, odometer_data.pose);
319  }
320  for (size_t i = 1; i < batches_.size(); ++i) {
321  // Only add constraints for this range data if we have bracketing data
322  // from the odometer.
323  if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time &&
324  batches_[i].time <= interpolation_buffer.latest_time())) {
325  continue;
326  }
327  const transform::Rigid3d previous_odometer_pose =
328  interpolation_buffer.Lookup(batches_[i - 1].time);
329  const transform::Rigid3d current_odometer_pose =
330  interpolation_buffer.Lookup(batches_[i].time);
331  const transform::Rigid3d delta_pose =
332  current_odometer_pose.inverse() * previous_odometer_pose;
333  problem.AddResidualBlock(
334  new ceres::AutoDiffCostFunction<RelativeTranslationAndYawCostFunction,
335  4, 3, 4, 3, 4>(
336  new RelativeTranslationAndYawCostFunction(
337  options_.optimizing_local_trajectory_builder_options()
338  .odometry_translation_weight(),
339  options_.optimizing_local_trajectory_builder_options()
340  .odometry_rotation_weight(),
341  delta_pose)),
342  nullptr, batches_[i - 1].state.translation.data(),
343  batches_[i - 1].state.rotation.data(),
344  batches_[i].state.translation.data(),
345  batches_[i].state.rotation.data());
346  }
347  }
348 
349  ceres::Solver::Summary summary;
350  ceres::Solve(ceres_solver_options_, &problem, &summary);
351  // The optimized states in 'batches_' are in the submap frame and we transform
352  // them in place to be in the local SLAM frame again.
353  TransformStates(matching_submap->local_pose());
354  if (num_accumulated_ < options_.scans_per_accumulation()) {
355  return nullptr;
356  }
357 
358  num_accumulated_ = 0;
359 
360  const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid();
361  sensor::RangeData accumulated_range_data_in_tracking = {
362  Eigen::Vector3f::Zero(), {}, {}};
363 
364  for (const auto& batch : batches_) {
365  const transform::Rigid3f transform =
366  (optimized_pose.inverse() * batch.state.ToRigid()).cast<float>();
367  for (const Eigen::Vector3f& point : batch.points) {
368  accumulated_range_data_in_tracking.returns.push_back(transform * point);
369  }
370  }
371 
372  return AddAccumulatedRangeData(time, optimized_pose,
373  accumulated_range_data_in_tracking);
374 }
375 
376 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
378  const common::Time time, const transform::Rigid3d& optimized_pose,
379  const sensor::RangeData& range_data_in_tracking) {
380  const sensor::RangeData filtered_range_data = {
381  range_data_in_tracking.origin,
382  sensor::VoxelFiltered(range_data_in_tracking.returns,
383  options_.voxel_filter_size()),
384  sensor::VoxelFiltered(range_data_in_tracking.misses,
385  options_.voxel_filter_size())};
386 
387  if (filtered_range_data.returns.empty()) {
388  LOG(WARNING) << "Dropped empty range data.";
389  return nullptr;
390  }
391 
393  time, optimized_pose,
394  sensor::TransformPointCloud(filtered_range_data.returns,
395  optimized_pose.cast<float>())};
396 
397  return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
398 }
399 
402  return last_pose_estimate_;
403 }
404 
405 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
407  const common::Time time, const sensor::RangeData& range_data_in_tracking,
408  const transform::Rigid3d& pose_observation) {
409  if (motion_filter_.IsSimilar(time, pose_observation)) {
410  return nullptr;
411  }
412  const Submap* const matching_submap =
413  submaps_->Get(submaps_->matching_index());
414  std::vector<const Submap*> insertion_submaps;
415  for (int insertion_index : submaps_->insertion_indices()) {
416  insertion_submaps.push_back(submaps_->Get(insertion_index));
417  }
418  // TODO(whess): Use an ImuTracker to track the gravity direction.
419  const Eigen::Quaterniond kFakeGravityOrientation =
420  Eigen::Quaterniond::Identity();
421  submaps_->InsertRangeData(
422  sensor::TransformRangeData(range_data_in_tracking,
423  pose_observation.cast<float>()),
424  kFakeGravityOrientation);
425 
426  return std::unique_ptr<InsertionResult>(
427  new InsertionResult{time, range_data_in_tracking, pose_observation,
428  matching_submap, insertion_submaps});
429 }
430 
433  const common::Time start_time,
434  const common::Time end_time) {
435  auto it = --imu_data_.cend();
436  while (it->time > start_time) {
437  CHECK(it != imu_data_.cbegin());
438  --it;
439  }
440 
441  const IntegrateImuResult<double> result =
442  IntegrateImu(imu_data_, start_time, end_time, &it);
443 
444  const Eigen::Quaterniond start_rotation(
445  start_state.rotation[0], start_state.rotation[1], start_state.rotation[2],
446  start_state.rotation[3]);
447  const Eigen::Quaterniond orientation = start_rotation * result.delta_rotation;
448  const double delta_time_seconds = common::ToSeconds(end_time - start_time);
449 
450  // TODO(hrapp): IntegrateImu should integration position as well.
451  const Eigen::Vector3d position =
452  Eigen::Map<const Eigen::Vector3d>(start_state.translation.data()) +
453  delta_time_seconds *
454  Eigen::Map<const Eigen::Vector3d>(start_state.velocity.data());
455  const Eigen::Vector3d velocity =
456  Eigen::Map<const Eigen::Vector3d>(start_state.velocity.data()) +
457  start_rotation * result.delta_velocity -
458  gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ();
459 
460  return State(position, orientation, velocity);
461 }
462 
463 } // namespace mapping_3d
464 } // namespace cartographer
void AddImuData(common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity) override
proto::RangeDataInserterOptions options_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
const Quaternion & rotation() const
const double translation_scaling_factor_
std::unique_ptr< InsertionResult > InsertIntoSubmap(const common::Time time, const sensor::RangeData &range_data_in_tracking, const transform::Rigid3d &pose_observation)
bool IsSimilar(common::Time time, const transform::Rigid3d &pose)
const transform::Rigid3d delta_
Rigid3< double > Rigid3d
const HybridGrid & high_resolution_hybrid_grid() const
Definition: 3d/submaps.h:55
PointCloud VoxelFiltered(const PointCloud &point_cloud, const float size)
Definition: voxel_filter.cc:80
void Push(common::Time time, const transform::Rigid3d &transform)
State PredictState(const State &start_state, const common::Time start_time, const common::Time end_time)
std::unique_ptr< InsertionResult > AddRangefinderData(common::Time time, const Eigen::Vector3f &origin, const sensor::PointCloud &ranges) override
transform::Rigid3d local_pose() const
Definition: submaps.h:65
Rigid3< OtherType > cast() const
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
T GetYaw(const Eigen::Quaternion< T > &rotation)
Definition: transform.h:43
RangeData TransformRangeData(const RangeData &range_data, const transform::Rigid3f &transform)
Definition: range_data.cc:41
transform::Rigid3d pose
const double scaling_factor_
PointCloud Filter(const PointCloud &point_cloud) const
const HybridGrid & low_resolution_hybrid_grid() const
Definition: 3d/submaps.h:58
const double rotation_scaling_factor_
void AddOdometerData(common::Time time, const transform::Rigid3d &pose) override
std::unique_ptr< InsertionResult > AddAccumulatedRangeData(common::Time time, const transform::Rigid3d &pose_observation, const sensor::RangeData &range_data_in_tracking)
const Vector & translation() const
ceres::Solver::Options CreateCeresSolverOptions(const proto::CeresSolverOptions &proto)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
bool hit
Definition: 3d/submaps.cc:36
std::unique_ptr< InsertionResult > MaybeOptimize(common::Time time)
double ToSeconds(const Duration duration)
Definition: time.cc:29
OptimizingLocalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions &options)
_Unique_if< T >::_Single_object make_unique(Args &&...args)
Definition: make_unique.h:46
IntegrateImuResult< double > IntegrateImu(const std::deque< ImuData > &imu_data, const common::Time start_time, const common::Time end_time, std::deque< ImuData >::const_iterator *it)
std::unique_ptr< Submaps > submaps_


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