diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index 3708f51d3aa1141756dc54c27f76039671bfa42e..c73e295083b71c0237d2569ff90ff0b9191b1192 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->isKeyOrAux()) 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->isKeyOrAux()) { Eigen::MatrixXs cov; kf->getCovariance(cov); diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h index d156fd80521b31da81975ea11f55d9949fbb3754..2bdc1d4bd002deaa4e01edfac6817618fae40951 100644 --- a/include/base/ceres_wrapper/ceres_manager.h +++ b/include/base/ceres_wrapper/ceres_manager.h @@ -56,7 +56,7 @@ class CeresManager : public SolverManager virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; - virtual void computeCovariances(const StateBlockPtrList& st_list) override; + virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override; virtual bool hasConverged() override; diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h index d4945e066a372d5ec95578a87552e4369eec1b2e..39d133d610fb2f497c88c70781cd3c047dba81d2 100644 --- a/include/base/ceres_wrapper/qr_manager.h +++ b/include/base/ceres_wrapper/qr_manager.h @@ -38,7 +38,7 @@ class QRManager : public SolverManager virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS); - virtual void computeCovariances(const StateBlockPtrList& _sb_list); + virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list); private: diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h index ffa84d74d0c7521ce226a5445e58591bfc43be66..f4befb4d11cbc5f54a093e7a6ebcfa221c72b275 100644 --- a/include/base/frame/frame_base.h +++ b/include/base/frame/frame_base.h @@ -18,12 +18,13 @@ class StateBlock; namespace wolf { -/** \brief Enumeration of frame types: key-frame or non-key-frame +/** \brief Enumeration of frame types */ typedef enum { - NON_KEY_FRAME = 0, ///< Regular frame. It does not play at optimization. - KEY_FRAME = 1 ///< key frame. It plays at optimizations. + KEY = 2, ///< key 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; //class FrameBase @@ -71,9 +72,14 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase public: unsigned int id(); - // KeyFrame / NonKeyFrame + // get type bool isKey() const; + bool isAux() const; + bool isKeyOrAux() const; + + // set type void setKey(); + void setAux(); // Frame values ------------------------------------------------ public: @@ -169,7 +175,17 @@ inline unsigned int FrameBase::id() inline bool FrameBase::isKey() const { - return (type_ == KEY_FRAME); + 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 diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h index e6375bff43943585dcdf703155fe6e9ad6697ce2..9c1d768e0d1be2fbac9cf2c9d926e69d8769564e 100644 --- a/include/base/problem/problem.h +++ b/include/base/problem/problem.h @@ -140,7 +140,7 @@ class Problem : public std::enable_shared_from_this<Problem> /** \brief Emplace frame from string frame_structure * \param _frame_structure String indicating the frame structure. - * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME + * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED * \param _frame_state State vector; must match the size and format of the chosen frame structure * \param _time_stamp Time stamp of the frame * @@ -156,7 +156,7 @@ class Problem : public std::enable_shared_from_this<Problem> /** \brief Emplace frame from string frame_structure without state * \param _frame_structure String indicating the frame structure. - * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME + * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: @@ -169,7 +169,7 @@ class Problem : public std::enable_shared_from_this<Problem> const TimeStamp& _time_stamp); /** \brief Emplace frame from string frame_structure without structure - * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME + * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED * \param _frame_state State vector; must match the size and format of the chosen frame structure * \param _time_stamp Time stamp of the frame * @@ -183,7 +183,7 @@ class Problem : public std::enable_shared_from_this<Problem> const TimeStamp& _time_stamp); /** \brief Emplace frame from string frame_structure without structure nor state - * \param _frame_key_type Either KEY_FRAME or NON_KEY_FRAME + * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED * \param _time_stamp Time stamp of the frame * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: @@ -195,26 +195,44 @@ class Problem : public std::enable_shared_from_this<Problem> const TimeStamp& _time_stamp); // Frame getters - FrameBasePtr getLastFrame ( ); - FrameBasePtr getLastKeyFrame ( ); - FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts); + FrameBasePtr getLastFrame( ) const; + 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 keyFrame + /** \brief Give the permission to a processor to create a new key Frame * - * This should implement a black list of processors that have forbidden keyframe creation. + * This should implement a black list of processors that have forbidden key frame creation. * - This decision is made at problem level, not at processor configuration level. - * - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors. + * - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors. */ bool permitKeyFrame(ProcessorBasePtr _processor_ptr); /** \brief New key frame callback * - * New key frame callback: It should be called by any processor that creates a new keyframe. It calls the keyFrameCallback of the rest of processors. + * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors. */ void keyFrameCallback(FrameBasePtr _keyframe_ptr, // ProcessorBasePtr _processor_ptr, // const Scalar& _time_tolerance); + /** \brief Give the permission to a processor to create a new auxiliary Frame + * + * This should implement a black list of processors that have forbidden auxiliary frame creation. + * - This decision is made at problem level, not at processor configuration level. + * - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors. + */ + bool permitAuxFrame(ProcessorBasePtr _processor_ptr); + + /** \brief New auxiliary frame callback + * + * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion. + */ + void auxFrameCallback(FrameBasePtr _frame_ptr, // + ProcessorBasePtr _processor_ptr, // + const Scalar& _time_tolerance); + // State getters Eigen::VectorXs getCurrentState ( ); void getCurrentState (Eigen::VectorXs& state); @@ -242,6 +260,7 @@ class Problem : public std::enable_shared_from_this<Problem> bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0); bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance); bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance); + bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance); bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance); // Solver management ---------------------------------------- diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h index 5d68c43c7b921532dc1d1d0d32ddabe89d75de93..9e149abcbdfca7c94942b92a1669c9c8ee5f6e70 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -110,16 +110,19 @@ struct ProcessorParamsBase ProcessorParamsBase() = default; ProcessorParamsBase(bool _voting_active, - Scalar _time_tolerance) - : voting_active(_voting_active) - , time_tolerance(_time_tolerance) + Scalar _time_tolerance, + bool _voting_aux_active = false) : + voting_active(_voting_active), + voting_aux_active(_voting_aux_active), + time_tolerance(_time_tolerance) { // } virtual ~ProcessorParamsBase() = default; - bool voting_active = false; + bool voting_active = false; ///< Whether this processor is allowed to vote for a Key Frame or not + bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not ///< maximum time difference between a Keyframe time stamp and /// a particular Capture of this processor to allow assigning @@ -159,8 +162,19 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce */ virtual bool voteForKeyFrame() = 0; + /** \brief Vote for Auxiliary Frame generation + * + * If a Auxiliary Frame criterion is validated, this function returns true, + * meaning that it wants to create a Auxiliary Frame at the \b last Capture. + * + * WARNING! This function only votes! It does not create Auxiliary Frames! + */ + virtual bool voteForAuxFrame(){return false;}; + virtual bool permittedKeyFrame() final; + virtual bool permittedAuxFrame() final; + /**\brief make a Frame with the provided Capture * * Provide the following functionality: @@ -192,7 +206,11 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce bool isVotingActive() const; + bool isVotingAuxActive() const; + void setVotingActive(bool _voting_active = true); + + void setVotingAuxActive(bool _voting_active = true); }; inline bool ProcessorBase::isVotingActive() const @@ -200,11 +218,21 @@ inline bool ProcessorBase::isVotingActive() const return params_->voting_active; } +inline bool ProcessorBase::isVotingAuxActive() const +{ + return params_->voting_aux_active; +} + inline void ProcessorBase::setVotingActive(bool _voting_active) { params_->voting_active = _voting_active; } +inline void ProcessorBase::setVotingAuxActive(bool _voting_active) +{ + params_->voting_aux_active = _voting_active; +} + } #include "base/sensor/sensor_base.h" diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h index 28e43acfa63426072e6883b2a3237535775d1fc8..8aefa72135e3897c6a52ac550a131db782ea7860 100644 --- a/include/base/solver/solver_manager.h +++ b/include/base/solver/solver_manager.h @@ -53,7 +53,7 @@ public: virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0; - virtual void computeCovariances(const StateBlockPtrList& st_list) = 0; + virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0; virtual bool hasConverged() = 0; diff --git a/include/base/trajectory/trajectory_base.h b/include/base/trajectory/trajectory_base.h index 37f9762c7c9955381eaba9bc52aef253441d36e1..aff013ea00d0b40af358ad4d3cb980d8298ccc4e 100644 --- a/include/base/trajectory/trajectory_base.h +++ b/include/base/trajectory/trajectory_base.h @@ -26,6 +26,7 @@ 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 public: TrajectoryBase(const std::string& _frame_sturcture); @@ -37,18 +38,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj // Frames FrameBasePtr addFrame(FrameBasePtr _frame_ptr); FrameBasePtrList& getFrameList(); - FrameBasePtr getLastFrame(); - FrameBasePtr getLastKeyFrame(); - FrameBasePtr findLastKeyFrame(); - FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts); - void setLastKeyFrame(FrameBasePtr _key_frame_ptr); + const FrameBasePtrList& getFrameList() const; + FrameBasePtr getLastFrame() const; + FrameBasePtr getLastKeyFrame() const; + FrameBasePtr getLastKeyOrAuxFrame() const; + FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; + FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const; void sortFrame(FrameBasePtr _frm_ptr); - void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place); - FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr); + void updateLastFrames(); // factors void getFactorList(FactorBasePtrList & _fac_list); + protected: + FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr); + void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place); }; inline FrameBasePtrList& TrajectoryBase::getFrameList() @@ -56,19 +60,24 @@ inline FrameBasePtrList& TrajectoryBase::getFrameList() return frame_list_; } -inline FrameBasePtr TrajectoryBase::getLastFrame() +inline const FrameBasePtrList& TrajectoryBase::getFrameList() const +{ + return frame_list_; +} + +inline FrameBasePtr TrajectoryBase::getLastFrame() const { return frame_list_.back(); } -inline FrameBasePtr TrajectoryBase::getLastKeyFrame() +inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const { return last_key_frame_ptr_; } -inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr) +inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const { - last_key_frame_ptr_ = _key_frame_ptr; + return last_key_or_aux_frame_ptr_; } inline std::string TrajectoryBase::getFrameStructure() const diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 5871b8fa12ee6f9ce5c3aff8741e80444ab9e166..5f3906c4c0ae0ec1af82199981c930b2fcabd3c5 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -62,7 +62,7 @@ std::string CeresManager::solveImpl(const ReportVerbosity report_level) } void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks) -{ +{ // update problem update(); @@ -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->isKeyOrAux()) 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->isKeyOrAux()) for (auto sb : fr_ptr->getStateBlockVec()) if (sb) for(auto sb2 : fr_ptr->getStateBlockVec()) @@ -194,17 +194,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { - Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize()); - covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data()); + Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize()); + covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data()); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); - //std::cout << "getted covariance " << std::endl << cov << std::endl; + // std::cout << "covariance got switch: " << std::endl << cov << std::endl; } } else std::cout << "WARNING: Couldn't compute covariances!" << std::endl; } - -void CeresManager::computeCovariances(const StateBlockPtrList& st_list) +void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list) { //std::cout << "CeresManager: computing covariances..." << std::endl; @@ -234,10 +233,10 @@ void CeresManager::computeCovariances(const StateBlockPtrList& st_list) // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { - Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize()); - covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data()); + Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize()); + covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data()); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); - //std::cout << "getted covariance " << std::endl << cov << std::endl; + // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl; } else std::cout << "WARNING: Couldn't compute covariances!" << std::endl; diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index d12f491de7ce0895ca301498454e7f3d451bbd4e..456367a2c67b1c98ca91e9631c3f3da47751882a 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(KEY, 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 136ba285e0d9fb2c62b4801dc75e597dcdd257ff..62ad277f98576b1bf17e17c7515d8dfb428c93be 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(KEY, 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 3d0483e9647f9e3203ab3bb89036de55e0f9adc9..4040194b382e9db0698b2a5d8acf0f852af1286b 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>(KEY, 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>(KEY, 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>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index be7df536e971273559252aeff8dc51bd808bb14b..8baee2c1e75af6ab5a8841e85b082d8f7cdd20c9 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(KEY, (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 6bd1805dc78a0c65bd27ca022fbb46bd272cf6d4..f07a2c1ce3da6eb7168571d3ec1377ffa8b0c388 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(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(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(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); + FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(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 56982c9dc7c6fda44b594a04f92a33f911b8ca49..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644 --- a/src/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -31,22 +31,22 @@ 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(KEY, 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(KEY, 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..03d214a3d09c8a624129243f181aed1dd722b5a9 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(KEY, 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(KEY, 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..40171695f14ac3ef2aeee7091ca53e8de50a2a90 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(KEY, 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, 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..b4d9ac23316dc5450b269d99ce45cbb0f7c0e2f0 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(KEY, 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, 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/frame_base.cpp b/src/frame/frame_base.cpp index cb7f089c1f21adad761f5fd7c13f0eee786b6741..10c75405b377474905b1040251794067d7df115c 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/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,8 +39,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr FrameBase::~FrameBase() { - if ( isKey() ) - removeStateBlocks(); } void FrameBase::remove() @@ -68,11 +66,12 @@ void FrameBase::remove() } // Remove Frame State Blocks - if ( isKey() ) + if ( isKeyOrAux() ) removeStateBlocks(); - if (getTrajectory()->getLastKeyFrame()->id() == this_F->id()) - getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame()); + + if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id()) + getTrajectory()->updateLastFrames(); // std::cout << "Removed F" << id() << std::endl; } @@ -81,7 +80,7 @@ void FrameBase::remove() void FrameBase::setTimeStamp(const TimeStamp& _ts) { time_stamp_ = _ts; - if (isKey() && getTrajectory() != nullptr) + if (isKeyOrAux() && getTrajectory() != nullptr) getTrajectory()->sortFrame(shared_from_this()); } @@ -113,18 +112,25 @@ void FrameBase::removeStateBlocks() void FrameBase::setKey() { - if (type_ != KEY_FRAME) - { - type_ = KEY_FRAME; + // register if previously not estimated + if (!isKeyOrAux()) registerNewStateBlocks(); - if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_) - getTrajectory()->setLastKeyFrame(shared_from_this()); + // WOLF_DEBUG("Set Key", this->id()); + type_ = KEY; + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); +} - getTrajectory()->sortFrame(shared_from_this()); +void FrameBase::setAux() +{ + if (!isKeyOrAux()) + registerNewStateBlocks(); -// WOLF_DEBUG("Set KF", this->id()); - } + // WOLF_DEBUG("Set Auxiliary", this->id()); + type_ = AUXILIARY; + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); } void FrameBase::fix() @@ -170,7 +176,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()), isKeyOrAux()); // do not notify if state block is not estimated by the solver index += sb->getSize(); } } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 34f37364291cc3d0710fb78a890799df87218b1f..17c30ca6bb3b07708c331f85a283b3bc87a7cea7 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -251,8 +251,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state) { if (processor_motion_ptr_ != nullptr) processor_motion_ptr_->getCurrentState(state); - else if (trajectory_ptr_->getLastKeyFrame() != nullptr) - trajectory_ptr_->getLastKeyFrame()->getState(state); + else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr) + trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state); else state = zeroState(); } @@ -264,10 +264,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts) processor_motion_ptr_->getCurrentState(state); processor_motion_ptr_->getCurrentTimeStamp(ts); } - else if (trajectory_ptr_->getLastKeyFrame() != nullptr) + else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr) { - trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts); - trajectory_ptr_->getLastKeyFrame()->getState(state); + trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts); + trajectory_ptr_->getLastKeyOrAuxFrame()->getState(state); } else state = zeroState(); @@ -278,7 +278,7 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state) // try to get the state from processor_motion if any, otherwise... if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state)) { - FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); + FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); if (closest_frame != nullptr) closest_frame->getState(state); else @@ -348,6 +348,30 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro processor->keyFrameCallback(_keyframe_ptr, _time_tolerance); } +bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) +{ + // This should implement a black list of processors that have forbidden auxiliary frame creation + // This decision is made at problem level, not at processor configuration level. + // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors. + + // Currently allowing all processors to vote: + return true; +} + +void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance) +{ + if (_processor_ptr) + { + WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp()); + } + else + { + WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp()); + } + + processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance); +} + LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr) { getMap()->addLandmark(_lmk_ptr); @@ -470,8 +494,8 @@ void Problem::clearCovariance() void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov) { - assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size"); - assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size"); + assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size"); + assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size"); std::lock_guard<std::mutex> lock(mut_covariances_); covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov; @@ -479,8 +503,8 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov) { - assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size"); - assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size"); + assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size"); + assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size"); std::lock_guard<std::mutex> lock(mut_covariances_); covariances_[std::make_pair(_state1, _state1)] = _cov; @@ -492,23 +516,23 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E //std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl; //std::cout << "_row " << _row << std::endl; //std::cout << "_col " << _col << std::endl; - //std::cout << "_state1 size: " << _state1->getSize() << std::endl; - //std::cout << "_state2 size: " << _state2->getSize() << std::endl; - //std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) << std::endl; + //std::cout << "_state1 tangent space size: " << _state1->getLocalSize() << std::endl; + //std::cout << "_state2 tangent space size: " << _state2->getLocalSize() << std::endl; + //std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) << std::endl; //if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end()) // std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl; //else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end()) // std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl; - assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); + assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); std::lock_guard<std::mutex> lock(mut_covariances_); if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end()) - _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) = + _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) = covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)]; else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end()) - _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) = + _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) = covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose(); else { @@ -535,23 +559,23 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx // search st1 & st2 if (covariances_.find(pair_12) != covariances_.end()) { - assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() && - _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); - assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() && - _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); + assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() && + _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); + assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() && + _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); - _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12]; - _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose(); + _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_12]; + _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_12].transpose(); } else if (covariances_.find(pair_21) != covariances_.end()) { - assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() && - _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); - assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() && - _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); + assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() && + _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); + assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() && + _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!"); - _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose(); - _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21]; + _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_21].transpose(); + _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_21]; } else return false; @@ -576,7 +600,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& SizeEigen sz = 0; for (const auto& sb : state_bloc_vec) if (sb) - sz += sb->getSize(); + sz += sb->getLocalSize(); // resizing _covariance = Eigen::MatrixXs(sz, sz); @@ -592,10 +616,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& if (sb_j) { success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getSize(); + j += sb_j->getLocalSize(); } } - i += sb_i->getSize(); + i += sb_i->getLocalSize(); } } return success; @@ -607,6 +631,12 @@ bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) return getFrameCovariance(frm, cov); } +bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov) +{ + FrameBasePtr frm = getLastKeyOrAuxFrame(); + return getFrameCovariance(frm, cov); +} + bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) { bool success(true); @@ -618,7 +648,7 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M SizeEigen sz = 0; for (const auto& sb : state_bloc_vec) if (sb) - sz += sb->getSize(); + sz += sb->getLocalSize(); // resizing _covariance = Eigen::MatrixXs(sz, sz); @@ -635,10 +665,10 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M if (sb_j) { success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getSize(); + j += sb_j->getLocalSize(); } } - i += sb_i->getSize(); + i += sb_i->getLocalSize(); } } return success; @@ -659,22 +689,37 @@ HardwareBasePtr Problem::getHardware() return hardware_ptr_; } -FrameBasePtr Problem::getLastFrame() +FrameBasePtr Problem::getLastFrame() const { return trajectory_ptr_->getLastFrame(); } -FrameBasePtr Problem::getLastKeyFrame() +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); +} + FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance) { if ( ! prior_is_set_ ) { // Create origin frame - FrameBasePtr origin_keyframe = emplaceFrame(KEY_FRAME, _prior_state, _ts); + FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts); // create origin capture with the given state as data // Capture fix only takes 3D position and Quaternion orientation @@ -783,10 +828,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()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " 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()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F") << pm->getLast()->getFrame()->id() << endl; if (pm->getIncoming()) cout << " i: C" << pm->getIncoming()->id() << endl; @@ -801,14 +846,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) // if (ptt) // { // if (ptt->getPrevOrigin()) -// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " F") +// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F") // << 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()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " 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()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F") << pt->getLast()->getFrame()->id() << endl; if (pt->getIncoming()) cout << " i: C" << pt->getIncoming()->id() << endl; @@ -824,7 +869,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->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); if (constr_by) { cout << " <-- "; @@ -981,11 +1026,6 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) cout << endl; } -FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) -{ - return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); -} - bool Problem::check(int verbose_level) { using std::cout; @@ -1065,7 +1105,7 @@ bool Problem::check(int verbose_level) { if (verbose_level > 0) { - cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl; + cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " 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_IMU.cpp b/src/processor/processor_IMU.cpp index 2dfa2cd7de6b14c3aea08eab02782d30235c15df..b192163cc10e3987640d2ee8b8e0025e86d5d758 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -35,8 +35,6 @@ ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const Pro bool ProcessorIMU::voteForKeyFrame() { - if(!isVotingActive()) - return false; // time span if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span) { diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 0679909164d86a96ac609a930619f35ff6a97fe3..77e00b83580074483b286457655c8b08edf534d3 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -26,9 +26,14 @@ bool ProcessorBase::permittedKeyFrame() return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this()); } +bool ProcessorBase::permittedAuxFrame() +{ + return isVotingAuxActive() && getProblem()->permitAuxFrame(shared_from_this()); +} + FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr) { - std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl; + std::cout << "Making " << (_type == KEY ? "key-" : (_type == AUXILIARY ? "aux-" : "")) << "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_motion.cpp b/src/processor/processor_motion.cpp index ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae..755b6c95091c88ad7c5f3362caa3950714046d0f 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -193,7 +193,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp()); last_ptr_->getFrame()->setState(getCurrentState()); - if (voteForKeyFrame() && permittedKeyFrame()) + if (permittedKeyFrame() && voteForKeyFrame()) { // Set the frame of last_ptr as key auto key_frame_ptr = last_ptr_->getFrame(); @@ -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(KEY, _x_origin, _ts_origin); setOrigin(key_frame_ptr); return key_frame_ptr; @@ -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..4d58f52386a281b5eee02beb4049f973cc9bad63 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(KEY, 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, ... @@ -174,15 +174,12 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) last_ptr_->getFrame()->setKey(); // 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 processNew(params_tracker_->max_new_features); - // // Set key - // last_ptr_->getFrame()->setKey(); - // // Set state to the keyframe last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); @@ -199,6 +196,38 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) incoming_ptr_ = nullptr; } + /* TODO: create an auxiliary frame + else if (voteForAuxFrame() && permittedAuxFrame()) + { + // We create an Auxiliary Frame + + // set AuxF on last + last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); + last_ptr_->getFrame()->setAuxiliary(); + + // make F; append incoming to new F + FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); + frm->addCapture(incoming_ptr_); + + // Set state to the keyframe + last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); + + // Establish factors + establishFactors(); + + // Call the new auxframe callback in order to let the ProcessorMotion to establish their factors + getProblem()->auxFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance); + + // Advance this + last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame + // do not remove! last_ptr_->remove(); + incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp()); + + // Update pointers + advanceDerived(); + last_ptr_ = incoming_ptr_; + incoming_ptr_ = nullptr; + }*/ else { // We do not create a KF diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 086afab738a5b66cebdde25cdda862606d28b924..e28e18cdea2cc5c8cfd88d55a2d311d81a22a6b0 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -6,7 +6,8 @@ namespace wolf { TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) : NodeBase("TRAJECTORY", "Base"), frame_structure_(_frame_structure), - last_key_frame_ptr_(nullptr) + last_key_frame_ptr_(nullptr), + last_key_or_aux_frame_ptr_(nullptr) { // WOLF_DEBUG("constructed T"); } @@ -22,17 +23,19 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) _frame_ptr->setTrajectory(shared_from_this()); _frame_ptr->setProblem(getProblem()); - if (_frame_ptr->isKey()) + // add to list + frame_list_.push_back(_frame_ptr); + + if (_frame_ptr->isKeyOrAux()) { + // register state blocks _frame_ptr->registerNewStateBlocks(); - if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp()) - last_key_frame_ptr_ = _frame_ptr; - frame_list_.insert(computeFrameOrder(_frame_ptr), _frame_ptr); - } - else - { - frame_list_.push_back(_frame_ptr); + // sort + sortFrame(_frame_ptr); + + // update last_estimated and last_key + updateLastFrames(); } return _frame_ptr; @@ -47,7 +50,6 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr) { moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr)); - // last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above } void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place) @@ -56,34 +58,46 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place) { frame_list_.remove(_frm_ptr); frame_list_.insert(_place, _frm_ptr); - last_key_frame_ptr_ = findLastKeyFrame(); + updateLastFrames(); } } 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)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp()) return frm_rit.base(); return getFrameList().begin(); } -FrameBasePtr TrajectoryBase::findLastKeyFrame() +void TrajectoryBase::updateLastFrames() { - // NOTE: Assumes keyframes are sorted by timestamp - for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit) - if ((*frm_rit)->isKey()) - return (*frm_rit); + bool last_estimated_set = false; - return nullptr; + // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp + for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().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) +FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const { + // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp FrameBasePtr closest_kf = nullptr; Scalar min_dt = 1e9; - for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) + for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++) if ((*frm_rit)->isKey()) { Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); @@ -98,4 +112,25 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) 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; + Scalar min_dt = 1e9; + + for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++) + if ((*frm_rit)->isKeyOrAux()) + { + Scalar dt = std::fabs((*frm_rit)->getTimeStamp() - _ts); + if (dt < min_dt) + { + min_dt = dt; + closest_kf = *frm_rit; + } + else + break; + } + return closest_kf; +} + } // namespace wolf diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index d2657e4156cf6991d3342505cb558712837cb344..253c7da0cba0aebdcbff3d30521b9511987d075f 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(KEY, 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 57efea34c4fd4a4786dedf1f33d8d29c4e125705..3c5f2502cbce65184e2d63d59b85145776a86f5c 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(KEY, 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(KEY, 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(KEY, 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(KEY, 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))); @@ -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_FRAME, P->zeroState(), TimeStamp(0)); + FrameBasePtr F = P->emplaceFrame(KEY, 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))); @@ -547,7 +547,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(KEY, 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()))); @@ -589,7 +589,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(KEY, 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..fd9650de70d970b9336d62e8471f3c5a090c0221 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(KEY, 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 21c7c912f801b39adcf3b5c75e56ca03d08e83c2..b9c03ab04dcafa19f9fd850a8b72508e25ba02b3 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 (KEY, pose1, 1.0); - F2 = problem->emplaceFrame (KEY_FRAME, pose2, 2.0); + F2 = problem->emplaceFrame (KEY, 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 98af13f6e834f73bd4bbab7e524cf8928d0467af..78223c6020f9241c9a4faa7d24db5346c06e9c6d 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(KEY, 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(KEY, 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(KEY, 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..2b8a6d20160d9464ffa7b48ff9044b66b03d50d9 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(KEY, problem_ptr->zeroState(), TimeStamp(0)); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 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..d07b564a3caeed5dd34bb9e107776bc769f3d85d 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(KEY, 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..620e389c6b5a483f84250584cbe6d318e62ec55b 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(KEY, 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 082e438d0b799e1ed32075f28e6f61b7450da8e0..c9d894e9ff3a8fafdee13087ef7364bf7173277f 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>(KEY, 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 diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 9b4c7b6cf882b1f8104fa71ac3d60e12d8580137..9984c258de1026f68debd1cf4ccd072d4cdbd3c5 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -32,6 +32,7 @@ TEST(FrameBase, GettersAndSetters) ASSERT_EQ(t, 1); ASSERT_FALSE(F->isFixed()); ASSERT_EQ(F->isKey(), false); + ASSERT_EQ(F->isKeyOrAux(), false); } TEST(FrameBase, StateBlocks) @@ -123,6 +124,7 @@ TEST(FrameBase, LinksToTree) // set key F1->setKey(); ASSERT_TRUE(F1->isKey()); + ASSERT_TRUE(F1->isKeyOrAux()); // Unlink F1->unlinkCapture(C); diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 604ef489478f5779ac4b79a14095c37436c81be4..feafb83e98ce51caf43aea88d2a192b00fff78e8 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -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(KEY, 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(KEY, 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(KEY, 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(KEY, 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 3b0ed8e164900db7f7bbe5e244e7a424f16758c9..99054f035c989ff45fa6bca332266293ac3375fe 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -138,7 +138,7 @@ TEST(Problem, SetOrigin_PO_2D) ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0); // check that the state is correct - ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); + ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL ); // check that we have one frame, one capture, one feature, one factor TrajectoryBasePtr T = P->getTrajectory(); @@ -156,8 +156,8 @@ TEST(Problem, SetOrigin_PO_2D) ASSERT_FALSE(c->getLandmarkOther()); // check that the Feature measurement and covariance are the ones provided - ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL)); - ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL)); + ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL ); + ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL ); // P->print(4,1,1,1); } @@ -203,9 +203,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", KEY, VectorXs(3), TimeStamp(0.0)); + FrameBasePtr f1 = P->emplaceFrame("PO 3D", KEY, VectorXs(7), TimeStamp(1.0)); + FrameBasePtr f2 = P->emplaceFrame("POV 3D", KEY, 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; @@ -249,7 +249,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 - auto KF = P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); + auto KF = P->emplaceFrame("PO 3D", KEY, xs, 0); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2)); // Notifications @@ -306,22 +306,20 @@ 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", KEY, xs, 0); // set covariance (they are not computed without a solver) P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity()); - P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity()); - P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero()); + P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity()); + P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero()); // get covariance Eigen::MatrixXs Cov; ASSERT_TRUE(P->getFrameCovariance(F, Cov)); - // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations) - // JV: The local parameterization projects the covariance to the state size. - ASSERT_EQ(Cov.cols() , 7); - ASSERT_EQ(Cov.rows() , 7); - ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12); + ASSERT_EQ(Cov.cols() , 6); + ASSERT_EQ(Cov.rows() , 6); + ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12); } diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 904dfd7646f1194892e0845b003cd20d17916c4b..34128f0db287f152a36074fe16b2e61db9a37153 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -59,7 +59,7 @@ TEST(ProcessorBase, KeyFrameCallback) std::cout << "sensor & processor created and added to wolf problem" << std::endl; - // Sequence to test KeyFrame creations (callback calls) + // Sequence to test Key Frame creations (callback calls) // initialize TimeStamp t(0.0); diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index cfbdd69e0f978d8822bf0e707ff48a2a3fcefbbd..75a0cdefbb70da4d75edb2d1e4a7e5d79d13a29c 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::KEY, 1, stateblock_pptr1, stateblock_optr1); - F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F2 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr2, stateblock_optr2); - F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F3 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr3, stateblock_optr3); - F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + F4 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr4, stateblock_optr4); @@ -147,8 +147,9 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY", 1.2, sensor_ptr); - // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F3); + // Make 3rd frame last Key frame + F3->setTimeStamp(wolf::TimeStamp(2.0)); + problem->getTrajectory()->sortFrame(F3); // trigger search for loopclosure processor_ptr_point2d->process(incomming_dummy); @@ -180,8 +181,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) problem->addCovarianceBlock( F4->getP(), F4->getP(), position_covariance_matrix); - // Make 3rd frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F3); + // Make 3rd frame last Key frame + F3->setTimeStamp(wolf::TimeStamp(2.0)); + problem->getTrajectory()->sortFrame(F3); // trigger search for loopclosure processor_ptr_ellipse2d->process(incomming_dummy); @@ -192,8 +194,9 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) ASSERT_EQ(testloops[0]->id(), (unsigned int) 1); ASSERT_EQ(testloops[1]->id(), (unsigned int) 2); - // Make 4th frame last Keyframe - problem->getTrajectory()->setLastKeyFrame(F4); + // Make 4th frame last Key frame + F4->setTimeStamp(wolf::TimeStamp(3.0)); + problem->getTrajectory()->sortFrame(F4); // trigger search for loopclosure again processor_ptr_ellipse2d->process(incomming_dummy); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index a38a7ee467e95f23a3f922c4b7bceb117e06cd14..91ec5dba639cc2a89dc99a94de23e5483adf981b 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -61,7 +61,7 @@ class SolverManagerWrapper : public SolverManager }; virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; - virtual void computeCovariances(const StateBlockPtrList& st_list){}; + virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){}; // The following are dummy implementations bool hasConverged() { return true; } @@ -504,7 +504,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(KEY, 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))); @@ -531,7 +531,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(KEY, 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))); @@ -564,7 +564,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(KEY, 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))); @@ -598,7 +598,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(KEY, 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 ec2b314fc2dda90f7b6b53bb83713095fe46e257..3a48ae21d7a9fbcd5ffcccc67d088fb9a9f8b11c 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -49,33 +49,74 @@ TEST(TrajectoryBase, ClosestKeyFrame) TrajectoryBasePtr T = P->getTrajectory(); // Trajectory status: - // kf1 kf2 f3 frames + // KF1 KF2 AuxF3 F4 frames + // 1 2 3 4 time stamps + // --+-----+-----+-----+---> time + + FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr); + FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 2, nullptr, nullptr); + FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY, 3, nullptr, nullptr); + FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr); + T->addFrame(F1); + T->addFrame(F2); + T->addFrame(F3); + T->addFrame(F4); + + FrameBasePtr KF; // closest key-frame queried + + KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! + + KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! + + KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! + + KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! + + KF = T->closestKeyFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! + + KF = T->closestKeyFrameToTimeStamp(4.2); // after the last frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! +} + +TEST(TrajectoryBase, ClosestKeyOrAuxFrame) +{ + + ProblemPtr P = Problem::create("PO 2D"); + TrajectoryBasePtr T = P->getTrajectory(); + + // Trajectory status: + // KF1 KF2 F3 frames // 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); - T->addFrame(f1); - T->addFrame(f2); - T->addFrame(f3); + FrameBasePtr F1 = std::make_shared<FrameBase>(KEY, 1, nullptr, nullptr); + FrameBasePtr F2 = std::make_shared<FrameBase>(AUXILIARY, 2, nullptr, nullptr); + FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr); + T->addFrame(F1); + T->addFrame(F2); + T->addFrame(F3); - FrameBasePtr kf; // closest key-frame queried + FrameBasePtr KF; // closest key-frame queried - kf = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return f0 - ASSERT_EQ(kf->id(), f1->id()); // same id! + KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0 + ASSERT_EQ(KF->id(), F1->id()); // same id! - kf = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return f1 - ASSERT_EQ(kf->id(), f1->id()); // same id! + KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! - kf = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! + KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - 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(2.6); // between keyframe and frame, but closer to frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - kf = T->closestKeyFrameToTimeStamp(3.2); // after the last frame --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! + KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! } TEST(TrajectoryBase, Add_Remove_Frame) @@ -88,37 +129,37 @@ TEST(TrajectoryBase, Add_Remove_Frame) DummySolverManager N(P); // Trajectory status: - // kf1 kf2 f3 frames + // KF1 KF2 F3 frames // 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>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed + FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 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; // add frames and keyframes - T->addFrame(f1); // KF + T->addFrame(F1); // KF 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; - T->addFrame(f2); // KF + T->addFrame(F2); // KF 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; - T->addFrame(f3); // F (not KF so state blocks are not notified) + T->addFrame(F3); // F (not KF so state blocks are not notified) if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); std::cout << __LINE__ << std::endl; N.update(); @@ -126,24 +167,24 @@ TEST(TrajectoryBase, Add_Remove_Frame) std::cout << __LINE__ << std::endl; // remove frames and keyframes - f2->remove(); // KF + F2->remove(); // KF if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id()); + ASSERT_EQ(T->getLastFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id()); std::cout << __LINE__ << std::endl; - f3->remove(); // F + F3->remove(); // F if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id()); - f1->remove(); // KF + F1->remove(); // KF if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0); std::cout << __LINE__ << std::endl; @@ -162,70 +203,109 @@ TEST(TrajectoryBase, KeyFramesAreSorted) TrajectoryBasePtr T = P->getTrajectory(); // Trajectory status: - // kf1 kf2 f3 frames + // KF1 KF2 F3 frames // 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>(KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed + FrameBasePtr F2 = std::make_shared<FrameBase>(KEY, 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 + T->addFrame(F2); // KF2 if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f2->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - T->addFrame(f3); // F3 + T->addFrame(F3); // F3 if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - T->addFrame(f1); // KF1 + T->addFrame(F1); // KF1 if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - f3->setKey(); // set KF3 + F3->setKey(); // set KF3 if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->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 - T->addFrame(f4); + 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 + // KF1 KF2 KF3 F4 frames // 1 2 3 1.5 time stamps // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f4->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F4->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - f4->setKey(); + F4->setKey(); // Trajectory status: - // kf1 kf4 kf2 kf3 frames + // KF1 KF4 KF2 KF3 frames // 1 1.5 2 3 time stamps // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - f2->setTimeStamp(4); + F2->setTimeStamp(4); // Trajectory status: - // kf1 kf4 kf3 kf2 frames + // KF1 KF4 KF3 KF2 frames // 1 1.5 3 4 time stamps // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getLastFrame() ->id(), f2->id()); - ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); + ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - f4->setTimeStamp(0.5); + F4->setTimeStamp(0.5); // Trajectory status: - // kf4 kf2 kf3 kf2 frames + // KF4 KF2 KF3 KF2 frames // 0.5 1 3 4 time stamps // --+-----+-----+------+---> time if (debug) P->print(2,0,1,0); - ASSERT_EQ(T->getFrameList().front()->id(), f4->id()); + ASSERT_EQ(T->getFrameList().front()->id(), F4->id()); + + FrameBasePtr F5 = std::make_shared<FrameBase>(AUXILIARY, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + T->addFrame(F5); + // Trajectory status: + // KF4 KF2 AuxF5 KF3 KF2 frames + // 0.5 1 1.5 3 4 time stamps + // --+-----+-----+-----+-----+---> 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()); + + F5->setTimeStamp(5); + // Trajectory status: + // KF4 KF2 KF3 KF2 AuxF5 frames + // 0.5 1 3 4 5 time stamps + // --+-----+-----+-----+-----+---> 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(), F2->id()); + + FrameBasePtr F6 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame + T->addFrame(F6); + // Trajectory status: + // KF4 KF2 KF3 KF2 AuxF5 F6 frames + // 0.5 1 3 4 5 6 time stamps + // --+-----+-----+-----+-----+-----+---> 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(), F2->id()); }