diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc index a446c01..5278e60 100644 --- a/cartographer/ground_truth/compute_relations_metrics_main.cc +++ b/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 = - transform::TransformInterpolationBuffer::FromTrajectory( - pose_graph.trajectory(0)); + const transform::TransformInterpolationBuffer transform_interpolation_buffer( + 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 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()); diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc index 4c1e8aa..94a71c0 100644 --- a/cartographer/transform/transform_interpolation_buffer.cc +++ b/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 = - std::lower_bound(deque_.begin(), deque_.end(), time, - [](const TimestampedTransform& timestamped_transform, - const common::Time time) { - return timestamped_transform.time < time; - }); - auto end = start; + auto start = 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; } @@ -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(); } - -std::unique_ptr -TransformInterpolationBuffer::FromTrajectory( - const mapping::proto::Trajectory& trajectory) { - auto interpolation_buffer = - common::make_unique(); - for (const mapping::proto::Trajectory::Node& node : trajectory.node()) { - interpolation_buffer->Push(common::FromUniversal(node.timestamp()), - transform::ToRigid3(node.pose())); - } - return interpolation_buffer; +bool TransformInterpolationBuffer::empty() const { + return timestamped_transforms_.empty(); } } // namespace transform diff --git a/cartographer/transform/transform_interpolation_buffer.h b/cartographer/transform/transform_interpolation_buffer.h index d532527..e21f7d4 100644 --- a/cartographer/transform/transform_interpolation_buffer.h +++ b/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 -#include +#include #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 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 deque_; + std::vector timestamped_transforms_; }; } // namespace transform