Tiny cleanup of TransformInterpolationBuffer. (#462)

cartographer/ground_truth/compute_relations_metrics_main.cc

@@ -119,9 +119,8 @@ void Run(const string& pose_graph_filename, const string& relations_filename,
     CHECK_EQ(pose_graph.trajectory_size(), 1)
         << "Only pose graphs containing a single trajectory are supported.";
   }
-  const auto transform_interpolation_buffer =
+  const transform::TransformInterpolationBuffer transform_interpolation_buffer(
-      transform::TransformInterpolationBuffer::FromTrajectory(
+      pose_graph.trajectory(0));
-          pose_graph.trajectory(0));
 
   proto::GroundTruth ground_truth;
   if (read_text_file_with_unix_timestamps) {
@@ -136,9 +135,9 @@ void Run(const string& pose_graph_filename, const string& relations_filename,
 
   std::vector<Error> errors;
   for (const auto& relation : ground_truth.relation()) {
-    const auto pose1 = transform_interpolation_buffer->Lookup(
+    const auto pose1 = transform_interpolation_buffer.Lookup(
         common::FromUniversal(relation.timestamp1()));
-    const auto pose2 = transform_interpolation_buffer->Lookup(
+    const auto pose2 = transform_interpolation_buffer.Lookup(
         common::FromUniversal(relation.timestamp2()));
     const transform::Rigid3d expected =
         transform::ToRigid3(relation.expected());

cartographer/transform/transform_interpolation_buffer.cc

@@ -20,23 +20,30 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#include "cartographer/common/make_unique.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
 namespace transform {
 
+TransformInterpolationBuffer::TransformInterpolationBuffer(
+    const mapping::proto::Trajectory& trajectory) {
+  for (const mapping::proto::Trajectory::Node& node : trajectory.node()) {
+    Push(common::FromUniversal(node.timestamp()),
+         transform::ToRigid3(node.pose()));
+  }
+}
+
 void TransformInterpolationBuffer::Push(const common::Time time,
                                         const transform::Rigid3d& transform) {
-  if (!deque_.empty()) {
+  if (!timestamped_transforms_.empty()) {
     CHECK_GE(time, latest_time()) << "New transform is older than latest.";
   }
-  deque_.push_back(TimestampedTransform{time, transform});
+  timestamped_transforms_.push_back(TimestampedTransform{time, transform});
 }
 
 bool TransformInterpolationBuffer::Has(const common::Time time) const {
-  if (deque_.empty()) {
+  if (timestamped_transforms_.empty()) {
     return false;
   }
   return earliest_time() <= time && time <= latest_time();
@@ -45,13 +52,13 @@ 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 =
+  auto start = std::lower_bound(
-      std::lower_bound(deque_.begin(), deque_.end(), time,
+      timestamped_transforms_.begin(), timestamped_transforms_.end(), time,
-                       [](const TimestampedTransform& timestamped_transform,
+      [](const TimestampedTransform& timestamped_transform,
-                          const common::Time time) {
+         const common::Time time) {
-                         return timestamped_transform.time < time;
+        return timestamped_transform.time < time;
-                       });
+      });
-  auto end = start;
+  const auto end = start;
   if (end->time == time) {
     return end->transform;
   }
@@ -73,26 +80,16 @@ transform::Rigid3d TransformInterpolationBuffer::Lookup(
 
 common::Time TransformInterpolationBuffer::earliest_time() const {
   CHECK(!empty()) << "Empty buffer.";
-  return deque_.front().time;
+  return timestamped_transforms_.front().time;
 }
 
 common::Time TransformInterpolationBuffer::latest_time() const {
   CHECK(!empty()) << "Empty buffer.";
-  return deque_.back().time;
+  return timestamped_transforms_.back().time;
 }
 
-bool TransformInterpolationBuffer::empty() const { return deque_.empty(); }
+bool TransformInterpolationBuffer::empty() const {
-
+  return timestamped_transforms_.empty();
-std::unique_ptr<TransformInterpolationBuffer>
-TransformInterpolationBuffer::FromTrajectory(
-    const mapping::proto::Trajectory& trajectory) {
-  auto interpolation_buffer =
-      common::make_unique<TransformInterpolationBuffer>();
-  for (const mapping::proto::Trajectory::Node& node : trajectory.node()) {
-    interpolation_buffer->Push(common::FromUniversal(node.timestamp()),
-                               transform::ToRigid3(node.pose()));
-  }
-  return interpolation_buffer;
 }
 
 }  // namespace transform

cartographer/transform/transform_interpolation_buffer.h

@@ -17,8 +17,7 @@
 #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
 #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
 
-#include <deque>
+#include <vector>
-#include <memory>
 
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
@@ -30,7 +29,8 @@ namespace transform {
 // A time-ordered buffer of transforms that supports interpolated lookups.
 class TransformInterpolationBuffer {
  public:
-  static std::unique_ptr<TransformInterpolationBuffer> FromTrajectory(
+  TransformInterpolationBuffer() = default;
+  explicit TransformInterpolationBuffer(
       const mapping::proto::Trajectory& trajectory);
 
   // Adds a new transform to the buffer and removes the oldest transform if the
@@ -61,7 +61,7 @@ class TransformInterpolationBuffer {
     transform::Rigid3d transform;
   };
 
-  std::deque<TimestampedTransform> deque_;
+  std::vector<TimestampedTransform> timestamped_transforms_;
 };
 
 }  // namespace transform