diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp index 8808418157e478daaf8b33a8914465aaaaa5ba25..f1b9362e17207818704728f8a4151402aae386e7 100644 --- a/src/test/gtest_feature_imu.cpp +++ b/src/test/gtest_feature_imu.cpp @@ -25,6 +25,7 @@ class FeatureIMU_test : public testing::Test Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov; std::shared_ptr<wolf::FeatureIMU> feat_imu; wolf::FrameIMUPtr last_frame; + wolf::FrameBasePtr origin_frame; //a new of this will be created for each new test virtual void SetUp() @@ -56,7 +57,7 @@ class FeatureIMU_test : public testing::Test //create a keyframe at origin ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_; Eigen::VectorXs origin_state = x0; - FrameBasePtr origin_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, origin_state); + origin_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, origin_state); wolf_problem_ptr_->getTrajectoryPtr()->addFrame(origin_frame); // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) @@ -108,8 +109,28 @@ TEST_F(FeatureIMU_test, check_frame) using namespace wolf; //FIXME : Feature not linked to origin frame - FrameBasePtr frame_base = feat_imu->getFramePtr(); - + FrameBasePtr left_frame = feat_imu->getFramePtr(); + wolf::TimeStamp t; + left_frame->getTimeStamp(t); + origin_frame->getTimeStamp(ts); + + ASSERT_EQ(t,ts) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl; + ASSERT_TRUE(origin_frame->isKey()); + ASSERT_TRUE(left_frame->isKey()); + + wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr; + origin_pptr = origin_frame->getPPtr(); + origin_optr = origin_frame->getOPtr(); + origin_vptr = origin_frame->getVPtr(); + left_pptr = left_frame->getPPtr(); + left_optr = left_frame->getOPtr(); + left_vptr = left_frame->getVPtr(); + + ASSERT_TRUE((origin_pptr->getVector() - left_pptr->getVector()).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); + ASSERT_TRUE((origin_optr->getVector() - left_optr->getVector()).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); + ASSERT_TRUE((origin_vptr->getVector() - left_vptr->getVector()).isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); + + ASSERT_EQ(origin_frame->id(), left_frame->id()); //ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, last_frame); //ConstraintIMU constraint_imu(feat_imu, last_frame); //feat_imu->addConstraint(constraint_imu);