diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index b1309f336c0f8979d5cc4fa8d57aad37de2a00dc..1089e4ae64f96db351e5f2aee74423b26642f0ed 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -234,7 +234,7 @@ int main() WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto& kf : *problem->getTrajectory()) - if (kf->isKeyOrAux()) + if (kf->isKey()) { Eigen::MatrixXd cov; kf->getCovariance(cov); diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index cad0474fa06cc72c6df41d8f95f20153c15fa06e..e974a19c11b3a271d63e3759d8a4735b13447cae 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -219,7 +219,7 @@ int main() WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto& kf : *problem->getTrajectory()) - if (kf->isKeyOrAux()) + if (kf->isKey()) { Eigen::MatrixXd cov; kf->getCovariance(cov); diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 14a8f14b799ab39abbccf2e11ded6da84bd2bfda..d3b4ae34704f3eb7bb6b2abc30eb15097c38acab 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -13,7 +13,7 @@ class StateBlock; typedef enum { KEY = 2, ///< key frame. It plays at optimizations (estimated). - AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated). + // AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated). NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization. } FrameType; } @@ -83,13 +83,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha // get type bool isKey() const; - bool isAux() const; - bool isKeyOrAux() const; // set type void setNonEstimated(); void setKey(ProblemPtr _prb); - void setAux(); // Frame values ------------------------------------------------ public: @@ -195,16 +192,6 @@ inline bool FrameBase::isKey() const return (type_ == KEY); } -inline bool FrameBase::isAux() const -{ - return (type_ == AUXILIARY); -} - -inline bool FrameBase::isKeyOrAux() const -{ - return (type_ == KEY || type_ == AUXILIARY); -} - inline void FrameBase::getTimeStamp(TimeStamp& _ts) const { _ts = time_stamp_; @@ -243,7 +230,7 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb) { - if (isKeyOrAux()) + if (isKey()) HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem()); else HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr); diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index ef5bba9a7ad087b08eae31699556af520b71cd32..2c48230e80bcef84517c9fe001854c3164a37ff9 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -252,7 +252,6 @@ class Problem : public std::enable_shared_from_this<Problem> FrameBasePtr getLastKeyFrame( ) const; FrameBasePtr getLastKeyOrAuxFrame( ) const; FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; - FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const; /** \brief Give the permission to a processor to create a new key Frame * diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 9c3729a61c80e868a0b8af2b6421ffdfddc0731d..840c09c103c69def1ea0355926bdaa270ef81c52 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -60,8 +60,8 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj protected: std::string frame_structure_; // Defines the structure of the Frames in the Trajectory. - FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame - FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame + // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame + // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame public: TrajectoryBase(const std::string& _frame_sturcture); @@ -75,14 +75,11 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj FrameBasePtr getLastFrame() const; FrameBasePtr getFirstFrame() const; FrameBasePtr getLastKeyFrame() const; - FrameBasePtr getLastKeyOrAuxFrame() const; FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; - FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const; FrameListIter begin() const; FrameListIter end() const; FrameListRevIter rbegin() const; FrameListRevIter rend() const; - void updateLastFrames(); virtual void printHeader(int depth, // bool constr_by, // @@ -141,12 +138,8 @@ inline FrameListRevIter TrajectoryBase::rend() const inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const { - return last_key_frame_ptr_; -} - -inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const -{ - return last_key_or_aux_frame_ptr_; + // return last_key_frame_ptr_; + return *(this->rbegin()); } inline std::string TrajectoryBase::getFrameStructure() const diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 579223b5df36039f1de51542f34fb69ff6958bbc..f22a7f2ed2fd8ad124e8185fd52720020db1ac65 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -89,7 +89,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks; //frame state blocks for(auto fr_ptr : *wolf_problem_->getTrajectory()) - if (fr_ptr->isKeyOrAux()) + if (fr_ptr->isKey()) for (const auto& key : fr_ptr->getStructure()) { const auto& sb = fr_ptr->getStateBlock(key); @@ -116,7 +116,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks { // first create a vector containing all state blocks for(auto fr_ptr : *wolf_problem_->getTrajectory()) - if (fr_ptr->isKeyOrAux()) + if (fr_ptr->isKey()) for (const auto& key1 : wolf_problem_->getFrameStructure()) for (const auto& key2 : wolf_problem_->getFrameStructure()) { diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index e39265816f31260a4a3c85dee679447f66788cb6..4e6bbf2507603ec9aed2050086878b186b21b5b8 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -145,7 +145,7 @@ void FrameBase::remove(bool viral_remove_empty_parent) } // Remove Frame State Blocks - if ( isKeyOrAux() ) + if ( isKey() ) removeStateBlocks(getProblem()); } } @@ -158,15 +158,11 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts) void FrameBase::setNonEstimated() { // unregister if previously estimated - if (isKeyOrAux()) + if (isKey()) for (const auto& sb : getStateBlockVec()) getProblem()->notifyStateBlock(sb, REMOVE); type_ = NON_ESTIMATED; - if (getTrajectory()) - { - getTrajectory()->updateLastFrames(); - } } void FrameBase::setKey(ProblemPtr _prb) @@ -174,24 +170,6 @@ void FrameBase::setKey(ProblemPtr _prb) assert(_prb != nullptr && "setting Key fram with a null problem pointer"); type_ = KEY; this->link(_prb->getTrajectory()); - - if (getTrajectory()) - { - getTrajectory()->updateLastFrames(); - } -} - -void FrameBase::setAux() -{ - if (!isKeyOrAux()) - registerNewStateBlocks(getProblem()); - - type_ = AUXILIARY; - - if (getTrajectory()) - { - getTrajectory()->updateLastFrames(); - } } bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const @@ -359,7 +337,7 @@ void FrameBase::setProblem(ProblemPtr _problem) void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { - _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : ""); + _stream << _tabs << (isKey() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : ""); if (_constr_by) { _stream << " <-- "; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index d0cbfaef5b142e5b122413981fd02f4caabd9b0d..b3db058da027d07f9697ebc449f99d3b39434a19 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -374,11 +374,12 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c getState(*min_it, _state); _ts = *min_it; } - else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr) + else if (trajectory_ptr_->rbegin() != trajectory_ptr_->rend()) { // kind of redundant with getState(_ts, _state) - trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts); - trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state); + auto last = trajectory_ptr_->rbegin(); + (*last)->getState(_state); + (*last)->getTimeStamp(_ts); } else _state = zeroState(); @@ -389,7 +390,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const { // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp - FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); if (processor_is_motion_list_.empty()){ if (closest_frame != nullptr) _state = closest_frame->getState(); @@ -796,12 +797,6 @@ bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const return getFrameCovariance(frm, cov); } -bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const -{ - FrameBasePtr frm = getLastKeyOrAuxFrame(); - return getFrameCovariance(frm, cov); -} - bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const { bool success(true); @@ -887,21 +882,11 @@ FrameBasePtr Problem::getLastKeyFrame() const return trajectory_ptr_->getLastKeyFrame(); } -FrameBasePtr Problem::getLastKeyOrAuxFrame() const -{ - return trajectory_ptr_->getLastKeyOrAuxFrame(); -} - FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const { return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); } -FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const -{ - return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); -} - void Problem::setPriorOptions(const std::string& _mode, const double _time_tolerance, const Eigen::VectorXd& _state, @@ -1159,7 +1144,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const for (auto F : *(T)) { if (_verbose) { - _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm") + _stream << (F->isKey() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm") << F->id() << " @ " << F.get() << std::endl; _stream << " -> Prb @ " << F->getProblem().get() << std::endl; _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl; @@ -1728,7 +1713,7 @@ void Problem::perturb(double amplitude) // Frames and Captures for (auto& F : *(getTrajectory())) { - if (F->isKeyOrAux()) + if (F->isKey()) { F->perturb(amplitude); for (auto& C : F->getCaptureList()) diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 233e3c0f1f3130511d7ebcf0debd70107ad45bc6..c1945df34593cea047b1b426dfce8b46402216e7 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -726,10 +726,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo { _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm") + _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm" ) << getOrigin()->getFrame()->id() << std::endl; if (getLast()) - _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm") << getLast()->getFrame()->id() << std::endl; if (getIncoming()) _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 947ca38052ee625d2fbb8452c68b2b2dc565619c..24c81bbdc7527e797a58d276baba54f3e8af9a33 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -343,10 +343,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo { _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; if (getOrigin()) - _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm") << getOrigin()->getFrame()->id() << std::endl; if (getLast()) - _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm") + _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm") << getLast()->getFrame()->id() << std::endl; if (getIncoming()) _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl; diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 1cf3a913e95271cd82a90cbb3d139b301ad1deba..77340c7e7f8d9946d200a1a20fbb98915449d8b7 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -5,9 +5,9 @@ namespace wolf { TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) : NodeBase("TRAJECTORY", "TrajectoryBase"), - frame_structure_(_frame_structure), - last_key_frame_ptr_(nullptr), - last_key_or_aux_frame_ptr_(nullptr) + frame_structure_(_frame_structure) + // last_key_frame_ptr_(nullptr), + // last_key_or_aux_frame_ptr_(nullptr) { // WOLF_DEBUG("constructed T"); } @@ -24,11 +24,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) auto success = frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr)); std::cout << "Sucess? " << success.second << std::endl; printDebug(); - if (_frame_ptr->isKeyOrAux()) - { - // update last_estimated and last_key - updateLastFrames(); - } return _frame_ptr; } @@ -38,10 +33,6 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr) // add to list // frame_list_.erase(_frame_ptr); frame_list_.erase(_frame_ptr->getTimeStamp()); - - // update last_estimated and last_key - if (_frame_ptr->isKeyOrAux()) - updateLastFrames(); } void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const @@ -51,27 +42,6 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const } -void TrajectoryBase::updateLastFrames() -{ - bool last_estimated_set = false; - - // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp - for (auto frm_rit = rbegin(); frm_rit != rend(); ++frm_rit) - if ((*frm_rit)->isKeyOrAux()) - { - if (!last_estimated_set) - { - last_key_or_aux_frame_ptr_ = (*frm_rit); - last_estimated_set = true; - } - if ((*frm_rit)->isKey()) - { - last_key_frame_ptr_ = (*frm_rit); - break; - } - } -} - FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const { // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp @@ -79,38 +49,16 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) co double min_dt = 1e9; for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++) - if ((*frm_rit)->isKey()) - { - double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); - if (dt < min_dt) - { - min_dt = dt; - closest_kf = *frm_rit; - } - else - break; - } - return closest_kf; -} - -FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const -{ - // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp - FrameBasePtr closest_kf = nullptr; - double min_dt = 1e9; - - for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++) - if ((*frm_rit)->isKeyOrAux()) + { + double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); + if (dt < min_dt) { - double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); - if (dt < min_dt) - { - min_dt = dt; - closest_kf = *frm_rit; - } - else - break; + min_dt = dt; + closest_kf = *frm_rit; } + else + break; + } return closest_kf; } diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index b8dceb6baa616fb4bced7733ed17da639e9c5082..da0ea77ae239314b8e6b16a94c27c05c9284e98d 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -32,7 +32,7 @@ TEST(FrameBase, GettersAndSetters) ASSERT_EQ(t, 1); ASSERT_FALSE(F->isFixed()); ASSERT_EQ(F->isKey(), false); - ASSERT_EQ(F->isKeyOrAux(), false); + ASSERT_EQ(F->isKey(), false); } TEST(FrameBase, StateBlocks) @@ -114,7 +114,7 @@ TEST(FrameBase, LinksToTree) // set key F1->setKey(P); ASSERT_TRUE(F1->isKey()); - ASSERT_TRUE(F1->isKeyOrAux()); + ASSERT_TRUE(F1->isKey()); } #include "core/state_block/state_quaternion.h" diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index e9951c270aab76a84ed56ff08ebfbf977da8afc7..04cede086256d544656095d2ce89ee5c0d9aef22 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -102,19 +102,19 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame) FrameBasePtr KF; // closest key-frame queried - KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0 + KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0 ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1 + KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1 ASSERT_EQ(KF->id(), F1->id()); // same id! - KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2 + KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestKeyOrAuxFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2 + KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! - KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2 + KF = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return F2 ASSERT_EQ(KF->id(), F2->id()); // same id! } @@ -208,25 +208,25 @@ TEST(TrajectoryBase, KeyFramesAreSorted) FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero()); // non-key-frame if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1); if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); F3->setKey(P); // set KF3 if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); auto F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero()); @@ -236,7 +236,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F4->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); F4->setKey(P); @@ -246,7 +246,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); F2->setTimeStamp(4); @@ -256,7 +256,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); F4->setTimeStamp(0.5); @@ -275,7 +275,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+-----+-----+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); F5->setTimeStamp(5); @@ -285,7 +285,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+-----+-----+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F5->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); auto F6 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 6, Eigen::Vector3d::Zero()); @@ -295,7 +295,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+-----+-----+-----+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F6->id()); - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); auto F7 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 5.5, Eigen::Vector3d::Zero()); @@ -305,7 +305,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // --+-----+-----+-----+-----+-----+-----+---> time if (debug) P->print(2,0,1,0); ASSERT_EQ(T->getLastFrame() ->id(), F7->id()); //Only auxiliary and key-frames are sorted - ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id()); ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); }