transform_interpolation_buffer.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 
21 #include "Eigen/Core"
22 #include "Eigen/Geometry"
25 #include "glog/logging.h"
26 
27 namespace cartographer {
28 namespace transform {
29 
31  const transform::Rigid3d& transform) {
32  if (deque_.size() > 0) {
33  CHECK_GE(time, latest_time()) << "New transform is older than latest.";
34  }
35  deque_.push_back(TimestampedTransform{time, transform});
36 }
37 
39  if (deque_.empty()) {
40  return false;
41  }
42  return earliest_time() <= time && time <= latest_time();
43 }
44 
46  const common::Time time) const {
47  CHECK(Has(time)) << "Missing transform for: " << time;
48  auto start =
49  std::lower_bound(deque_.begin(), deque_.end(), time,
50  [](const TimestampedTransform& timestamped_transform,
51  const common::Time time) {
52  return timestamped_transform.time < time;
53  });
54  auto end = start;
55  if (end->time == time) {
56  return end->transform;
57  }
58  --start;
59  if (start->time == time) {
60  return start->transform;
61  }
62 
63  const double duration = common::ToSeconds(end->time - start->time);
64  const double factor = common::ToSeconds(time - start->time) / duration;
65  const Eigen::Vector3d origin =
66  start->transform.translation() +
67  (end->transform.translation() - start->transform.translation()) * factor;
68  const Eigen::Quaterniond rotation =
69  Eigen::Quaterniond(start->transform.rotation())
70  .slerp(factor, Eigen::Quaterniond(end->transform.rotation()));
71  return transform::Rigid3d(origin, rotation);
72 }
73 
75  CHECK(!empty()) << "Empty buffer.";
76  return deque_.front().time;
77 }
78 
80  CHECK(!empty()) << "Empty buffer.";
81  return deque_.back().time;
82 }
83 
84 bool TransformInterpolationBuffer::empty() const { return deque_.empty(); }
85 
86 std::unique_ptr<TransformInterpolationBuffer>
88  const mapping::proto::Trajectory& trajectory) {
89  auto interpolation_buffer =
90  common::make_unique<TransformInterpolationBuffer>();
91  for (const mapping::proto::Trajectory::Node& node : trajectory.node()) {
92  interpolation_buffer->Push(common::FromUniversal(node.timestamp()),
93  transform::ToRigid3(node.pose()));
94  }
95  return interpolation_buffer;
96 }
97 
98 } // namespace transform
99 } // namespace cartographer
static std::unique_ptr< TransformInterpolationBuffer > FromTrajectory(const mapping::proto::Trajectory &trajectory)
Rigid3< double > Rigid3d
void Push(common::Time time, const transform::Rigid3d &transform)
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
transform::Rigid3d ToRigid3(const proto::Rigid3d &rigid)
Definition: transform.cc:67
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
double ToSeconds(const Duration duration)
Definition: time.cc:29


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