From e10650910e157bd68c55f4d1bc6603f25f89d293 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 30 Oct 2017 15:41:02 +0100 Subject: [PATCH] Pull out time-based transformation interpolation. (#615) --- .../transform/timestamped_transform.cc | 40 +++++++++++++++++++ .../transform/timestamped_transform.h | 38 ++++++++++++++++++ .../transform_interpolation_buffer.cc | 19 ++------- .../transform_interpolation_buffer.h | 6 +-- .../transform_interpolation_buffer_test.cc | 8 ++++ 5 files changed, 90 insertions(+), 21 deletions(-) create mode 100644 cartographer/transform/timestamped_transform.cc create mode 100644 cartographer/transform/timestamped_transform.h diff --git a/cartographer/transform/timestamped_transform.cc b/cartographer/transform/timestamped_transform.cc new file mode 100644 index 0000000..4901ae2 --- /dev/null +++ b/cartographer/transform/timestamped_transform.cc @@ -0,0 +1,40 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace transform { + +TimestampedTransform Interpolate(const TimestampedTransform& start, + const TimestampedTransform& end, + const common::Time time) { + CHECK_LE(start.time, time); + CHECK_GE(end.time, time); + + const double duration = common::ToSeconds(end.time - start.time); + const double factor = common::ToSeconds(time - start.time) / duration; + const Eigen::Vector3d origin = + start.transform.translation() + + (end.transform.translation() - start.transform.translation()) * factor; + const Eigen::Quaterniond rotation = + Eigen::Quaterniond(start.transform.rotation()) + .slerp(factor, Eigen::Quaterniond(end.transform.rotation())); + return TimestampedTransform{time, transform::Rigid3d(origin, rotation)}; +} + +} // namespace transform +} // namespace cartographer diff --git a/cartographer/transform/timestamped_transform.h b/cartographer/transform/timestamped_transform.h new file mode 100644 index 0000000..04f2709 --- /dev/null +++ b/cartographer/transform/timestamped_transform.h @@ -0,0 +1,38 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ + +#include "cartographer/common/time.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace transform { + +struct TimestampedTransform { + common::Time time; + transform::Rigid3d transform; +}; + +TimestampedTransform Interpolate(const TimestampedTransform& start, + const TimestampedTransform& end, + const common::Time time); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TIMESTAMPED_TRANSFORM_H_ diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc index 94a71c0..7d9c3d7 100644 --- a/cartographer/transform/transform_interpolation_buffer.cc +++ b/cartographer/transform/transform_interpolation_buffer.cc @@ -52,30 +52,17 @@ bool TransformInterpolationBuffer::Has(const common::Time time) const { transform::Rigid3d TransformInterpolationBuffer::Lookup( const common::Time time) const { CHECK(Has(time)) << "Missing transform for: " << time; - auto start = std::lower_bound( + const auto end = std::lower_bound( timestamped_transforms_.begin(), timestamped_transforms_.end(), time, [](const TimestampedTransform& timestamped_transform, const common::Time time) { return timestamped_transform.time < time; }); - const auto end = start; if (end->time == time) { return end->transform; } - --start; - if (start->time == time) { - return start->transform; - } - - const double duration = common::ToSeconds(end->time - start->time); - const double factor = common::ToSeconds(time - start->time) / duration; - const Eigen::Vector3d origin = - start->transform.translation() + - (end->transform.translation() - start->transform.translation()) * factor; - const Eigen::Quaterniond rotation = - Eigen::Quaterniond(start->transform.rotation()) - .slerp(factor, Eigen::Quaterniond(end->transform.rotation())); - return transform::Rigid3d(origin, rotation); + const auto start = std::prev(end); + return Interpolate(*start, *end, time).transform; } common::Time TransformInterpolationBuffer::earliest_time() const { diff --git a/cartographer/transform/transform_interpolation_buffer.h b/cartographer/transform/transform_interpolation_buffer.h index e21f7d4..4dc51bb 100644 --- a/cartographer/transform/transform_interpolation_buffer.h +++ b/cartographer/transform/transform_interpolation_buffer.h @@ -22,6 +22,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" namespace cartographer { namespace transform { @@ -56,11 +57,6 @@ class TransformInterpolationBuffer { bool empty() const; private: - struct TimestampedTransform { - common::Time time; - transform::Rigid3d transform; - }; - std::vector timestamped_transforms_; }; diff --git a/cartographer/transform/transform_interpolation_buffer_test.cc b/cartographer/transform/transform_interpolation_buffer_test.cc index d26b0b0..193f95e 100644 --- a/cartographer/transform/transform_interpolation_buffer_test.cc +++ b/cartographer/transform/transform_interpolation_buffer_test.cc @@ -62,6 +62,14 @@ TEST(TransformInterpolationBufferTest, testLookup) { 1e-6)); } +TEST(TransformInterpolationBufferTest, testLookupSingleTransform) { + TransformInterpolationBuffer buffer; + const common::Time time = common::FromUniversal(75); + buffer.Push(time, transform::Rigid3d::Identity()); + const transform::Rigid3d interpolated = buffer.Lookup(time); + EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6)); +} + } // namespace } // namespace transform } // namespace cartographer