diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index 7eabd88301bad8fdb4e5297e259c410353241f32..73bd7d2eb09c7f1cc9222e26f6f54f2acb14d820 100644 --- a/hello_wolf/hello_wolf.cpp +++ b/hello_wolf/hello_wolf.cpp @@ -225,7 +225,7 @@ int main() if (sb && !sb->isFixed()) sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! for (auto kf : problem->getTrajectory()->getFrameList()) - if (kf->isKey()) + if (kf->isEstimated()) for (auto sb : kf->getStateBlockVec()) if (sb && !sb->isFixed()) sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! @@ -245,7 +245,7 @@ int main() WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto kf : problem->getTrajectory()->getFrameList()) - if (kf->isKey()) + if (kf->isEstimated()) { Eigen::MatrixXs cov; kf->getCovariance(cov); diff --git a/include/base/frame_base.h b/include/base/frame_base.h index 0177bd8e9253dbd9840ec8fe3a765720a0754fc6..845bad9634bda5147c11b8f91d73e77144029c9f 100644 --- a/include/base/frame_base.h +++ b/include/base/frame_base.h @@ -22,8 +22,9 @@ namespace wolf { */ typedef enum { - NON_KEY_FRAME = 0, ///< Regular frame. It does not play at optimization. - KEY_FRAME = 1 ///< key frame. It plays at optimizations. + KEY_ESTIMATED = 2, ///< estimated key frame. It plays at optimizations. + ESTIMATED = 1, ///< estimated frame. It plays at optimizations. + NON_ESTIMATED = 0 ///< Regular frame. It does not play at optimization. } FrameType; //class FrameBase @@ -38,9 +39,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase static unsigned int frame_id_count_; protected: - unsigned int frame_id_; - FrameType type_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h) - TimeStamp time_stamp_; ///< frame time stamp + unsigned int frame_id_; ///< frame id + FrameType type_; ///< type of frame. Either KEY_ESTIMATED, KEY_ESTIMATED or NON_ESTIMATED. (types defined above) + TimeStamp time_stamp_; ///< frame time stamp public: /** \brief Constructor of non-key Frame with only time stamp @@ -71,9 +72,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase public: unsigned int id(); - // KeyFrame / NonKeyFrame - bool isKey() const; - void setKey(); + // Estimated / Non estimated + bool isEstimated() const; + void setEstimated(); // Frame values ------------------------------------------------ public: @@ -167,9 +168,9 @@ inline unsigned int FrameBase::id() return frame_id_; } -inline bool FrameBase::isKey() const +inline bool FrameBase::isEstimated() const { - return (type_ == KEY_FRAME); + return (type_ == ESTIMATED || type_ == KEY_ESTIMATED); } inline void FrameBase::getTimeStamp(TimeStamp& _ts) const diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 829393980aa4093dfd86438ece963a58eba8a85c..ad59c22406aa8265063c4bac1fa7ad036ef06148 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -81,7 +81,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()->getFrameList()) - if (fr_ptr->isKey()) + if (fr_ptr->isEstimated()) for (auto sb : fr_ptr->getStateBlockVec()) if (sb) all_state_blocks.push_back(sb); @@ -106,7 +106,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks { // first create a vector containing all state blocks for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList()) - if (fr_ptr->isKey()) + if (fr_ptr->isEstimated()) for (auto sb : fr_ptr->getStateBlockVec()) if (sb) for(auto sb2 : fr_ptr->getStateBlockVec()) diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index d4afdc7d00f1805782648bf826159d5ea955d288..e014bcccc1e1d3912bdebe76f9d090a189be2c13 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -247,7 +247,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(ESTIMATED_FRAME, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp index 9e9ca5390a97a42ec61f7ad2a76081fa40e72b5c..ef5697b6458e18fc234c71c12404be5f14a67920 100644 --- a/src/examples/test_ceres_2_lasers_polylines.cpp +++ b/src/examples/test_ceres_2_lasers_polylines.cpp @@ -254,7 +254,7 @@ int main(int argc, char** argv) odom_trajectory.head(3) = ground_truth_pose; // Origin Key Frame - FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); + FrameBasePtr origin_frame = problem.createFrame(ESTIMATED_FRAME, ground_truth_pose, ts); // Prior covariance CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp index 5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6..48998791cf7ce36c865961de6092cadd957738ec 100644 --- a/src/examples/test_factor_imu.cpp +++ b/src/examples/test_factor_imu.cpp @@ -80,7 +80,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED_FRAME, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -141,7 +141,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED_FRAME, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature @@ -199,7 +199,7 @@ int main(int argc, char** argv) //create FrameIMU ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState(); - FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec); + FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED_FRAME, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp index e7ed6a9157f104629a2d6f41d4277e4450763672..0bf823bca5a41c909b55cbd4673f4db719e78ce7 100644 --- a/src/examples/test_imuPlateform_Offline.cpp +++ b/src/examples/test_imuPlateform_Offline.cpp @@ -215,7 +215,7 @@ int main(int argc, char** argv) wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList(); for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isKey()) + if(frm_ptr->isEstimated()) { //prepare needed variables FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr); diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index f134ccc124603660a1d687fe5fa3357427677415..bd958eba3c1a0e04297c7f33f9b87e8815f0e759 100644 --- a/src/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -278,7 +278,7 @@ int main(int argc, char** argv) wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList(); for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isKey()) + if(frm_ptr->isEstimated()) { //prepare needed variables FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr); @@ -324,7 +324,7 @@ int main(int argc, char** argv) for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isKey()) + if(frm_ptr->isEstimated()) { FactorBasePtrList fac_list = frm_ptr->getConstrainedByList(); for(FactorBasePtr fac_ptr : fac_list) diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index d8b22c18d234ff658b22179060a6008ef663e117..ed488dbee366d075b2cbef86cfc24539996b8fc2 100644 --- a/src/examples/test_processor_tracker_landmark_image.cpp +++ b/src/examples/test_processor_tracker_landmark_image.cpp @@ -98,7 +98,7 @@ int main(int argc, char** argv) //===================================================== // Origin Key Frame is fixed TimeStamp t = 0; - FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); + FrameBasePtr origin_frame = problem->emplaceFrame(ESTIMATED_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t); problem->getProcessorMotion()->setOrigin(origin_frame); origin_frame->fix(); diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp index 142b9980846a38c123e01a61c4df2513f724729e..416f4cddff28dc5ba2fed5bec087adc6c19fe10f 100644 --- a/src/examples/test_simple_AHP.cpp +++ b/src/examples/test_simple_AHP.cpp @@ -97,11 +97,11 @@ int main(int argc, char** argv) /* 1 */ ProblemPtr problem = Problem::create("PO 3D"); // One anchor frame to define the lmk, and a copy to make a factor - FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_1 = problem->emplaceFrame(ESTIMATED_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_2 = problem->emplaceFrame(ESTIMATED_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); // and two other frames to observe the lmk - FrameBasePtr kf_3 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_4 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_3 = problem->emplaceFrame(ESTIMATED_FRAME,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_4 = problem->emplaceFrame(ESTIMATED_FRAME,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0)); kf_1->fix(); kf_2->fix(); diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp index e046765f0bd18cb289a4137f452c581440ab08c8..0e4e6999479cab7bb4a31a2c7d55dbbf82a76bfc 100644 --- a/src/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -24,29 +24,29 @@ void printFrames(ProblemPtr _problem_ptr) { std::cout << "TRAJECTORY:" << std::endl; for (auto frm : _problem_ptr->getTrajectory()->getFrameList()) - std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl; + std::cout << "\t " << (frm->isEstimated() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl; } int main() { ProblemPtr problem_ptr = Problem::create(FRM_PO_2D); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); - FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); - FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); - problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); - FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); - FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.1)); + FrameBasePtr frm2 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.2)); + FrameBasePtr frm3 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.3)); + problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.4)); + FrameBasePtr frm5 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.5)); + FrameBasePtr frm6 = problem_ptr->emplaceFrame(NON_ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.6)); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm5->id() << " set KEY" << std::endl; - frm5->setKey(); + frm5->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm2->id() << " set KEY" << std::endl; - frm2->setKey(); + frm2->setEstimated(); printFrames(problem_ptr); @@ -56,21 +56,21 @@ int main() printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm3->id() << " set KEY" << std::endl; - frm3->setKey(); + frm3->setEstimated(); printFrames(problem_ptr); std::cout << std::endl << "Frame " << frm6->id() << " set KEY" << std::endl; - frm6->setKey(); + frm6->setEstimated(); printFrames(problem_ptr); - FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); + FrameBasePtr frm7 = problem_ptr->emplaceFrame(ESTIMATED_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.7)); std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl; printFrames(problem_ptr); - FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); + FrameBasePtr frm8 = problem_ptr->emplaceFrame(ESTIMATED_FRAME, Eigen::VectorXs::Zero(3), TimeStamp(0.35)); std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl; printFrames(problem_ptr); diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp index 7b9e85c085a04dc401097db87be3dcc8ea25e17d..c47258bab530f5276863c441d8bdcfd79e908896 100644 --- a/src/examples/test_sparsification.cpp +++ b/src/examples/test_sparsification.cpp @@ -216,7 +216,7 @@ int main(int argc, char** argv) // ------------------------ START EXPERIMENT ------------------------ // First frame FIXED - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0)); + last_frame_ptr = bl_problem_ptr->emplaceFrame(ESTIMATED_FRAME, Eigen::Vector3s::Zero(),TimeStamp(0)); last_frame_ptr->fix(); bl_problem_ptr->print(4, true, false, true); @@ -238,7 +238,7 @@ int main(int argc, char** argv) Eigen::Vector3s from_pose = frame_from_ptr->getState(); R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix(); Eigen::Vector3s new_frame_pose = from_pose + R*meas; - last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY_FRAME, new_frame_pose, TimeStamp(double(edge_to))); + last_frame_ptr = bl_problem_ptr->emplaceFrame(ESTIMATED_FRAME, new_frame_pose, TimeStamp(double(edge_to))); frame_to_ptr = last_frame_ptr; diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp index 1113af8b7f7e9bd40033ed60e182cc2f485eee99..5c774f7754151d151d2844a9e895e7b84f444eb9 100644 --- a/src/examples/test_wolf_imported_graph.cpp +++ b/src/examples/test_wolf_imported_graph.cpp @@ -130,8 +130,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(ESTIMATED_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp index f3ed29007190ada48bba1758a1ac4e57843ff423..894e2d54bfb3002b39bd60e3107372596ebaa130 100644 --- a/src/examples/test_wolf_prunning.cpp +++ b/src/examples/test_wolf_prunning.cpp @@ -148,8 +148,8 @@ int main(int argc, char** argv) bNum.clear(); // add frame to problem - FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_full = new FrameBase(ESTIMATED_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); + FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full); wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun); // store diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 32c65515203711405d50b7b95946a76d3dbe10bd..b0ab35f5bc6b7bfc2ec377bdc605c720749cb151 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -16,7 +16,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ trajectory_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), - type_(NON_KEY_FRAME), + type_(NON_ESTIMATED), time_stamp_(_ts) { state_block_vec_[0] = _p_ptr; @@ -39,7 +39,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr FrameBase::~FrameBase() { - if ( isKey() ) + if ( isEstimated() ) removeStateBlocks(); } @@ -68,7 +68,7 @@ void FrameBase::remove() } // Remove Frame State Blocks - if ( isKey() ) + if ( isEstimated() ) removeStateBlocks(); if (getTrajectory()->getLastKeyFrame()->id() == this_F->id()) @@ -81,7 +81,7 @@ void FrameBase::remove() void FrameBase::setTimeStamp(const TimeStamp& _ts) { time_stamp_ = _ts; - if (isKey() && getTrajectory() != nullptr) + if (isEstimated() && getTrajectory() != nullptr) getTrajectory()->sortFrame(shared_from_this()); } @@ -111,11 +111,11 @@ void FrameBase::removeStateBlocks() } } -void FrameBase::setKey() +void FrameBase::setEstimated() { - if (type_ != KEY_FRAME) + if (type_ == NON_ESTIMATED ) { - type_ = KEY_FRAME; + type_ = ESTIMATED; registerNewStateBlocks(); if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_) @@ -170,7 +170,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state) for (StateBlockPtr sb : state_block_vec_) if (sb && (index < _st_size)) { - sb->setState(_state.segment(index, sb->getSize()), isKey()); + sb->setState(_state.segment(index, sb->getSize()), isEstimated()); index += sb->getSize(); } } diff --git a/src/problem.cpp b/src/problem.cpp index 4fe1df930f72885805988ad8038bbe760e865982..b8d4f63d8154f58463969b2466c6e942f5746b8d 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -665,7 +665,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: if ( ! prior_is_set_ ) { // Create origin frame - FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts); + FrameBasePtr origin_keyframe = emplaceFrame(ESTIMATED_FRAME, _prior_state, _ts); // create origin capture with the given state as data // Capture fix only takes 3D position and Quaternion orientation @@ -774,10 +774,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) std::cout << " pm" << p->id() << " " << p->getType() << endl; ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p); if (pm->getOrigin()) - cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? " KF" : " F") + cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isEstimated() ? " KF" : " F") << pm->getOrigin()->getFrame()->id() << endl; if (pm->getLast()) - cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? " KF" : " F") + cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? " KF" : " F") << pm->getLast()->getFrame()->id() << endl; if (pm->getIncoming()) cout << " i: C" << pm->getIncoming()->id() << endl; @@ -796,10 +796,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) // << ptt->getPrevOrigin()->getFrame()->id() << endl; // } if (pt->getOrigin()) - cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? " KF" : " F") + cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? " KF" : " F") << pt->getOrigin()->getFrame()->id() << endl; if (pt->getLast()) - cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? " KF" : " F") + cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? " KF" : " F") << pt->getLast()->getFrame()->id() << endl; if (pt->getIncoming()) cout << " i: C" << pt->getIncoming()->id() << endl; @@ -815,7 +815,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) // Frames ======================================================================================= for (auto F : getTrajectory()->getFrameList()) { - cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); + cout << (F->isEstimated() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); if (constr_by) { cout << " <-- "; @@ -1056,7 +1056,7 @@ bool Problem::check(int verbose_level) { if (verbose_level > 0) { - cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl; + cout << (F->isEstimated() ? " KF" : " F") << F->id() << " @ " << F.get() << endl; cout << " -> P @ " << F->getProblem().get() << endl; cout << " -> T @ " << F->getTrajectory().get() << endl; for (auto sb : F->getStateBlockVec()) diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 82fa41c137c0211bb78a699125d287118edc1775..5708e2965e9857bb00e7a2abbb9cc33e7f1fff3e 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -28,7 +28,7 @@ bool ProcessorBase::permittedKeyFrame() FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr) { - std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl; + std::cout << "Making " << (_type == ESTIMATED_FRAME? "key-" : "") << "frame" << std::endl; FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp()); new_frame_ptr->addCapture(_capture_ptr); diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp index 7c4e63a75031de399473c9fa1c6e764fa28331a9..894a8ee8395e91df5ec5f6a9c8e70ff155b27f73 100644 --- a/src/processor/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -72,7 +72,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / // check for LC just if frame is key frame // Assert that the evaluated KF has a capture of the // same sensor as this processor - if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr) + if (key_it->isEstimated() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr) { // Check if the two frames currently evaluated are already // constrained one-another. diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae..69909facd79e5014d64199c23e5d2c63e1241fc2 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -197,7 +197,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) { // Set the frame of last_ptr as key auto key_frame_ptr = last_ptr_->getFrame(); - key_frame_ptr->setKey(); + key_frame_ptr->setEstimated(); // create motion feature and add it to the key_capture auto key_feature_ptr = emplaceFeature(last_ptr_); @@ -206,7 +206,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_); // create a new frame - auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, + auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED, getCurrentState(), getCurrentTimeStamp()); // create a new capture @@ -288,7 +288,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin) { - FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME, _x_origin, _ts_origin); + FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(ESTIMATED_FRAME, _x_origin, _ts_origin); setOrigin(key_frame_ptr); return key_frame_ptr; @@ -299,7 +299,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr."); assert(_origin_frame->getTrajectory() != nullptr && "ProcessorMotion::setOrigin: origin frame must be in the trajectory."); - assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME."); + assert(_origin_frame->isEstimated() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME."); // -------- ORIGIN --------- // emplace (empty) origin Capture @@ -314,7 +314,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) // ---------- LAST ---------- // Make non-key-frame for last Capture - auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, + auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED, _origin_frame->getState(), _origin_frame->getTimeStamp()); // emplace (emtpy) last Capture diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index afdc9446a7f3dc0cc54c8453bc07c190a821f90e..d014713ab1e2cb2f2e955afd664c4a2567022a17 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -75,7 +75,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) } case FIRST_TIME_WITHOUT_PACK : { - FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp()); + FrameBasePtr kfrm = getProblem()->emplaceFrame(ESTIMATED_FRAME, incoming_ptr_->getTimeStamp()); kfrm->addCapture(incoming_ptr_); // Process info @@ -101,7 +101,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) } // @suppress("No break at end of case") case SECOND_TIME_WITHOUT_PACK : { - FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); frm->addCapture(incoming_ptr_); // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF. @@ -137,7 +137,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) pack->key_frame->addCapture(last_ptr_); // Create new frame - FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); frm->addCapture(incoming_ptr_); // Detect new Features, initialize Landmarks, create Factors, ... @@ -171,10 +171,10 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // set KF on last last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - last_ptr_->getFrame()->setKey(); + last_ptr_->getFrame()->setEstimated(); // make F; append incoming to new F - FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp()); + FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); frm->addCapture(incoming_ptr_); // process @@ -258,7 +258,7 @@ void ProcessorTracker::computeProcessingStep() if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance)) { - if (last_ptr_->getFrame()->isKey()) + if (last_ptr_->getFrame()->isEstimated()) { WOLF_WARN("||*||"); WOLF_INFO(" ... It seems you missed something!"); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index a30f777fc109da531c0d56237a650ccad0090bf5..90849e432011b6c41fb98a9cf93d30f1cd6fa960 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -226,7 +226,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) FrameBaseRevIter frame_rev_it = frame_list.rbegin(); while (frame_rev_it != frame_list.rend()) { - if ((*frame_rev_it)->isKey()) + if ((*frame_rev_it)->isEstimated()) { capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); if (capture) diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index 5820b99ba5df29f281ee2e4e61c82368a93d3561..6a645fb5a19280d99e59602986c9161c96d5d387 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -22,7 +22,7 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) _frame_ptr->setTrajectory(shared_from_this()); _frame_ptr->setProblem(getProblem()); - if (_frame_ptr->isKey()) + if (_frame_ptr->isEstimated()) { _frame_ptr->registerNewStateBlocks(); if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp()) @@ -63,7 +63,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place) FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr) { for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) - if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKey() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp()) + if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isEstimated() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp()) return frm_rit.base(); return getFrameList().begin(); } @@ -72,7 +72,7 @@ FrameBasePtr TrajectoryBase::findLastKeyFrame() { // NOTE: Assumes keyframes are sorted by timestamp for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit) - if ((*frm_rit)->isKey()) + if ((*frm_rit)->isEstimated()) return (*frm_rit); return nullptr; @@ -84,7 +84,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) Scalar min_dt = 1e9; for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) - if ((*frm_rit)->isKey()) + if ((*frm_rit)->isEstimated()) { Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); if (dt < min_dt) diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index 7487eb2fe49ada12c03120ce83810abf3d0d8943..0a78f0888b6331a46bd2d6f52c5774cdcef8c1ed 100644 --- a/test/gtest_IMU.cpp +++ b/test/gtest_IMU.cpp @@ -480,7 +480,7 @@ class Process_Factor_IMU : public testing::Test virtual void buildProblem() { // ===================================== SET KF in Wolf tree - FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t); + FrameBasePtr KF = problem->emplaceFrame(ESTIMATED_FRAME, x1_exact, t); // ===================================== IMU CALLBACK problem->keyFrameCallback(KF, nullptr, dt/2); diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index cbe91a6fdf1c276f55bf50e9c32451f0618feb5b..67221f23c3dd661de1e7265e8ba974d8765a6f52 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_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -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_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -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_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -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_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -422,7 +422,7 @@ TEST(CeresManager, DoubleRemoveFactor) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -549,7 +549,7 @@ TEST(CeresManager, FactorsRemoveLocalParam) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); @@ -591,7 +591,7 @@ TEST(CeresManager, FactorsUpdateLocalParam) CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); // Create (and add) 2 factors quaternion - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO()))); diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp index 204c6ce633a36380d46eacbc65d66abe1a7747fe..2c1eb508b40534208cba0e4f54ee2ec945ec8032 100644 --- a/test/gtest_factor_absolute.cpp +++ b/test/gtest_factor_absolute.cpp @@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV 3D"); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0)); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(ESTIMATED_FRAME, problem_ptr->zeroState(), TimeStamp(0)); // Capture, feature and factor CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("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 7559bfa7582aa1449232391806e498fb294d03be..27f5cd934144c2d81ec115c91a07c3a6db2c1774 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 3D"); ceres_manager = std::make_shared<CeresManager>(problem); - F1 = problem->emplaceFrame (KEY_FRAME, pose1, 1.0); + F1 = problem->emplaceFrame (ESTIMATED_FRAME, pose1, 1.0); - F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0); + F2 = problem->emplaceFrame (ESTIMATED_FRAME, pose2, 2.0); C2 = std::make_shared<CaptureBase>("Base", 1.0); F2->addCapture(C2); f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov); diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp index 71401381fe35c768d047b8324e0b2e2f74df35c4..c25ab6e8abbb30631464a09501521e8f9aa1b3a8 100644 --- a/test/gtest_factor_autodiff_trifocal.cpp +++ b/test/gtest_factor_autodiff_trifocal.cpp @@ -133,19 +133,19 @@ class FactorAutodiffTrifocalTest : public testing::Test{ Vector2s pix(0,0); Matrix2s pix_cov; pix_cov.setIdentity(); - F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0); + F1 = problem->emplaceFrame(ESTIMATED_FRAME, pose1, 1.0); I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1)); F1-> addCapture(I1); f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin I1-> addFeature(f1); - F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0); + F2 = problem->emplaceFrame(ESTIMATED_FRAME, pose2, 2.0); I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1)); F2-> addCapture(I2); f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin I2-> addFeature(f2); - F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0); + F3 = problem->emplaceFrame(ESTIMATED_FRAME, pose3, 3.0); I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1)); F3-> addCapture(I3); f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp index c3f767d56ea04682d2e33651c4e4e1a89f865585..57ebd34819730a22449e821b3171b0616bc89552 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 3D"); CeresManager ceres_mgr(problem_ptr); // Two frames -FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0)); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1)); +FrameBasePtr frm0 = problem_ptr->emplaceFrame(ESTIMATED_FRAME, problem_ptr->zeroState(), TimeStamp(0)); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(ESTIMATED_FRAME, delta, TimeStamp(1)); // Capture, feature and factor from frm1 to frm0 CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr)); diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp index d7bce0c84f734613f1a2f143bebf2690897149f2..fc8c8464a03189bb03d4694bb44dcc94495bcea3 100644 --- a/test/gtest_factor_pose_2D.cpp +++ b/test/gtest_factor_pose_2D.cpp @@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO 2D"); CeresManager ceres_mgr(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); +FrameBasePtr frm0 = problem->emplaceFrame(ESTIMATED_FRAME, problem->zeroState(), TimeStamp(0)); // Capture, feature and factor from frm1 to frm0 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr)); diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..599cc990b93158e7b4013c3162257a9b99fd1a09 100644 --- a/test/gtest_factor_pose_3D.cpp +++ b/test/gtest_factor_pose_3D.cpp @@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO 3D"); CeresManager ceres_mgr(problem); // Two frames -FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0)); +FrameBasePtr frm0 = problem->emplaceFrame(ESTIMATED_FRAME, problem->zeroState(), TimeStamp(0)); // Capture, feature and factor CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr)); diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 82828c6c274a3ed2e72d53f15e8f625583644697..707c9b7b9ac00badee9cf3b627b56007bfddda20 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -77,7 +77,7 @@ class FeatureIMU_test : public testing::Test //create Frame ts = problem->getProcessorMotion()->getBuffer().get().back().ts_; state_vec = problem->getProcessorMotion()->getCurrentState(); - last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); + last_frame = std::make_shared<FrameBase>(ESTIMATED_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3))); problem->getTrajectory()->addFrame(last_frame); //create a feature @@ -119,9 +119,9 @@ TEST_F(FeatureIMU_test, check_frame) origin_frame->getTimeStamp(ts); ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl; - ASSERT_TRUE(origin_frame->isKey()); - ASSERT_TRUE(last_frame->isKey()); - ASSERT_TRUE(left_frame->isKey()); + ASSERT_TRUE(origin_frame->isEstimated()); + ASSERT_TRUE(last_frame->isEstimated()); + ASSERT_TRUE(left_frame->isEstimated()); wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr; origin_pptr = origin_frame->getP(); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 06436282a934f4b61bcfe04e50c77b1f3f27eea8..028597e734d4b63000041b17347a9e26bb96c849 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -31,7 +31,7 @@ TEST(FrameBase, GettersAndSetters) F->getTimeStamp(t); ASSERT_EQ(t, 1); ASSERT_FALSE(F->isFixed()); - ASSERT_EQ(F->isKey(), false); + ASSERT_EQ(F->isEstimated(), false); } TEST(FrameBase, StateBlocks) @@ -121,8 +121,8 @@ TEST(FrameBase, LinksToTree) ASSERT_TRUE(F1->isFixed()); // set key - F1->setKey(); - ASSERT_TRUE(F1->isKey()); + F1->setEstimated(); + ASSERT_TRUE(F1->isEstimated()); // Unlink F1->unlinkCapture(C); diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index df5d418e017d2862941212fd3f94ff173fdaf13a..3890a0f1e3c2fb12ebdd429b58b2281280ffa3f7 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -75,7 +75,7 @@ void show(const ProblemPtr& problem) for (FrameBasePtr F : problem->getTrajectory()->getFrameList()) { - if (F->isKey()) + if (F->isEstimated()) { cout << "----- Key Frame " << F->id() << " -----" << endl; if (!F->getCaptureList().empty()) @@ -131,7 +131,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) // KF1 and motion from KF0 t += dt; - FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); + FrameBasePtr F1 = Pr->emplaceFrame(ESTIMATED_FRAME, Vector3s::Zero(), t); CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr)); @@ -139,7 +139,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) // KF2 and motion from KF1 t += dt; - FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); + FrameBasePtr F2 = Pr->emplaceFrame(ESTIMATED_FRAME, Vector3s::Zero(), t); CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr)); @@ -404,7 +404,7 @@ TEST(Odom2D, KF_callback) std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl; Vector3s x_split = processor_odom2d->getState(t_split); - FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); + FrameBasePtr keyframe_2 = problem->emplaceFrame(ESTIMATED_FRAME, x_split, t_split); ASSERT_TRUE(problem->check(0)); processor_odom2d->keyFrameCallback(keyframe_2, dt/2); @@ -436,7 +436,7 @@ TEST(Odom2D, KF_callback) problem->print(4,1,1,1); x_split = processor_odom2d->getState(t_split); - FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY_FRAME, x_split, t_split); + FrameBasePtr keyframe_1 = problem->emplaceFrame(ESTIMATED_FRAME, x_split, t_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 e7a1d6a32d705fef8c5085ef475e3a358a4624e7..52ce84f5670b54b3df1daf00829968f87db696c7 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -177,9 +177,9 @@ TEST(Problem, emplaceFrame_factory) { ProblemPtr P = Problem::create("PO 2D"); - FrameBasePtr f0 = P->emplaceFrame("PO 2D", KEY_FRAME, VectorXs(3), TimeStamp(0.0)); - FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY_FRAME, VectorXs(7), TimeStamp(1.0)); - FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY_FRAME, VectorXs(10), TimeStamp(2.0)); + FrameBasePtr f0 = P->emplaceFrame("PO 2D", ESTIMATED_FRAME, VectorXs(3), TimeStamp(0.0)); + FrameBasePtr f1 = P->emplaceFrame("PO 3D", ESTIMATED_FRAME, VectorXs(7), TimeStamp(1.0)); + FrameBasePtr f2 = P->emplaceFrame("POV 3D", ESTIMATED_FRAME, VectorXs(10), TimeStamp(2.0)); // std::cout << "f0: type PO 2D? " << f0->getType() << std::endl; // std::cout << "f1: type PO 3D? " << f1->getType() << std::endl; @@ -225,7 +225,7 @@ TEST(Problem, StateBlocks) ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); // 2 state blocks, estimated - P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); + P->emplaceFrame("PO 3D", ESTIMATED_FRAME, xs, 0); ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2)); ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2)); @@ -260,7 +260,7 @@ TEST(Problem, Covariances) // 4 state blocks, estimated St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); + FrameBasePtr F = P->emplaceFrame("PO 3D", ESTIMATED_FRAME, xs, 0); // set covariance (they are not computed without a solver) P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity()); diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index 425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3..d9d45a241795c16df77fb1c852d16dd4638dc4d7 100644 --- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp +++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp @@ -72,19 +72,19 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>()); // create Keyframes - F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F1 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED_FRAME, 1, stateblock_pptr1, stateblock_optr1); - F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F2 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED_FRAME, 1, stateblock_pptr2, stateblock_optr2); - F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F3 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED_FRAME, 1, stateblock_pptr3, stateblock_optr3); - F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F4 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED_FRAME, 1, stateblock_pptr4, stateblock_optr4); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03..a93bf8d90dd250b8658d25ab75a9e697ff30b297 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -505,7 +505,7 @@ TEST(SolverManager, AddFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -527,7 +527,7 @@ TEST(SolverManager, RemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -555,7 +555,7 @@ TEST(SolverManager, AddRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); @@ -584,7 +584,7 @@ TEST(SolverManager, DoubleRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(ESTIMATED_FRAME, P->zeroState(), TimeStamp(0)); CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f))); diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index d77defcbc697d87cf827a465d1ef99e9221293a6..618ff1fc54df6f95029a40d222c5d71b4debed93 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -69,9 +69,9 @@ TEST(TrajectoryBase, ClosestKeyFrame) // 1 2 3 time stamps // --+-----+-----+---> time - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, nullptr, nullptr); - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, nullptr, nullptr); - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr); + FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 1, nullptr, nullptr); + FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 2, nullptr, nullptr); + FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr); T->addFrame(f1); T->addFrame(f2); T->addFrame(f3); @@ -108,9 +108,9 @@ TEST(TrajectoryBase, Add_Remove_Frame) // 1 2 3 time stamps // --+-----+-----+---> time - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed + FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not + FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame std::cout << __LINE__ << std::endl; @@ -189,9 +189,9 @@ TEST(TrajectoryBase, KeyFramesAreSorted) // 1 2 3 time stamps // --+-----+-----+---> time - FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed + FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED_FRAME, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not + FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame // add frames and keyframes in random order --> keyframes must be sorted after that T->addFrame(f2); // KF2 @@ -209,12 +209,12 @@ TEST(TrajectoryBase, KeyFramesAreSorted) ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); - f3->setKey(); // set KF3 + f3->setEstimated(); // set KF3 if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); - FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + FrameBasePtr f4 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame T->addFrame(f4); // Trajectory status: // kf1 kf2 kf3 f4 frames @@ -224,7 +224,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted) ASSERT_EQ(T->getLastFrame() ->id(), f4->id()); ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); - f4->setKey(); + f4->setEstimated(); // Trajectory status: // kf1 kf4 kf2 kf3 frames // 1 1.5 2 3 time stamps