Tiny cleanup of TransformInterpolationBuffer. (#462)

master
Wolfgang Hess 2017-08-18 16:19:50 +02:00 committed by GitHub
parent 14d868869f
commit bcde3b45b0
3 changed files with 30 additions and 34 deletions

View File

@ -119,8 +119,7 @@ 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(
const transform::TransformInterpolationBuffer transform_interpolation_buffer(
pose_graph.trajectory(0));
proto::GroundTruth ground_truth;
@ -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());

View File

@ -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,
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;
});
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(); }
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;
bool TransformInterpolationBuffer::empty() const {
return timestamped_transforms_.empty();
}
} // namespace transform

View File

@ -17,8 +17,7 @@
#ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
#define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_
#include <deque>
#include <memory>
#include <vector>
#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