diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index ffdcfe3231e0636c7a1d2af4949ee54383a83c8e..2b6c9cc6c645205f16e63db4d92bc146daacdc34 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -59,6 +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);
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 07e728fd6f138717b8f205af92744630d8dbcd03..9467e1bc735328aa16217e7e47f12eb9204f68df 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -160,56 +160,28 @@ FrameBaseConstPtr FrameBase::getPreviousFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
-
- if (current_frame_it == getTrajectory()->getFrameMap().begin())
- return nullptr;
-
- return std::prev(current_frame_it)->second;
+ return getTrajectory()->getPreviousFrame(shared_from_this());
}
FrameBasePtr FrameBase::getPreviousFrame()
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
-
- if (current_frame_it == getTrajectory()->getFrameMap().begin())
- return nullptr;
-
- return std::prev(current_frame_it)->second;
+ return getTrajectory()->getPreviousFrame(shared_from_this());
}
FrameBaseConstPtr FrameBase::getNextFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
-
- if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
- return nullptr;
-
- return std::next(current_frame_it)->second;
+ return getTrajectory()->getNextFrame(shared_from_this());
}
FrameBasePtr FrameBase::getNextFrame()
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
- auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
-
- assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
-
- if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
- return nullptr;
-
- return std::next(current_frame_it)->second;
+ return getTrajectory()->getNextFrame(shared_from_this());
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 78b61297e5c746b3ab8ac543b460c2f52904323d..ef4b1ea19c681434f3b5ce9bc1c01a9a43df8ede 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -901,8 +901,9 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
FrameBaseConstPtr frame = nullptr;
CaptureBaseConstPtr capture = nullptr;
CaptureMotionConstPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameMap().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameMap().rend();
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
++frame_rev_iter)
{
frame = frame_rev_iter->second;
@@ -951,8 +952,9 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameMap().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameMap().rend();
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ for (auto frame_rev_iter = frame_map.rbegin();
+ frame_rev_iter != frame_map.rend();
++frame_rev_iter)
{
frame = frame_rev_iter->second;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index c081dc0103bd0f1eb5dd25f5fe54bfa9febcb3b0..2e6748bcf9a77cf123cf2f35aff88223848eb993 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -49,8 +49,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
- // add to list
- // frame_map_.erase(_frame_ptr);
frame_map_.erase(_frame_ptr->getTimeStamp());
frame_const_map_.erase(_frame_ptr->getTimeStamp());
}
@@ -96,7 +94,7 @@ TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) cons
return upper_bound->first;
// unreachable
- return TimeStamp();
+ return TimeStamp::Invalid();
}
FrameBaseConstPtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
@@ -123,10 +121,56 @@ FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts)
return nullptr;
}
+FrameBaseConstPtr TrajectoryBase::getNextFrame(FrameBaseConstPtr _frame) 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())
+ return nullptr;
+
+ return std::next(frame_it)->second;
+}
+
+FrameBasePtr TrajectoryBase::getNextFrame(FrameBasePtr _frame)
+{
+ 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())
+ return nullptr;
+
+ return std::next(frame_it)->second;
+}
+
+FrameBaseConstPtr TrajectoryBase::getPreviousFrame(FrameBaseConstPtr _frame) 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())
+ return nullptr;
+
+ return std::prev(frame_it)->second;
+}
+
+FrameBasePtr TrajectoryBase::getPreviousFrame(FrameBasePtr _frame)
+{
+ 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())
+ return nullptr;
+
+ return std::prev(frame_it)->second;
+}
+
+
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
+
void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
@@ -152,6 +196,7 @@ CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBaseConstPtr _trj_p
log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
return log;
}
+
bool TrajectoryBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto trj_ptr = std::static_pointer_cast<const TrajectoryBase>(_node_ptr);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 26c9b3cc0fe7f5aff4eb533c93826c84cb9d0d55..de1fc08ce7f926a380aeb04d21b33ec4dddde7b7 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -140,15 +140,25 @@ 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));
@@ -170,28 +180,28 @@ TEST(FrameBase, Frames)
ASSERT_EQ(F9, T->closestFrameToTimeStamp(9));
// Next frame
- ASSERT_EQ(F1, F0->getNextFrame());
- ASSERT_EQ(F2, F1->getNextFrame());
- ASSERT_EQ(F3, F2->getNextFrame());
- ASSERT_EQ(F4, F3->getNextFrame());
- ASSERT_EQ(F5, F4->getNextFrame());
- ASSERT_EQ(F6, F5->getNextFrame());
- ASSERT_EQ(F7, F6->getNextFrame());
- ASSERT_EQ(F8, F7->getNextFrame());
- ASSERT_EQ(F9, F8->getNextFrame());
- ASSERT_EQ(nullptr, F9->getNextFrame());
+ 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());
// Prev frame
- ASSERT_EQ(nullptr, F0->getPreviousFrame());
- ASSERT_EQ(F0, F1->getPreviousFrame());
- ASSERT_EQ(F1, F2->getPreviousFrame());
- ASSERT_EQ(F2, F3->getPreviousFrame());
- ASSERT_EQ(F3, F4->getPreviousFrame());
- ASSERT_EQ(F4, F5->getPreviousFrame());
- ASSERT_EQ(F5, F6->getPreviousFrame());
- ASSERT_EQ(F6, F7->getPreviousFrame());
- ASSERT_EQ(F7, F8->getPreviousFrame());
- ASSERT_EQ(F8, F9->getPreviousFrame());
+ 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());
}
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 1192372ff97072462258a560a7b64bdfc0776b14..7391619e1b59f1e3258a740ef034936e1a757d2d 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -399,7 +399,6 @@ TEST(Odom2d, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
@@ -407,7 +406,8 @@ TEST(Odom2d, KF_callback)
t += dt;
// re-use capture with updated timestamp
- capture->setTimeStamp(t);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ //capture->setTimeStamp(t);
// Processor
capture->process();
@@ -452,7 +452,7 @@ TEST(Odom2d, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));