Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/transform/transform_interpolation_buffer.h"
00018
00019 #include <algorithm>
00020
00021 #include "Eigen/Core"
00022 #include "Eigen/Geometry"
00023 #include "cartographer/transform/transform.h"
00024 #include "glog/logging.h"
00025
00026 namespace cartographer {
00027 namespace transform {
00028
00029 TransformInterpolationBuffer::TransformInterpolationBuffer(
00030 const mapping::proto::Trajectory& trajectory) {
00031 for (const mapping::proto::Trajectory::Node& node : trajectory.node()) {
00032 Push(common::FromUniversal(node.timestamp()),
00033 transform::ToRigid3(node.pose()));
00034 }
00035 }
00036
00037 void TransformInterpolationBuffer::Push(const common::Time time,
00038 const transform::Rigid3d& transform) {
00039 if (!timestamped_transforms_.empty()) {
00040 CHECK_GE(time, latest_time()) << "New transform is older than latest.";
00041 }
00042 timestamped_transforms_.push_back(TimestampedTransform{time, transform});
00043 }
00044
00045 bool TransformInterpolationBuffer::Has(const common::Time time) const {
00046 if (timestamped_transforms_.empty()) {
00047 return false;
00048 }
00049 return earliest_time() <= time && time <= latest_time();
00050 }
00051
00052 transform::Rigid3d TransformInterpolationBuffer::Lookup(
00053 const common::Time time) const {
00054 CHECK(Has(time)) << "Missing transform for: " << time;
00055 const auto end = std::lower_bound(
00056 timestamped_transforms_.begin(), timestamped_transforms_.end(), time,
00057 [](const TimestampedTransform& timestamped_transform,
00058 const common::Time time) {
00059 return timestamped_transform.time < time;
00060 });
00061 if (end->time == time) {
00062 return end->transform;
00063 }
00064 const auto start = std::prev(end);
00065 return Interpolate(*start, *end, time).transform;
00066 }
00067
00068 common::Time TransformInterpolationBuffer::earliest_time() const {
00069 CHECK(!empty()) << "Empty buffer.";
00070 return timestamped_transforms_.front().time;
00071 }
00072
00073 common::Time TransformInterpolationBuffer::latest_time() const {
00074 CHECK(!empty()) << "Empty buffer.";
00075 return timestamped_transforms_.back().time;
00076 }
00077
00078 bool TransformInterpolationBuffer::empty() const {
00079 return timestamped_transforms_.empty();
00080 }
00081
00082 }
00083 }