diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp index 437c18945ccc14e9df143db8d48b50fef70ee3f2..b9d873cb0f1d4d44dad177880751c6356ea94423 100644 --- a/src/test/gtest_frame_base.cpp +++ b/src/test/gtest_frame_base.cpp @@ -12,6 +12,8 @@ #include "../frame_base.h" #include "../sensor_odom_2D.h" +#include "../constraint_odom_2D.h" +#include "../capture_motion.h" #include <iostream> @@ -23,37 +25,90 @@ using namespace wolf; TEST(FrameBase, GettersAndSetters) { - FrameBasePtr f = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // getters - ASSERT_EQ(f->id(), 1); - ASSERT_EQ(f->getTimeStamp(), 1); + ASSERT_EQ(F->id(), 1); + ASSERT_EQ(F->getTimeStamp(), 1); TimeStamp t; - f->getTimeStamp(t); + F->getTimeStamp(t); ASSERT_EQ(t, 1); - ASSERT_EQ(f->getPPtr()->getVector().size(), 2); - ASSERT_EQ(f->getOPtr()->getVector().size(), 1); - ASSERT_EQ(f->getVPtr(), nullptr); - ASSERT_EQ(f->isFixed(), false); - ASSERT_EQ(f->isKey(), false); - ASSERT_EQ(f->getStateBlockVec().size(), 3); - ASSERT_FALSE(f->getTrajectoryPtr()); - ASSERT_FALSE(f->getProblem()); - // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() - // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected - ASSERT_FALSE(f->getCaptureOf(make_shared<SensorOdom2D>(nullptr, nullptr, 1,1))); - ASSERT_TRUE(f->getCaptureList().empty()); - ASSERT_TRUE(f->getConstrainedByList().empty()); + ASSERT_EQ(F->isFixed(), false); + ASSERT_EQ(F->isKey(), false); } -TEST(FrameBase, TimeStamp) +TEST(FrameBase, StateBlocks) { + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + ASSERT_EQ(F->getStateBlockVec().size(), 3); + ASSERT_EQ(F->getPPtr()->getVector().size(), 2); + ASSERT_EQ(F->getOPtr()->getVector().size(), 1); + ASSERT_EQ(F->getVPtr(), nullptr); } -TEST(FrameBase, LinksToTree) +TEST(FrameBase, LinksBasic) { + FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + + ASSERT_FALSE(F->getTrajectoryPtr()); + ASSERT_FALSE(F->getProblem()); + // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() + // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected + ASSERT_FALSE(F->getCaptureOf(make_shared<SensorOdom2D>(nullptr, nullptr, 1,1))); + ASSERT_TRUE(F->getCaptureList().empty()); + ASSERT_TRUE(F->getConstrainedByList().empty()); + ASSERT_EQ(F->getHits() , 0); +} + +TEST(FrameBase, LinksToTree) +{ + // Problem with 2 frames and one motion constraint between them + ProblemPtr P = Problem::create(FRM_PO_2D); + TrajectoryBasePtr T = P->getTrajectoryPtr(); + SensorOdom2D::Ptr S = make_shared<SensorOdom2D>(make_shared<StateBlock>(2), make_shared<StateBlock>(1), 1,1); + P->getHardwarePtr()->addSensor(S); + FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + T->addFrame(F1); + FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + T->addFrame(F2); + CaptureMotion::Ptr C = make_shared<CaptureMotion>(1, S, Vector3s::Zero()); + F1->addCapture(C); + FeatureBasePtr f = make_shared<FeatureBase>("f", 1); + C->addFeature(f); + ConstraintOdom2D::Ptr c = make_shared<ConstraintOdom2D>(f, F2); + f->addConstraint(c); + + // c-by link F2 -> c not yet established + ASSERT_TRUE(F2->getConstrainedByList().empty()); + + // establish link F2 -> c + F2->addConstrainedBy(c); + + // F1 has one capture and no constraints-by + ASSERT_FALSE(F1->getCaptureList().empty()); + ASSERT_TRUE(F1->getConstrainedByList().empty()); + ASSERT_EQ(F1->getHits() , 0); + + // F2 has no capture and one constraint-by + ASSERT_TRUE(F2->getCaptureList().empty()); + ASSERT_FALSE(F2->getConstrainedByList().empty()); + ASSERT_EQ(F2->getHits() , 1); + + // fix and unfix + F1->fix(); + ASSERT_TRUE(F1->isFixed()); + F1->unfix(); + ASSERT_FALSE(F1->isFixed()); + + // set key + F1->setKey(); + ASSERT_TRUE(F1->isKey()); + + // Unlink + F1->unlinkCapture(C); + ASSERT_TRUE(F1->getCaptureList().empty()); }