Skip to content
Snippets Groups Projects
Commit c804877d authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

reimplemented and tests added

parent 0f7ad039
No related branches found
No related tags found
1 merge request!409Resolve "Reimplement FrameBase::getPreviousFrame() and getNextFrame()"
Pipeline #6314 passed
...@@ -141,43 +141,28 @@ FrameBasePtr FrameBase::getPreviousFrame() const ...@@ -141,43 +141,28 @@ FrameBasePtr FrameBase::getPreviousFrame() const
{ {
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory"); assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory) auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ )
{ assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if ( this->frame_id_ == (*f_it)->id() )
{ if (current_frame_it == getTrajectory()->getFrameMap().begin())
f_it++; return nullptr;
if (f_it != getTrajectory()->rend())
{ return std::prev(current_frame_it)->second;
return *f_it;
}
else
{
return nullptr;
}
}
}
return nullptr;
} }
FrameBasePtr FrameBase::getNextFrame() const FrameBasePtr FrameBase::getNextFrame() const
{ {
//std::cout << "finding next frame of " << this->frame_id_ << std::endl; assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
auto f_it = getTrajectory()->rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory auto current_frame_it = getTrajectory()->getFrameMap().find(time_stamp_);
while (f_it != getTrajectory()->rend())
{ assert(current_frame_it != getTrajectory()->getFrameMap().end() && "Frame not found in the frame map!");
if ( this->frame_id_ == (*f_it)->id())
{ if (std::next(current_frame_it) == getTrajectory()->getFrameMap().end())
f_it--; return nullptr;
return *f_it;
} return std::next(current_frame_it)->second;
f_it++;
}
std::cout << "next frame not found!" << std::endl;
return nullptr;
} }
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr) CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
......
...@@ -80,9 +80,6 @@ TEST(FrameBase, LinksToTree) ...@@ -80,9 +80,6 @@ TEST(FrameBase, LinksToTree)
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic. //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
ASSERT_FALSE(F2->getConstrainedByList().empty()); ASSERT_FALSE(F2->getConstrainedByList().empty());
// tree is inconsistent since we are missing the constrained_by link
// ASSERT_FALSE(P->check(0));
// tree is now consistent // tree is now consistent
ASSERT_TRUE(P->check(0)); ASSERT_TRUE(P->check(0));
...@@ -113,6 +110,70 @@ TEST(FrameBase, LinksToTree) ...@@ -113,6 +110,70 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE(F1->isFixed()); ASSERT_TRUE(F1->isFixed());
} }
TEST(FrameBase, Frames)
{
// Problem with 10 frames
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
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));
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F3 = FrameBase::emplace<FrameBase>(T, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F4 = FrameBase::emplace<FrameBase>(T, 4, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F5 = FrameBase::emplace<FrameBase>(T, 5, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F6 = FrameBase::emplace<FrameBase>(T, 6, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F7 = FrameBase::emplace<FrameBase>(T, 7, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F8 = FrameBase::emplace<FrameBase>(T, 8, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F9 = FrameBase::emplace<FrameBase>(T, 9, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
// tree is now consistent
ASSERT_TRUE(P->check(0));
// First and last
ASSERT_EQ(F0, T->getFirstFrame());
ASSERT_EQ(F9, T->getLastFrame());
// Find by timestamp
ASSERT_EQ(F0, T->closestFrameToTimeStamp(0));
ASSERT_EQ(F1, T->closestFrameToTimeStamp(1));
ASSERT_EQ(F2, T->closestFrameToTimeStamp(2));
ASSERT_EQ(F3, T->closestFrameToTimeStamp(3));
ASSERT_EQ(F4, T->closestFrameToTimeStamp(4));
ASSERT_EQ(F5, T->closestFrameToTimeStamp(5));
ASSERT_EQ(F6, T->closestFrameToTimeStamp(6));
ASSERT_EQ(F7, T->closestFrameToTimeStamp(7));
ASSERT_EQ(F8, T->closestFrameToTimeStamp(8));
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());
// 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());
}
#include "core/state_block/state_quaternion.h" #include "core/state_block/state_quaternion.h"
TEST(FrameBase, GetSetState) TEST(FrameBase, GetSetState)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment