diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 37fc3a0ea2ea96144c6ab11d016250197a20dd0c..8f0e36f28eb76497c3dfc0ddc9417317a2a5795b 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -1,4 +1,5 @@ + #ifndef FRAME_BASE_H_ #define FRAME_BASE_H_ @@ -79,9 +80,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha const std::string _frame_structure, const std::list<VectorXd>& _vectors); - //Auxiliary constructor, needed to put frametype in front - FrameBase(const FrameType & _tp, const std::string _frame_structure, const SizeEigen _dim, const TimeStamp& _ts, const Eigen::VectorXd& _x); - virtual ~FrameBase(); virtual void remove(bool viral_remove_empty_parent=false); diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 863a43571478656496f9035bcefca96c29756cfc..14dded715cc33d9f3cba7e0275c7120a8f75a746 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -217,11 +217,10 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state); + FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim, // + const Eigen::VectorXd& _frame_state); /** \brief Emplace frame from string frame_structure and state * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED @@ -236,10 +235,9 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const VectorComposite& _frame_state); + FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const VectorComposite& _frame_state); /** \brief Emplace frame from state * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED @@ -254,9 +252,8 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state); + FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, // + const VectorComposite& _frame_state); /** \brief Emplace frame from string frame_structure and dimension * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED @@ -272,10 +269,9 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim); + FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim); /** \brief Emplace frame from state vector * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED @@ -290,9 +286,8 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ - FrameBasePtr emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const Eigen::VectorXd& _frame_state); + FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, // + const Eigen::VectorXd& _frame_state); /** \brief Emplace frame, guess all values * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index aed4c7bc69b99233426faeb6baa592844e0fa63a..a33cac51a10e152825be936be438e9b4adaa09cb 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -149,11 +149,6 @@ FrameBase::FrameBase(const FrameType & _tp, } -FrameBase::FrameBase(const FrameType & _tp, const std::string _frame_structure, const SizeEigen _dim, const TimeStamp& _ts, const Eigen::VectorXd& _x): - FrameBase(_frame_structure, _dim, _tp, _ts, _x) -{ -} - FrameBase::~FrameBase() { } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 535a60350ec1e2ef84671c6e0644f478d2cfbf98..5a2f02eb8b59871e3a42466e5a9bd2be443c8b5d 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -325,14 +325,12 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const return (*sen_it); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state) +FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim, // + const Eigen::VectorXd& _frame_state) { - auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, - _frame_key_type, + auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_, _time_stamp, _frame_structure, _dim, @@ -340,69 +338,59 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // return frm; } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim) +FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const SizeEigen _dim) { - return emplaceFrame(_frame_key_type, - _time_stamp, - _frame_structure, - getState(_time_stamp)); + return emplaceKeyFrame(_time_stamp, + _frame_structure, + getState(_time_stamp)); } -FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const VectorComposite& _frame_state) +FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, // + const StateStructure& _frame_structure, // + const VectorComposite& _frame_state) { - return FrameBase::emplace<FrameBase>(getTrajectory(), - _frame_key_type, - _time_stamp, - _frame_structure, - _frame_state); + return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(), + _time_stamp, + _frame_structure, + _frame_state); } -FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const VectorComposite& _frame_state) +FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, // + const VectorComposite& _frame_state) { - return FrameBase::emplace<FrameBase>(getTrajectory(), - _frame_key_type, - _time_stamp, - getFrameStructure(), - _frame_state); + return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(), + _time_stamp, + getFrameStructure(), + _frame_state); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp, // - const Eigen::VectorXd& _frame_state) +FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, // + const Eigen::VectorXd& _frame_state) { - return emplaceFrame(_frame_key_type, - _time_stamp, - this->getFrameStructure(), - this->getDim(), - _frame_state); + return emplaceKeyFrame(_time_stamp, + this->getFrameStructure(), + this->getDim(), + _frame_state); } -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp) +FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp) { - return emplaceFrame(_frame_key_type, - _time_stamp, - this->getFrameStructure(), - this->getDim()); + return emplaceKeyFrame(_time_stamp, + this->getFrameStructure(), + this->getDim()); } TimeStamp Problem::getTimeStamp ( ) const { if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state { - auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); + auto last_kf = trajectory_ptr_->getLastKeyFrame(); - assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!"); + assert(last_kf != nullptr && "Problem has no Keyframe so no timestamp can be obtained!"); - return last_kf_or_aux->getTimeStamp(); + return last_kf->getTimeStamp(); } else { @@ -422,9 +410,9 @@ VectorComposite Problem::getState(const StateStructure& _structure) const if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state { - auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame(); - if (last_kf_or_aux) - state = last_kf_or_aux->getState(structure); + auto last_kf = trajectory_ptr_->getLastKeyFrame(); + if (last_kf) + state = last_kf->getState(structure); else state = stateZero(structure); } @@ -458,10 +446,10 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state { - const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); - if (last_kf_or_aux) + const auto& last_kf = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); + if (last_kf) { - return last_kf_or_aux->getState(structure); + return last_kf->getState(structure); } else { @@ -971,9 +959,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) if (prior_options_->mode != "nothing" and prior_options_->mode != "") { - prior_keyframe = emplaceFrame(KEY, - _ts, - prior_options_->state); + prior_keyframe = emplaceKeyFrame(_ts, + prior_options_->state); if (prior_options_->mode == "fix") prior_keyframe->fix(); diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index a1006ed3838b21a66afaa6f25467b001e53d0dbc..8836badc8463565af3df36947d25d0d3aaa4e021 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -360,10 +360,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) auto factor = emplaceFactor(key_feature, origin_ptr_); // create a new frame - auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED, - getTimeStamp(), - getStateStructure(), - getProblem()->getState()); + auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(getTimeStamp(), + getStateStructure(), + getProblem()->getState()); // create a new capture auto capture_new = emplaceCapture(frame_new, getSensor(), @@ -550,10 +549,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin) { - FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, - _ts_origin, - getStateStructure(), - _x_origin); + FrameBasePtr key_frame_ptr = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(), + _ts_origin, + getStateStructure(), + _x_origin); setOrigin(key_frame_ptr); return key_frame_ptr; @@ -581,10 +580,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) // ---------- LAST ---------- // Make non-key-frame for last Capture - auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED, - origin_ts, - getStateStructure(), - _origin_frame->getState()); + auto new_frame_ptr = FrameBase::createNonKeyFrame<FrameBase>(origin_ts, + getStateStructure(), + _origin_frame->getState()); // emplace (emtpy) last Capture last_ptr_ = emplaceCapture(new_frame_ptr, diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index be506f6da3a3369578164002b2f6a21590ba1c6f..4e777ec5b54e09229faafede6f714dfdb7f26b52 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -26,7 +26,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type, incoming_ptr_(nullptr), origin_frame_ptr_(nullptr), last_frame_ptr_(nullptr), - incoming_frame_ptr_(nullptr) + incoming_frame_ptr_(nullptr), state_structure_(_structure) { // @@ -83,10 +83,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" ); - FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, - incoming_ptr_->getTimeStamp(), - getStateStructure(), - getProblem()->getState(getStateStructure())); + FrameBasePtr kfrm = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(), + incoming_ptr_->getTimeStamp(), + getStateStructure(), + getProblem()->getState(getStateStructure())); incoming_ptr_->link(kfrm); // Process info @@ -116,8 +116,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) { WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" ); - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getState()); incoming_ptr_->link(frm); incoming_frame_ptr_ = frm; // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF. @@ -155,8 +156,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) last_old_frame->remove(); // Create new frame - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getState()); incoming_ptr_->link(frm); incoming_frame_ptr_ = frm; @@ -209,9 +211,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) resetDerived(); // make F; append incoming to new F - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp(), - last_ptr_->getFrame()->getStateVector()); + FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getDim(), + last_ptr_->getFrame()->getStateVector()); incoming_ptr_->link(frm); incoming_frame_ptr_ = frm; @@ -262,9 +265,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) advanceDerived(); // Replace last frame for a new one in incoming - FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, - incoming_ptr_->getTimeStamp(), - last_ptr_->getFrame()->getStateVector()); + FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(), + getProblem()->getFrameStructure(), + getProblem()->getDim(), + last_ptr_->getFrame()->getStateVector()); incoming_ptr_->link(frm); incoming_frame_ptr_ = frm; last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove(); diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 4d0b6de19dc18a77d8c62f7e6b7fa329965fba3f..3e78796c928c38aed9c59238eaa9d5b76b0491ca 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -4,8 +4,7 @@ namespace wolf { TrajectoryBase::TrajectoryBase() : - NodeBase("TRAJECTORY", "TrajectoryBase"), - frame_structure_(_frame_structure) + NodeBase("TRAJECTORY", "TrajectoryBase") // last_key_frame_ptr_(nullptr), // last_key_or_aux_frame_ptr_(nullptr) { diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index d2b5c7e01b70fa65472f3e18bbfa7b38d8989679..f218ca36518f5fa0c226ba237dde25913e973d51 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame( 0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); @@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); @@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); @@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); @@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false); @@ -546,7 +546,7 @@ TEST(CeresManager, FactorsRemoveLocalParam) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); @@ -588,7 +588,7 @@ TEST(CeresManager, FactorsUpdateLocalParam) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 82b56da523c70dff134908ba3e63adf97493d914..d8a084d2b0cb4b922e0e20e9c4cacb8ecc2155b1 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV", 3); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() ); +FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, problem_ptr->stateZero() ); // Capture // auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr); diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp index e25e1505a3ddaa15ecfa6152fc007bc6009696f9..8651072129bb77028caa12bbbe3e2f3e9be0c954 100644 --- a/test/gtest_factor_autodiff_distance_3d.cpp +++ b/test/gtest_factor_autodiff_distance_3d.cpp @@ -55,9 +55,9 @@ class FactorAutodiffDistance3d_Test : public testing::Test problem = Problem::create("PO", 3); ceres_manager = std::make_shared<CeresManager>(problem); - F1 = problem->emplaceFrame (KEY, 1.0, pose1); + F1 = problem->emplaceKeyFrame (1.0, pose1); - F2 = problem->emplaceFrame (KEY, 2.0, pose2); + F2 = problem->emplaceKeyFrame (2.0, pose2); C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0); } diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index bee51d78e417f1f446a5890fa1ead4834dc366bc..b03cc13336e054c1a119b92aab2cb1a9a02ae3a8 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -59,7 +59,7 @@ class FixtureFactorBlockDifference : public testing::Test //FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>()); //FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false); - KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero()); + KF1_ = problem_->emplaceKeyFrame(t1, problem_->stateZero()); Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1); } diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index 0413c21cb2c30bb91b1a03dabac0b595d1843349..c8fde25cb2a39beafdb2ec03b4e1baaba1a9c797 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -159,18 +159,16 @@ class FactorDiffDriveTest : public testing::Test processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc); // frames - F0 = FrameBase::emplace<FrameBase>(trajectory, - KEY, - 0.0, - "PO", - 2, - Vector3d(0,0,0)); - F1 = FrameBase::emplace<FrameBase>(trajectory, - KEY, - 1.0, - "PO", - 2, - Vector3d(1,0,0)); + F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory, + 0.0, + "PO", + 2, + Vector3d(0,0,0)); + F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory, + 1.0, + "PO", + 2, + Vector3d(1,0,0)); // captures C0 = CaptureBase::emplace<CaptureDiffDrive>(F0, diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp index 8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92..0931795263da44176ae4379f8904f0bb1e7aad0c 100644 --- a/test/gtest_factor_odom_2d.cpp +++ b/test/gtest_factor_odom_2d.cpp @@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero()); +FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), Vector3d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp index 8001bbe70f25f774a55cf0c834950529934bdb36..0d71e02baf3dd1e1d7cbb52da4d85def82e57336 100644 --- a/test/gtest_factor_odom_3d.cpp +++ b/test/gtest_factor_odom_3d.cpp @@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 3); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta); +FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), problem_ptr->stateZero()); +FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), delta); // Capture, feature and factor from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr); diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp index e3107b7a5f976f5626f22016f64f60695e1fc982..aefb16b7ded2f0c790449c29ac1d369f20e4f512 100644 --- a/test/gtest_factor_pose_2d.cpp +++ b/test/gtest_factor_pose_2d.cpp @@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO", 2); CeresManager ceres_mgr(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero()); +FrameBasePtr frm0 = problem->emplaceKeyFrame(TimeStamp(0), problem->stateZero()); // Capture, feature and factor from frm1 to frm0 auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr); diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp index d57aee85c27b47e41f9610040c3b8bcceb57ad35..4e7b1c4b3f521327c6b85a8265f13ee77d3b0fe0 100644 --- a/test/gtest_factor_pose_3d.cpp +++ b/test/gtest_factor_pose_3d.cpp @@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO", 3); CeresManager ceres_mgr(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() ); +FrameBasePtr frm0 = problem->emplaceKeyFrame(0, problem->stateZero() ); // Capture, feature and factor auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr); diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp index c461bbb94743c9ee8b3665650fb00686d979fc20..8acd170b165d3de098a17e937f8dcc24bda7fce1 100644 --- a/test/gtest_factor_relative_2d_analytic.cpp +++ b/test/gtest_factor_relative_2d_analytic.cpp @@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero()); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero()); +FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, Vector3d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1.0, Vector3d::Zero()); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov); diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp index b9ad5bbe208d981a4ca69aa3eab8eac806f62e14..8dab63ee647ad5e15abc67e3472e6178465b6212 100644 --- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp +++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp @@ -27,8 +27,8 @@ auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>()); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, Vector3d::Zero() ); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1, Vector3d::Zero() ); +FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, Vector3d::Zero() ); +FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1, Vector3d::Zero() ); // Capture from frm1 to frm0 auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index df0c7ce316ecef3897f990a24a970a38ef042d11..a6c8592ea5548eaacb5da8a409a5bf84e9590eba 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -69,8 +69,8 @@ TEST(FrameBase, LinksToTree) 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 F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); + auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr); WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size()); auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>()); diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 8ef700a29dca74d56baeee8e7869c7fb04f67947..ff8886d4490841a11ce64d59f66b4a23a0337d36 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -132,14 +132,14 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) // KF1 and motion from KF0 t += dt; - FrameBasePtr F1 = Pr->emplaceFrame(KEY, t, Vector3d::Zero()); + FrameBasePtr F1 = Pr->emplaceKeyFrame(t, Vector3d::Zero()); auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t); auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov); auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false); // KF2 and motion from KF1 t += dt; - FrameBasePtr F2 = Pr->emplaceFrame(KEY, t, Vector3d::Zero()); + FrameBasePtr F2 = Pr->emplaceKeyFrame(t, Vector3d::Zero()); auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t); auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov); auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false); @@ -423,7 +423,7 @@ TEST(Odom2d, KF_callback) std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; Vector3d x_split = processor_odom2d->getState(t_split).vector("PO"); - FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split); + FrameBasePtr keyframe_2 = problem->emplaceKeyFrame(t_split, x_split); // there must be 2KF and one F ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 2); @@ -460,7 +460,7 @@ TEST(Odom2d, KF_callback) problem->print(4,1,1,1); x_split = processor_odom2d->getState(t_split).vector("PO"); - FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split); + FrameBasePtr keyframe_1 = problem->emplaceKeyFrame(t_split, x_split); ASSERT_TRUE(problem->check(0)); processor_odom2d->keyFrameCallback(keyframe_1, dt/2); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 7778f7b46b98a94c204e67f65bc3ef72442388d8..03189b8b933d25ba6a0690137e30de9e4ce60fdb 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -223,9 +223,9 @@ TEST(Problem, emplaceFrame_factory) { ProblemPtr P = Problem::create("PO", 2); - FrameBasePtr f0 = P->emplaceFrame(KEY, 0, "PO" , 2, VectorXd(3) ); - FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) ); - FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) ); + FrameBasePtr f0 = P->emplaceKeyFrame(0, "PO" , 2, VectorXd(3) ); + FrameBasePtr f1 = P->emplaceKeyFrame(1, "PO" , 3, VectorXd(7) ); + FrameBasePtr f2 = P->emplaceKeyFrame(2, "POV", 3, VectorXd(10) ); // std::cout << "f0: type PO 2d? " << f0->getType() << std::endl; // std::cout << "f1: type PO 3d? " << f1->getType() << std::endl; @@ -264,7 +264,7 @@ TEST(Problem, StateBlocks) auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); // 2 state blocks, estimated - auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d ); + auto KF = P->emplaceKeyFrame(0, "PO", 3, xs3d ); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // Notifications @@ -323,7 +323,7 @@ TEST(Problem, Covariances) // 4 state blocks, estimated St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d ); + FrameBasePtr F = P->emplaceKeyFrame(0, "PO", 3, xs3d ); // set covariance (they are not computed without a solver) P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity()); @@ -359,7 +359,7 @@ TEST(Problem, perturb) int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(KEY, t, pose ); + auto F = problem->emplaceKeyFrame(t, pose ); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) @@ -449,7 +449,7 @@ TEST(Problem, check) int i = 0; for (TimeStamp t = 0.0; t < 2.0; t += 1.0) { - auto F = problem->emplaceFrame(KEY, t, pose); + auto F = problem->emplaceKeyFrame(t, pose); if (i==0) F->fix(); for (int j = 0; j< 2 ; j++) diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp index 9ca6f9850a22cafef2fc58168c1351910b07ebe2..5075ab48fca33e23e94be57a8ae7d91bf4b463bd 100644 --- a/test/gtest_processor_loopclosure.cpp +++ b/test/gtest_processor_loopclosure.cpp @@ -87,7 +87,7 @@ TEST(ProcessorLoopClosure, installProcessor) // new KF t += dt; - auto kf = problem->emplaceFrame(KEY, t, x); //KF2 + auto kf = problem->emplaceKeyFrame(t, x); //KF2 // emplace a capture in KF auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc); proc_lc->captureCallback(capt_lc); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 382f08108aa7fcf503fddd53730382e11bae0b93..384ca142cee72d01b58275f1faf664b3d89616f9 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -431,7 +431,7 @@ TEST(SolverManager, AddFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); @@ -458,7 +458,7 @@ TEST(SolverManager, RemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); @@ -491,7 +491,7 @@ TEST(SolverManager, AddRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); @@ -526,7 +526,7 @@ TEST(SolverManager, DoubleRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero() ); + FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero() ); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); @@ -572,7 +572,7 @@ TEST(SolverManager, MultiThreadingTruncatedNotifications) while (true) { // Emplace Frame, Capture, feature and factor pose 2d - FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero()); + FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero()); auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity()); auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp index 689c167522f7884d27b3e9fdde5e88fb01ba7b27..fed1010e8d193c15e4bf247e24a98464340081e4 100644 --- a/test/gtest_track_matrix.cpp +++ b/test/gtest_track_matrix.cpp @@ -37,11 +37,11 @@ class TrackMatrixTest : public testing::Test // unlinked frames // Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer - F0 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 0.0, nullptr); - F1 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 1.0, nullptr); - F2 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 2.0, nullptr); - F3 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 3.0, nullptr); - F4 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 4.0, nullptr); + F0 = FrameBase::createNonKeyFrame<FrameBase>(0.0, nullptr); + F1 = FrameBase::createNonKeyFrame<FrameBase>(1.0, nullptr); + F2 = FrameBase::createNonKeyFrame<FrameBase>(2.0, nullptr); + F3 = FrameBase::createNonKeyFrame<FrameBase>(3.0, nullptr); + F4 = FrameBase::createNonKeyFrame<FrameBase>(4.0, nullptr); // unlinked features // Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index b8fc8608ccc1118bce118b28a7a9957ad688e16a..bb13105d9843e3747967e5ed938347bb5a3bb9f7 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -32,10 +32,15 @@ TEST(TrajectoryBase, ClosestKeyFrame) // 1 2 3 4 time stamps // --+-----+-----+-----+---> time - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() ); - FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero() ); - FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() ); - FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, 4, Eigen::Vector3d::Zero() ); + FrameBasePtr F1 = P->emplaceKeyFrame( 1, Eigen::Vector3d::Zero() ); + FrameBasePtr F2 = P->emplaceKeyFrame( 2, Eigen::Vector3d::Zero() ); + // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() ); + FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(), + P->getDim(), + Eigen::Vector3d::Zero() ); + FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(4, P->getFrameStructure(), + P->getDim(), + Eigen::Vector3d::Zero() ); FrameBasePtr KF; // closest key-frame queried @@ -75,21 +80,23 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; // add F1 - FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed + FrameBasePtr F1 = P->emplaceKeyFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); std::cout << __LINE__ << std::endl; // add F2 - FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not + FrameBasePtr F2 = P->emplaceKeyFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); std::cout << __LINE__ << std::endl; // add F3 - FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero()); + FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(), + P->getDim(), + Eigen::Vector3d::Zero()); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index 507d2104e14bf43f57d563dc4873481322f57090..d914b2f7cc19212c5976418008ee9e73a51dbe0f 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback) ASSERT_EQ(GM->n_KF_, 0); - auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) ); + auto F0 = P->emplaceKeyFrame(0, "PO", 3, VectorXd(7) ); P->keyFrameCallback(F0, nullptr, 0); ASSERT_EQ(GM->n_KF_, 1); diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index 8909987f091831fddc686c585e827035ce629ebd..f06578b483e8fc32bc7adec6020ae9effc981697 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/test/gtest_tree_manager_sliding_window.cpp @@ -87,7 +87,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) Matrix6d cov = Matrix6d::Identity(); // FRAME 2 ---------------------------------------------------------- - auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state); + auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state); P->keyFrameCallback(F2, nullptr, 0); // absolute factor @@ -103,7 +103,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) ASSERT_FALSE(F1->isRemoving()); // FRAME 3 ---------------------------------------------------------- - auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state); + auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state); P->keyFrameCallback(F3, nullptr, 0); // absolute factor @@ -119,7 +119,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) ASSERT_FALSE(F1->isRemoving()); // FRAME 4 ---------------------------------------------------------- - auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state); + auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state); P->keyFrameCallback(F4, nullptr, 0); // absolute factor @@ -139,7 +139,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral) ASSERT_TRUE(F2->isFixed()); //Fixed // FRAME 5 ---------------------------------------------------------- - auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state); + auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state); P->keyFrameCallback(F5, nullptr, 0); // absolute factor @@ -183,7 +183,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) Matrix6d cov = Matrix6d::Identity(); // FRAME 2 ---------------------------------------------------------- - auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state); + auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state); P->keyFrameCallback(F2, nullptr, 0); // absolute factor @@ -199,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) ASSERT_FALSE(F1->isRemoving()); // FRAME 3 ---------------------------------------------------------- - auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state); + auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state); P->keyFrameCallback(F3, nullptr, 0); // absolute factor @@ -215,7 +215,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) ASSERT_FALSE(F1->isRemoving()); // FRAME 4 ---------------------------------------------------------- - auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state); + auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state); P->keyFrameCallback(F4, nullptr, 0); // absolute factor @@ -235,7 +235,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) ASSERT_FALSE(F2->isFixed()); //Not fixed // FRAME 5 ---------------------------------------------------------- - auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state); + auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state); P->keyFrameCallback(F5, nullptr, 0); // absolute factor