Pull out time-based transformation interpolation. (#615)

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

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_

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;
+  const auto start = std::prev(end);
-  if (start->time == time) {
+  return Interpolate(*start, *end, time).transform;
-    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);
 }
 
 common::Time TransformInterpolationBuffer::earliest_time() const {

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<TimestampedTransform> timestamped_transforms_;
 };
 

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