diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 9050ea3203fa3084aa6b4fa36eccb1f789f867d0..fbdac3eb8d398055be5db4b995e2e9a029aac446 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -122,10 +122,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
TrajectoryBaseConstPtr getTrajectory() const;
TrajectoryBasePtr getTrajectory();
- FrameBaseConstPtr getPreviousFrame() const;
- FrameBaseConstPtr getNextFrame() const;
- FrameBasePtr getPreviousFrame();
- FrameBasePtr getNextFrame();
+ FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
+ FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(const unsigned int& i = 1);
+ FrameBasePtr getNextFrame(const unsigned int& i = 1);
CaptureBaseConstPtrList getCaptureList() const;
CaptureBasePtrList getCaptureList();
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 2b6c9cc6c645205f16e63db4d92bc146daacdc34..cc6a8abafc9e65062fc2a83580977b9bb4193658 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -59,10 +59,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
FrameBasePtr getLastFrame();
FrameBaseConstPtr getFirstFrame() const;
FrameBasePtr getFirstFrame();
- FrameBaseConstPtr getNextFrame(FrameBaseConstPtr) const;
- FrameBasePtr getNextFrame(FrameBasePtr);
- FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr) const;
- FrameBasePtr getPreviousFrame(FrameBasePtr);
+ FrameBaseConstPtr getNextFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getNextFrame(FrameBasePtr, const unsigned int& i = 1);
+ FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
+ FrameBasePtr getPreviousFrame(FrameBasePtr, const unsigned int& i = 1);
TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 9467e1bc735328aa16217e7e47f12eb9204f68df..c76d30896265afe23200f194f872a27e7183dd10 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -156,32 +156,32 @@ bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
-FrameBaseConstPtr FrameBase::getPreviousFrame() const
+FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return getTrajectory()->getPreviousFrame(shared_from_this());
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
}
-FrameBasePtr FrameBase::getPreviousFrame()
+FrameBasePtr FrameBase::getPreviousFrame(const unsigned int& i)
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return getTrajectory()->getPreviousFrame(shared_from_this());
+ return getTrajectory()->getPreviousFrame(shared_from_this(), i);
}
-FrameBaseConstPtr FrameBase::getNextFrame() const
+FrameBaseConstPtr FrameBase::getNextFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return getTrajectory()->getNextFrame(shared_from_this());
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
}
-FrameBasePtr FrameBase::getNextFrame()
+FrameBasePtr FrameBase::getNextFrame(const unsigned int& i)
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- return getTrajectory()->getNextFrame(shared_from_this());
+ return getTrajectory()->getNextFrame(shared_from_this(), i);
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 2e6748bcf9a77cf123cf2f35aff88223848eb993..d109846bcc45ded6add566208f05f9a6adc999a7 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -21,6 +21,7 @@
//--------LICENSE_END--------
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
+#include <iterator>
namespace wolf {
@@ -121,48 +122,52 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
return nullptr;
}
-FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame) const
+FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
{
auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_const_map_.end() or std::next(frame_it) == frame_const_map_.end())
+ if (frame_it == frame_const_map_.end() or
+ std::distance(frame_it, frame_const_map_.end()) <= i)
return nullptr;
- return std::next(frame_it)->second;
+ return std::next(frame_it,i)->second;
}
-FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame)
+FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame, const unsigned int& i)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_map_.end() or std::next(frame_it) == frame_map_.end())
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_it, frame_map_.end()) <= i)
return nullptr;
- return std::next(frame_it)->second;
+ return std::next(frame_it,i)->second;
}
-FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame) const
+FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame, const unsigned int& i) const
{
auto frame_it = frame_const_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_const_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_const_map_.end() or frame_it == frame_const_map_.begin())
+ if (frame_it == frame_const_map_.end() or
+ std::distance(frame_const_map_.begin(), frame_it) < i)
return nullptr;
- return std::prev(frame_it)->second;
+ return std::prev(frame_it,i)->second;
}
-FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame)
+FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame, const unsigned int& i)
{
auto frame_it = frame_map_.find(_frame->getTimeStamp());
WOLF_WARN_COND(frame_it == frame_map_.end(), "Frame not found in the frame map!");
- if (frame_it == frame_map_.end() or frame_it == frame_map_.begin())
+ if (frame_it == frame_map_.end() or
+ std::distance(frame_map_.begin(), frame_it) < i)
return nullptr;
- return std::prev(frame_it)->second;
+ return std::prev(frame_it,i)->second;
}
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 61276bc7755ae02071fd9bfa219260c41029bda1..3b8800bfc4cd1de2803a9ac29b7cabf9b7a088e7 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -46,10 +46,8 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
if (count_frames_ != 0)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
- FrameBasePtr remove_recent_frame = std::next(getProblem()->getTrajectory()->getFrameMap().rbegin(),
- params_swdr_->n_frames_recent)->second;
- FrameBasePtr keep_recent_frame = std::next(getProblem()->getTrajectory()->getFrameMap().rbegin(),
- params_swdr_->n_frames_recent - 1)->second;
+ FrameBasePtr remove_recent_frame = getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent);
+ FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
// compose motion captures for all processors motion
for (auto motion_provider_pair : getProblem()->getMotionProviderMap())
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index de1fc08ce7f926a380aeb04d21b33ec4dddde7b7..eb61f798d41b2aec792b864dffb39cb1cdb7d098 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -140,25 +140,15 @@ TEST(FrameBase, Frames)
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
auto F0 = FrameBase::emplace<FrameBase>(T, 0, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- P->print(2,0,0,0);
// tree is now consistent
ASSERT_TRUE(P->check(0));
@@ -180,28 +170,226 @@ TEST(FrameBase, Frames)
ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
// Next frame
- EXPECT_EQ(F1->id(), F0->getNextFrame()->id());
- EXPECT_EQ(F2->id(), F1->getNextFrame()->id());
- EXPECT_EQ(F3->id(), F2->getNextFrame()->id());
- EXPECT_EQ(F4->id(), F3->getNextFrame()->id());
- EXPECT_EQ(F5->id(), F4->getNextFrame()->id());
- EXPECT_EQ(F6->id(), F5->getNextFrame()->id());
- EXPECT_EQ(F7->id(), F6->getNextFrame()->id());
- EXPECT_EQ(F8->id(), F7->getNextFrame()->id());
- EXPECT_EQ(F9->id(), F8->getNextFrame()->id());
- EXPECT_EQ(nullptr, F9->getNextFrame());
+ EXPECT_EQ(F1, F0->getNextFrame(1));
+ EXPECT_EQ(F2, F0->getNextFrame(2));
+ EXPECT_EQ(F3, F0->getNextFrame(3));
+ EXPECT_EQ(F4, F0->getNextFrame(4));
+ EXPECT_EQ(F5, F0->getNextFrame(5));
+ EXPECT_EQ(F6, F0->getNextFrame(6));
+ EXPECT_EQ(F7, F0->getNextFrame(7));
+ EXPECT_EQ(F8, F0->getNextFrame(8));
+ EXPECT_EQ(F9, F0->getNextFrame(9));
+ EXPECT_EQ(nullptr, F0->getNextFrame(10));
+
+ EXPECT_EQ(F2, F1->getNextFrame(1));
+ EXPECT_EQ(F3, F1->getNextFrame(2));
+ EXPECT_EQ(F4, F1->getNextFrame(3));
+ EXPECT_EQ(F5, F1->getNextFrame(4));
+ EXPECT_EQ(F6, F1->getNextFrame(5));
+ EXPECT_EQ(F7, F1->getNextFrame(6));
+ EXPECT_EQ(F8, F1->getNextFrame(7));
+ EXPECT_EQ(F9, F1->getNextFrame(8));
+ EXPECT_EQ(nullptr, F1->getNextFrame(9));
+ EXPECT_EQ(nullptr, F1->getNextFrame(10));
+
+ EXPECT_EQ(F3, F2->getNextFrame(1));
+ EXPECT_EQ(F4, F2->getNextFrame(2));
+ EXPECT_EQ(F5, F2->getNextFrame(3));
+ EXPECT_EQ(F6, F2->getNextFrame(4));
+ EXPECT_EQ(F7, F2->getNextFrame(5));
+ EXPECT_EQ(F8, F2->getNextFrame(6));
+ EXPECT_EQ(F9, F2->getNextFrame(7));
+ EXPECT_EQ(nullptr, F2->getNextFrame(8));
+ EXPECT_EQ(nullptr, F2->getNextFrame(9));
+ EXPECT_EQ(nullptr, F2->getNextFrame(10));
+
+ EXPECT_EQ(F4, F3->getNextFrame(1));
+ EXPECT_EQ(F5, F3->getNextFrame(2));
+ EXPECT_EQ(F6, F3->getNextFrame(3));
+ EXPECT_EQ(F7, F3->getNextFrame(4));
+ EXPECT_EQ(F8, F3->getNextFrame(5));
+ EXPECT_EQ(F9, F3->getNextFrame(6));
+ EXPECT_EQ(nullptr, F3->getNextFrame(7));
+ EXPECT_EQ(nullptr, F3->getNextFrame(8));
+ EXPECT_EQ(nullptr, F3->getNextFrame(9));
+ EXPECT_EQ(nullptr, F3->getNextFrame(10));
+
+ EXPECT_EQ(F5, F4->getNextFrame(1));
+ EXPECT_EQ(F6, F4->getNextFrame(2));
+ EXPECT_EQ(F7, F4->getNextFrame(3));
+ EXPECT_EQ(F8, F4->getNextFrame(4));
+ EXPECT_EQ(F9, F4->getNextFrame(5));
+ EXPECT_EQ(nullptr, F4->getNextFrame(6));
+ EXPECT_EQ(nullptr, F4->getNextFrame(7));
+ EXPECT_EQ(nullptr, F4->getNextFrame(8));
+ EXPECT_EQ(nullptr, F4->getNextFrame(9));
+ EXPECT_EQ(nullptr, F4->getNextFrame(10));
+
+ EXPECT_EQ(F6, F5->getNextFrame(1));
+ EXPECT_EQ(F7, F5->getNextFrame(2));
+ EXPECT_EQ(F8, F5->getNextFrame(3));
+ EXPECT_EQ(F9, F5->getNextFrame(4));
+ EXPECT_EQ(nullptr, F5->getNextFrame(5));
+ EXPECT_EQ(nullptr, F5->getNextFrame(6));
+ EXPECT_EQ(nullptr, F5->getNextFrame(7));
+ EXPECT_EQ(nullptr, F5->getNextFrame(8));
+ EXPECT_EQ(nullptr, F5->getNextFrame(9));
+ EXPECT_EQ(nullptr, F5->getNextFrame(10));
+
+ EXPECT_EQ(F7, F6->getNextFrame(1));
+ EXPECT_EQ(F8, F6->getNextFrame(2));
+ EXPECT_EQ(F9, F6->getNextFrame(3));
+ EXPECT_EQ(nullptr, F6->getNextFrame(4));
+ EXPECT_EQ(nullptr, F6->getNextFrame(5));
+ EXPECT_EQ(nullptr, F6->getNextFrame(6));
+ EXPECT_EQ(nullptr, F6->getNextFrame(7));
+ EXPECT_EQ(nullptr, F6->getNextFrame(8));
+ EXPECT_EQ(nullptr, F6->getNextFrame(9));
+ EXPECT_EQ(nullptr, F6->getNextFrame(10));
+
+ EXPECT_EQ(F8, F7->getNextFrame(1));
+ EXPECT_EQ(F9, F7->getNextFrame(2));
+ EXPECT_EQ(nullptr, F7->getNextFrame(3));
+ EXPECT_EQ(nullptr, F7->getNextFrame(4));
+ EXPECT_EQ(nullptr, F7->getNextFrame(5));
+ EXPECT_EQ(nullptr, F7->getNextFrame(6));
+ EXPECT_EQ(nullptr, F7->getNextFrame(7));
+ EXPECT_EQ(nullptr, F7->getNextFrame(8));
+ EXPECT_EQ(nullptr, F7->getNextFrame(9));
+ EXPECT_EQ(nullptr, F7->getNextFrame(10));
+
+ EXPECT_EQ(F9, F8->getNextFrame(1));
+ EXPECT_EQ(nullptr, F8->getNextFrame(2));
+ EXPECT_EQ(nullptr, F8->getNextFrame(3));
+ EXPECT_EQ(nullptr, F8->getNextFrame(4));
+ EXPECT_EQ(nullptr, F8->getNextFrame(5));
+ EXPECT_EQ(nullptr, F8->getNextFrame(6));
+ EXPECT_EQ(nullptr, F8->getNextFrame(7));
+ EXPECT_EQ(nullptr, F8->getNextFrame(8));
+ EXPECT_EQ(nullptr, F8->getNextFrame(9));
+ EXPECT_EQ(nullptr, F8->getNextFrame(10));
+
+ EXPECT_EQ(nullptr, F9->getNextFrame(1));
+ EXPECT_EQ(nullptr, F9->getNextFrame(2));
+ EXPECT_EQ(nullptr, F9->getNextFrame(3));
+ EXPECT_EQ(nullptr, F9->getNextFrame(4));
+ EXPECT_EQ(nullptr, F9->getNextFrame(5));
+ EXPECT_EQ(nullptr, F9->getNextFrame(6));
+ EXPECT_EQ(nullptr, F9->getNextFrame(7));
+ EXPECT_EQ(nullptr, F9->getNextFrame(8));
+ EXPECT_EQ(nullptr, F9->getNextFrame(9));
+ EXPECT_EQ(nullptr, F9->getNextFrame(10));
// Prev frame
- EXPECT_EQ(nullptr, F0->getPreviousFrame());
- EXPECT_EQ(F0, F1->getPreviousFrame());
- EXPECT_EQ(F1, F2->getPreviousFrame());
- EXPECT_EQ(F2, F3->getPreviousFrame());
- EXPECT_EQ(F3, F4->getPreviousFrame());
- EXPECT_EQ(F4, F5->getPreviousFrame());
- EXPECT_EQ(F5, F6->getPreviousFrame());
- EXPECT_EQ(F6, F7->getPreviousFrame());
- EXPECT_EQ(F7, F8->getPreviousFrame());
- EXPECT_EQ(F8, F9->getPreviousFrame());
+ EXPECT_EQ(F8, F9->getPreviousFrame(1));
+ EXPECT_EQ(F7, F9->getPreviousFrame(2));
+ EXPECT_EQ(F6, F9->getPreviousFrame(3));
+ EXPECT_EQ(F5, F9->getPreviousFrame(4));
+ EXPECT_EQ(F4, F9->getPreviousFrame(5));
+ EXPECT_EQ(F3, F9->getPreviousFrame(6));
+ EXPECT_EQ(F2, F9->getPreviousFrame(7));
+ EXPECT_EQ(F1, F9->getPreviousFrame(8));
+ EXPECT_EQ(F0, F9->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F9->getPreviousFrame(10));
+
+ EXPECT_EQ(F7, F8->getPreviousFrame(1));
+ EXPECT_EQ(F6, F8->getPreviousFrame(2));
+ EXPECT_EQ(F5, F8->getPreviousFrame(3));
+ EXPECT_EQ(F4, F8->getPreviousFrame(4));
+ EXPECT_EQ(F3, F8->getPreviousFrame(5));
+ EXPECT_EQ(F2, F8->getPreviousFrame(6));
+ EXPECT_EQ(F1, F8->getPreviousFrame(7));
+ EXPECT_EQ(F0, F8->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F8->getPreviousFrame(10));
+
+ EXPECT_EQ(F6, F7->getPreviousFrame(1));
+ EXPECT_EQ(F5, F7->getPreviousFrame(2));
+ EXPECT_EQ(F4, F7->getPreviousFrame(3));
+ EXPECT_EQ(F3, F7->getPreviousFrame(4));
+ EXPECT_EQ(F2, F7->getPreviousFrame(5));
+ EXPECT_EQ(F1, F7->getPreviousFrame(6));
+ EXPECT_EQ(F0, F7->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F7->getPreviousFrame(10));
+
+ EXPECT_EQ(F5, F6->getPreviousFrame(1));
+ EXPECT_EQ(F4, F6->getPreviousFrame(2));
+ EXPECT_EQ(F3, F6->getPreviousFrame(3));
+ EXPECT_EQ(F2, F6->getPreviousFrame(4));
+ EXPECT_EQ(F1, F6->getPreviousFrame(5));
+ EXPECT_EQ(F0, F6->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F6->getPreviousFrame(10));
+
+ EXPECT_EQ(F4, F5->getPreviousFrame(1));
+ EXPECT_EQ(F3, F5->getPreviousFrame(2));
+ EXPECT_EQ(F2, F5->getPreviousFrame(3));
+ EXPECT_EQ(F1, F5->getPreviousFrame(4));
+ EXPECT_EQ(F0, F5->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F5->getPreviousFrame(10));
+
+ EXPECT_EQ(F3, F4->getPreviousFrame(1));
+ EXPECT_EQ(F2, F4->getPreviousFrame(2));
+ EXPECT_EQ(F1, F4->getPreviousFrame(3));
+ EXPECT_EQ(F0, F4->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F4->getPreviousFrame(10));
+
+ EXPECT_EQ(F2, F3->getPreviousFrame(1));
+ EXPECT_EQ(F1, F3->getPreviousFrame(2));
+ EXPECT_EQ(F0, F3->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F3->getPreviousFrame(10));
+
+ EXPECT_EQ(F1, F2->getPreviousFrame(1));
+ EXPECT_EQ(F0, F2->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F2->getPreviousFrame(10));
+
+ EXPECT_EQ(F0, F1->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F1->getPreviousFrame(10));
+
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(1));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(2));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(3));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(4));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(5));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(6));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(7));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(8));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(9));
+ EXPECT_EQ(nullptr, F0->getPreviousFrame(10));
}
#include "core/state_block/state_quaternion.h"