diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 03417063241c103308552200b9e56552261e9970..25d7fd4067c2401b46db2c247961b54a8213f71f 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -41,7 +41,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture) if (!kf) { // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below) - kf = getProblem()->closestImportantFrameToTimeStamp(_capture->getTimeStamp()); + kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp()); assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!"); } diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h index e440355e866c9f99f4d49c989aa4755fc873ac87..fbe3914ab8b76c290d0bcdb13680e6cbea6493cd 100644 --- a/include/base/frame/frame_base.h +++ b/include/base/frame/frame_base.h @@ -22,9 +22,9 @@ namespace wolf { */ typedef enum { - IMPORTANT = 2, ///< estimated important frame. It plays at optimizations. - ESTIMATED = 1, ///< estimated frame. It plays at optimizations. - NON_ESTIMATED = 0 ///< Regular frame. It does not play at optimization. + 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 @@ -72,13 +72,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase public: unsigned int id(); - // Important / Non Important - bool isImportant() const; - void setImportant(); - - // Estimated / Non estimated + // get type + bool isKey() const; bool isEstimated() const; - void setEstimated(); + + // set type + void setKey(); + void setAuxiliary(); // Frame values ------------------------------------------------ public: @@ -172,14 +172,14 @@ inline unsigned int FrameBase::id() return frame_id_; } -inline bool FrameBase::isImportant() const +inline bool FrameBase::isKey() const { - return (type_ == IMPORTANT); + return (type_ == KEY); } inline bool FrameBase::isEstimated() const { - return (type_ == IMPORTANT || type_ == ESTIMATED); + 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 b7070994829419d35c20b3e602fd37b1f6390d45..82df6bc2665f9f8e14d9b6846a7f7c4be60c784f 100644 --- a/include/base/problem/problem.h +++ b/include/base/problem/problem.h @@ -138,7 +138,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 IMPORTANT, ESTIMATED or NON_ESTIMATED + * \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 * @@ -154,7 +154,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 IMPORTANT, ESTIMATED or NON_ESTIMATED + * \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: @@ -167,7 +167,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 IMPORTANT, ESTIMATED or NON_ESTIMATED + * \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 * @@ -181,7 +181,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 IMPORTANT, ESTIMATED or NON_ESTIMATED + * \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: @@ -194,42 +194,42 @@ class Problem : public std::enable_shared_from_this<Problem> // Frame getters FrameBasePtr getLastFrame( ) const; - FrameBasePtr getLastImportantFrame( ) const; + FrameBasePtr getLastKeyFrame( ) const; FrameBasePtr getLastEstimatedFrame( ) const; - FrameBasePtr closestImportantFrameToTimeStamp(const TimeStamp& _ts) const; + FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; FrameBasePtr closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const; - /** \brief Give the permission to a processor to create a new important Frame + /** \brief Give the permission to a processor to create a new key Frame * - * This should implement a black list of processors that have forbidden important frame 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 important frames, 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 permitImportantFrame(ProcessorBasePtr _processor_ptr); + bool permitKeyFrame(ProcessorBasePtr _processor_ptr); - /** \brief New important frame callback + /** \brief New key frame callback * - * New important frame callback: It should be called by any processor that creates a new important frame. It calls the importantFrameCallback 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 importantFrameCallback(FrameBasePtr _frame_ptr, // + void keyFrameCallback(FrameBasePtr _keyframe_ptr, // ProcessorBasePtr _processor_ptr, // const Scalar& _time_tolerance); - /** \brief Give the permission to a processor to create a new estimated Frame + /** \brief Give the permission to a processor to create a new auxiliary Frame * - * This should implement a black list of processors that have forbidden estimated frame creation. + * 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 permitEstimatedFrame(ProcessorBasePtr _processor_ptr); + bool permitAuxFrame(ProcessorBasePtr _processor_ptr); - /** \brief New estimated frame callback + /** \brief New auxiliary frame callback * - * New estimated frame callback: It should be called by any processor that creates a new estimated frame. It calls the estimatedFrameCallback of the processor motion. + * 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 estimatedFrameCallback(FrameBasePtr _frame_ptr, // - ProcessorBasePtr _processor_ptr, // - const Scalar& _time_tolerance); + void auxFrameCallback(FrameBasePtr _frame_ptr, // + ProcessorBasePtr _processor_ptr, // + const Scalar& _time_tolerance); // State getters Eigen::VectorXs getCurrentState ( ); @@ -257,7 +257,8 @@ class Problem : public std::enable_shared_from_this<Problem> bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov); bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0); bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance); - bool getLastImportantFrameCovariance(Eigen::MatrixXs& _covariance); + bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance); + bool getLastEstimatedFrameCovariance(Eigen::MatrixXs& _covariance); bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance); // Solver management ---------------------------------------- diff --git a/include/base/trajectory/trajectory_base.h b/include/base/trajectory/trajectory_base.h index fe7d201e6cd565ca69f3cf5cb3c6e15a60e691c4..4ce1a6e744305bd03181ee52991c0b1b027633d0 100644 --- a/include/base/trajectory/trajectory_base.h +++ b/include/base/trajectory/trajectory_base.h @@ -25,7 +25,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_important_frame_ptr_; // keeps pointer to the last important frame + FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame FrameBasePtr last_estimated_frame_ptr_; // keeps pointer to the last estimated frame public: @@ -40,9 +40,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj FrameBasePtrList& getFrameList(); const FrameBasePtrList& getFrameList() const; FrameBasePtr getLastFrame() const; - FrameBasePtr getLastImportantFrame() const; + FrameBasePtr getLastKeyFrame() const; FrameBasePtr getLastEstimatedFrame() const; - FrameBasePtr closestImportantFrameToTimeStamp(const TimeStamp& _ts) const; + FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const; FrameBasePtr closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const; void sortFrame(FrameBasePtr _frm_ptr); void updateLastFrames(); @@ -70,9 +70,9 @@ inline FrameBasePtr TrajectoryBase::getLastFrame() const return frame_list_.back(); } -inline FrameBasePtr TrajectoryBase::getLastImportantFrame() const +inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const { - return last_important_frame_ptr_; + return last_key_frame_ptr_; } inline FrameBasePtr TrajectoryBase::getLastEstimatedFrame() const diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index a237f5ee029eb3a888329ddf310891e81a1df61c..590f8de1d0ff37e6e9c27d11f79f0f54dc5d4372 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -144,7 +144,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: { //robot-robot - auto last_key_frame = wolf_problem_->getLastImportantFrame(); + auto last_key_frame = wolf_problem_->getLastKeyFrame(); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP()); state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO()); diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 077f5324b027296a0244885212ce10bf221a9459..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(ESTIMATED, 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 200945f345247f84ab2fd27c7cec795ae586c926..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(ESTIMATED, 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 e3ace5f555f7d64a6f79db6047edc5df9ef10e93..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>(ESTIMATED, 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>(ESTIMATED, 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>(ESTIMATED, 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_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp index ed3d8c13d78a8dbc7347ecffad7341e541292637..4b923de89990eac72ffc20df61b13e28dbb76b66 100644 --- a/src/examples/test_imuPlateform_Offline.cpp +++ b/src/examples/test_imuPlateform_Offline.cpp @@ -215,7 +215,7 @@ int main(int argc, char** argv) wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList(); for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isEstimated()) + if(frm_ptr->isKey()) { //prepare needed variables FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr); diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index 81be64b485fff8ef6bbea5bbd754af9ba7e117ef..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644 --- a/src/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -278,7 +278,7 @@ int main(int argc, char** argv) wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList(); for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isEstimated()) + if(frm_ptr->isKey()) { //prepare needed variables FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr); @@ -324,7 +324,7 @@ int main(int argc, char** argv) for(FrameBasePtr frm_ptr : frame_list) { - if(frm_ptr->isEstimated()) + if(frm_ptr->isKey()) { FactorBasePtrList fac_list = frm_ptr->getConstrainedByList(); for(FactorBasePtr fac_ptr : fac_list) diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index d2246b962426aafe5c9ae913a45e619007d275f9..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(ESTIMATED, (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 547518fc183244b3637014e6f1c9a90f14099bfe..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(ESTIMATED,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_2 = problem->emplaceFrame(ESTIMATED,(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(ESTIMATED,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0)); - FrameBasePtr kf_4 = problem->emplaceFrame(ESTIMATED,(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 5cac693a0213f0d310115d4f8f86f2dab8c2a22a..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644 --- a/src/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -24,7 +24,7 @@ void printFrames(ProblemPtr _problem_ptr) { std::cout << "TRAJECTORY:" << std::endl; for (auto frm : _problem_ptr->getTrajectory()->getFrameList()) - std::cout << "\t " << (frm->isEstimated() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl; + std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl; } int main() @@ -65,12 +65,12 @@ int main() printFrames(problem_ptr); - FrameBasePtr frm7 = problem_ptr->emplaceFrame(ESTIMATED, 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(ESTIMATED, 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 97af22fa1df25cf2d05c772c6b8a5ca9d19a8b3f..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(ESTIMATED, 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(ESTIMATED, 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 5d065b8ce83113c085bdd12dd8b927f8fc520584..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(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED, 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 9338b47287d238c6604077fed662d2ba2f290212..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(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1))); - FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED, 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 bbe37ef96fdc4d2e33347b34d02b5248dc2cb032..ccdb80f05c15225e7500dddfe6b65f59ebf06a24 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -39,8 +39,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr FrameBase::~FrameBase() { - if ( isEstimated() ) - removeStateBlocks(); } void FrameBase::remove() @@ -71,7 +69,8 @@ void FrameBase::remove() if ( isEstimated() ) removeStateBlocks(); - if (getTrajectory()->getLastImportantFrame()->id() == this_F->id() || getTrajectory()->getLastEstimatedFrame()->id() == this_F->id()) + + if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastEstimatedFrame()->id() == this_F->id()) getTrajectory()->updateLastFrames(); // std::cout << "Removed F" << id() << std::endl; @@ -111,28 +110,27 @@ void FrameBase::removeStateBlocks() } } -void FrameBase::setImportant() +void FrameBase::setKey() { - if (type_ == NON_ESTIMATED) - { + // register if previously not estimated + if (!isEstimated()) registerNewStateBlocks(); - getTrajectory()->sortFrame(shared_from_this()); -// WOLF_DEBUG("Set Important", this->id()); - } - type_ = IMPORTANT; + // WOLF_DEBUG("Set Key", this->id()); + type_ = KEY; + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); } -void FrameBase::setEstimated() +void FrameBase::setAuxiliary() { - if (type_ == NON_ESTIMATED) - { + if (!isEstimated()) registerNewStateBlocks(); - getTrajectory()->sortFrame(shared_from_this()); -// WOLF_DEBUG("Set Estimated", this->id()); - } - type_ = ESTIMATED; + // WOLF_DEBUG("Set Auxiliary", this->id()); + type_ = AUXILIARY; + getTrajectory()->sortFrame(shared_from_this()); + getTrajectory()->updateLastFrames(); } void FrameBase::fix() @@ -178,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()), isEstimated()); + sb->setState(_state.segment(index, sb->getSize()), isEstimated()); // do not notify if state block is not estimated by the solver index += sb->getSize(); } } diff --git a/src/problem.cpp b/src/problem.cpp deleted file mode 100644 index f8eb1676a6384e57fc8772c21aa3ef31c577604d..0000000000000000000000000000000000000000 --- a/src/problem.cpp +++ /dev/null @@ -1,1385 +0,0 @@ -// wolf includes -#include "base/problem.h" -#include "base/hardware_base.h" -#include "base/trajectory_base.h" -#include "base/map_base.h" -#include "base/sensor/sensor_base.h" -#include "base/processor/processor_motion.h" -#include "base/processor/processor_tracker.h" -#include "base/capture/capture_pose.h" -#include "base/factor/factor_base.h" -#include "base/sensor/sensor_factory.h" -#include "base/processor/processor_factory.h" -#include "base/state_block.h" - - -// IRI libs includes - -// C++ includes -#include <algorithm> - -namespace wolf -{ - -// unnamed namespace used for helper functions local to this file. -namespace -{ -std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;} -} - -Problem::Problem(const std::string& _frame_structure) : - hardware_ptr_(std::make_shared<HardwareBase>()), - trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)), - map_ptr_(std::make_shared<MapBase>()), - processor_motion_ptr_(), - prior_is_set_(false) -{ - if (_frame_structure == "PO 2D") - { - state_size_ = 3; - state_cov_size_ = 3; - dim_ = 2; - } - - else if (_frame_structure == "PO 3D") - { - state_size_ = 7; - state_cov_size_ = 6; - dim_ = 3; - } - else if (_frame_structure == "POV 3D") - { - state_size_ = 10; - state_cov_size_ = 9; - dim_ = 3; - } - else std::runtime_error( - "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); - -} - -void Problem::setup() -{ - hardware_ptr_ -> setProblem(shared_from_this()); - trajectory_ptr_-> setProblem(shared_from_this()); - map_ptr_ -> setProblem(shared_from_this()); -} - -ProblemPtr Problem::create(const std::string& _frame_structure) -{ - ProblemPtr p(new Problem(_frame_structure)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. - p->setup(); - return p->shared_from_this(); -} - -Problem::~Problem() -{ - // WOLF_DEBUG("destructed -P"); -} - -void Problem::addSensor(SensorBasePtr _sen_ptr) -{ - getHardware()->addSensor(_sen_ptr); -} - -SensorBasePtr Problem::installSensor(const std::string& _sen_type, // - const std::string& _unique_sensor_name, // - const Eigen::VectorXs& _extrinsics, // - IntrinsicsBasePtr _intrinsics) -{ - SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics); - addSensor(sen_ptr); - return sen_ptr; -} - -SensorBasePtr Problem::installSensor(const std::string& _sen_type, // - const std::string& _unique_sensor_name, // - const Eigen::VectorXs& _extrinsics, // - const std::string& _intrinsics_filename) -{ - - if (_intrinsics_filename != "") - { - assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist."); - IntrinsicsBasePtr intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename); - return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr); - } - else - return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr()); - -} - -ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // - const std::string& _unique_processor_name, // - SensorBasePtr _corresponding_sensor_ptr, // - ProcessorParamsBasePtr _prc_params) -{ - if (_corresponding_sensor_ptr == nullptr) - { - WOLF_ERROR("Cannot install processor '", _unique_processor_name, - "' since the associated sensor does not exist !"); - return ProcessorBasePtr(); - } - - ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr); - prc_ptr->configure(_corresponding_sensor_ptr); - _corresponding_sensor_ptr->addProcessor(prc_ptr); - - // setting the origin in all processor motion if origin already setted - if (prc_ptr->isMotion() && prior_is_set_) - (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - - // setting the main processor motion - if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) - processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr); - - return prc_ptr; -} - -ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // - const std::string& _unique_processor_name, // - const std::string& _corresponding_sensor_name, // - const std::string& _params_filename) -{ - SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name); - if (sen_ptr == nullptr) - throw std::runtime_error("Sensor not found. Cannot bind Processor."); - if (_params_filename == "") - return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr); - else - { - assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist."); - ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename); - return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params); - } -} - -SensorBasePtr Problem::getSensor(const std::string& _sensor_name) -{ - auto sen_it = std::find_if(getHardware()->getSensorList().begin(), - getHardware()->getSensorList().end(), - [&](SensorBasePtr sb) - { - return sb->getName() == _sensor_name; - }); // lambda function for the find_if - - if (sen_it == getHardware()->getSensorList().end()) - return nullptr; - - return (*sen_it); -} - -ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name) -{ - for (auto sen : getHardware()->getSensorList()) // loop all sensors - { - auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name - sen->getProcessorList().end(), - [&](ProcessorBasePtr prc) - { - return prc->getName() == _processor_name; - }); // lambda function for the find_if - - if (prc_it != sen->getProcessorList().end()) // found something! - { - if ((*prc_it)->isMotion()) // found, and it's motion! - { - std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl; - processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it); - return processor_motion_ptr_; - } - else // found, but it's not motion! - { - std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl; - return nullptr; - } - } - } - // nothing found! - std::cout << "Processor '" << _processor_name << "' not found!" << std::endl; - return nullptr; -} - -void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr) -{ - processor_motion_ptr_ = _processor_motion_ptr; -} - -void Problem::clearProcessorMotion() -{ - processor_motion_ptr_.reset(); -} - -FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // - FrameType _frame_key_type, // - const Eigen::VectorXs& _frame_state, // - const TimeStamp& _time_stamp) -{ - FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state); - trajectory_ptr_->addFrame(frm); - return frm; -} - -FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // - FrameType _frame_key_type, // - const TimeStamp& _time_stamp) -{ - return emplaceFrame(_frame_structure, _frame_key_type, getState(_time_stamp), _time_stamp); -} - -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const Eigen::VectorXs& _frame_state, // - const TimeStamp& _time_stamp) -{ - return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _frame_state, _time_stamp); -} - -FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, // - const TimeStamp& _time_stamp) -{ - return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _time_stamp); -} - -Eigen::VectorXs Problem::getCurrentState() -{ - Eigen::VectorXs state(getFrameStructureSize()); - getCurrentState(state); - return state; -} - -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 - state = zeroState(); -} - -void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts) -{ - if (processor_motion_ptr_ != nullptr) - { - processor_motion_ptr_->getCurrentState(state); - processor_motion_ptr_->getCurrentTimeStamp(ts); - } - else if (trajectory_ptr_->getLastKeyFrame() != nullptr) - { - trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts); - trajectory_ptr_->getLastKeyFrame()->getState(state); - } - else - state = zeroState(); -} - -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); - if (closest_frame != nullptr) - closest_frame->getState(state); - else - state = zeroState(); - } -} - -Eigen::VectorXs Problem::getState(const TimeStamp& _ts) -{ - Eigen::VectorXs state(getFrameStructureSize()); - getState(_ts, state); - return state; -} - -SizeEigen Problem::getFrameStructureSize() const -{ - return state_size_; -} - -void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const -{ - _x_size = state_size_; - _cov_size = state_cov_size_; -} - -SizeEigen Problem::getDim() const -{ - return dim_; -} - -Eigen::VectorXs Problem::zeroState() -{ - Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize()); - - // Set the quaternion identity for 3D states. Set only the real part to 1: - if (trajectory_ptr_->getFrameStructure() == "PO 3D" || - trajectory_ptr_->getFrameStructure() == "POV 3D") - state(6) = 1.0; - - return state; -} - -bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) -{ - // This should implement a black list of processors that have forbidden keyframe creation - // This decision is made at problem level, not at processor configuration level. - // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors. - - // Currently allowing all processors to vote: - return true; -} - -void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance) -{ - if (_processor_ptr) - { - WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); - } - else - { - WOLF_DEBUG("External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); - } - - for (auto sensor : hardware_ptr_->getSensorList()) - for (auto processor : sensor->getProcessorList()) - if (processor && (processor != _processor_ptr) ) - processor->keyFrameCallback(_keyframe_ptr, _time_tolerance); -} - -LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr) -{ - getMap()->addLandmark(_lmk_ptr); - return _lmk_ptr; -} - -void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list) -{ - getMap()->addLandmarkList(_lmk_list); -} - -StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr) -{ - //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl; - if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end()) - { - WOLF_WARN("Adding a state block that has already been added"); - return _state_ptr; - } - - // add the state unit to the list - state_block_list_.push_back(_state_ptr); - - // Add add notification - // Check if there is already a notification for this state block - auto notification_it = state_block_notification_map_.find(_state_ptr); - if (notification_it != state_block_notification_map_.end() && notification_it->second == ADD) - { - WOLF_WARN("There is already an ADD notification of this state block"); - } - else - state_block_notification_map_[_state_ptr] = ADD; - - return _state_ptr; -} - -void Problem::removeStateBlock(StateBlockPtr _state_ptr) -{ - //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl; - //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed"); - if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end()) - { - WOLF_WARN("Removing a state_block that hasn't been added or already removed"); - return; - } - - // add the state unit to the list - state_block_list_.remove(_state_ptr); - - // Check if there is already a notification for this state block - auto notification_it = state_block_notification_map_.find(_state_ptr); - if (notification_it != state_block_notification_map_.end()) - { - if (notification_it->second == REMOVE) - { - WOLF_WARN("There is already an REMOVE notification of this state block"); - } - // Remove ADD notification - else - { - state_block_notification_map_.erase(notification_it); - } - } - // Add REMOVE notification - else - state_block_notification_map_[_state_ptr] = REMOVE; -} - -FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr) -{ - //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl; - - // Add ADD notification - // Check if there is already a notification for this state block - auto notification_it = factor_notification_map_.find(_factor_ptr); - if (notification_it != factor_notification_map_.end() && notification_it->second == ADD) - { - WOLF_WARN("There is already an ADD notification of this factor"); - } - // Add ADD notification (override in case of REMOVE) - else - factor_notification_map_[_factor_ptr] = ADD; - - return _factor_ptr; -} - -void Problem::removeFactor(FactorBasePtr _factor_ptr) -{ - //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl; - - // Check if there is already a notification for this state block - auto notification_it = factor_notification_map_.find(_factor_ptr); - if (notification_it != factor_notification_map_.end()) - { - if (notification_it->second == REMOVE) - { - WOLF_WARN("There is already an REMOVE notification of this state block"); - } - // Remove ADD notification - else - factor_notification_map_.erase(notification_it); - } - // Add REMOVE notification - else - factor_notification_map_[_factor_ptr] = REMOVE; -} - -void Problem::clearCovariance() -{ - covariances_.clear(); -} - -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"); - - covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov; -} - -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"); - - covariances_[std::make_pair(_state1, _state1)] = _cov; -} - -bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row, - const int _col) -{ - //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; - //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!"); - - if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end()) - _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) = - 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()) = - covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose(); - else - { - WOLF_DEBUG("Could not find the requested covariance block."); - return false; - } - - return true; -} - -bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov) -{ - // fill covariance - for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++) - for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++) - { - StateBlockPtr sb1 = it1->first; - StateBlockPtr sb2 = it2->first; - std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2); - std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1); - - // 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!"); - - _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(); - } - 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!"); - - _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]; - } - else - return false; - } - - return true; -} - -bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col) -{ - return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col); -} - -bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) -{ - bool success(true); - int i = 0, j = 0; - - const auto& state_bloc_vec = _frame_ptr->getStateBlockVec(); - - // computing size - SizeEigen sz = 0; - for (const auto& sb : state_bloc_vec) - if (sb) - sz += sb->getSize(); - - // resizing - _covariance = Eigen::MatrixXs(sz, sz); - - // filling covariance - for (const auto& sb_i : state_bloc_vec) - { - if (sb_i) - { - j = 0; - for (const auto& sb_j : state_bloc_vec) - { - if (sb_j) - { - success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getSize(); - } - } - i += sb_i->getSize(); - } - } - return success; -} - -bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) -{ - FrameBasePtr frm = getLastKeyFrame(); - return getFrameCovariance(frm, cov); -} - -bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) -{ - bool success(true); - int i = 0, j = 0; - - const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec(); - - // computing size - SizeEigen sz = 0; - for (const auto& sb : state_bloc_vec) - if (sb) - sz += sb->getSize(); - - // resizing - _covariance = Eigen::MatrixXs(sz, sz); - - // filling covariance - - for (const auto& sb_i : state_bloc_vec) - { - if (sb_i) - { - j = 0; - for (const auto& sb_j : state_bloc_vec) - { - if (sb_j) - { - success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getSize(); - } - } - i += sb_i->getSize(); - } - } - return success; -} - -MapBasePtr Problem::getMap() -{ - return map_ptr_; -} - -TrajectoryBasePtr Problem::getTrajectory() -{ - return trajectory_ptr_; -} - -HardwareBasePtr Problem::getHardware() -{ - return hardware_ptr_; -} - -FrameBasePtr Problem::getLastFrame() -{ - return trajectory_ptr_->getLastFrame(); -} - -FrameBasePtr Problem::getLastKeyFrame() -{ - return trajectory_ptr_->getLastKeyFrame(); -} - -StateBlockPtrList& Problem::getStateBlockPtrList() -{ - return state_block_list_; -} - -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(IMPORTANT, _prior_state, _ts); - - // create origin capture with the given state as data - // Capture fix only takes 3D position and Quaternion orientation - CapturePosePtr init_capture; - if (trajectory_ptr_->getFrameStructure() == "POV 3D") - init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); - else - init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov); - - origin_keyframe->addCapture(init_capture); - - // emplace feature and factor - init_capture->emplaceFeatureAndFactor(); - - // Notify all processors about the prior KF - for (auto sensor : hardware_ptr_->getSensorList()) - for (auto processor : sensor->getProcessorList()) - if (processor->isMotion()) - // Motion processors will set its origin at the KF - (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe); - - prior_is_set_ = true; - - // Notify all other processors about the origin KF --> they will join it or not depending on their received data - for (auto sensor : hardware_ptr_->getSensorList()) - for (auto processor : sensor->getProcessorList()) - if ( !processor->isMotion() ) - processor->keyFrameCallback(origin_keyframe, _time_tolerance); - - return origin_keyframe; - } - else - throw std::runtime_error("Origin already set!"); -} - -void Problem::loadMap(const std::string& _filename_dot_yaml) -{ - getMap()->load(_filename_dot_yaml); -} - -void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name) -{ - getMap()->save(_filename_dot_yaml, _map_name); -} - -void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) -{ - using std::cout; - using std::endl; - - cout << endl; - cout << "P: wolf tree status ---------------------" << endl; - cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl; - if (depth >= 1) - { - // Sensors ======================================================================================= - for (auto S : getHardware()->getSensorList()) - { - cout << " S" << S->id() << " " << S->getType(); - if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]"); - if (depth < 2) - cout << " -- " << S->getProcessorList().size() << "p"; - cout << endl; - if (metric && state_blocks) - { - for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++) - { - if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = ["; - if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = ["; - auto sb = S->getStateBlock(i); - if (sb) - { - cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )"; - } - if (i==1) cout << " ]" << endl; - } - if (S->getStateBlockVec().size() > 2) cout << " ]" << endl; - } - else if (metric) - { - cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( "; - if (S->getP()) - cout << S->getP()->getState().transpose(); - if (S->getO()) - cout << " " << S->getO()->getState().transpose(); - cout << " )"; - if (S->getIntrinsic()) - cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )"; - cout << endl; - } - else if (state_blocks) - { - cout << " sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]"); - for (auto sb : S->getStateBlockVec()) - if (sb != nullptr) - cout << " " << (sb->isFixed() ? "Fix" : "Est"); - cout << endl; - } - if (depth >= 2) - { - // Processors ======================================================================================= - for (auto p : S->getProcessorList()) - { - if (p->isMotion()) - { - 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()->isEstimated() ? " KF" : " F") - << pm->getOrigin()->getFrame()->id() << endl; - if (pm->getLast()) - cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? " KF" : " F") - << pm->getLast()->getFrame()->id() << endl; - if (pm->getIncoming()) - cout << " i: C" << pm->getIncoming()->id() << endl; - } - else - { - cout << " pt" << p->id() << " " << p->getType() << endl; - ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p); - if (pt) - { -// ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt); -// if (ptt) -// { -// if (ptt->getPrevOrigin()) -// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? " KF" : " F") -// << ptt->getPrevOrigin()->getFrame()->id() << endl; -// } - if (pt->getOrigin()) - cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? " KF" : " F") - << pt->getOrigin()->getFrame()->id() << endl; - if (pt->getLast()) - cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? " KF" : " F") - << pt->getLast()->getFrame()->id() << endl; - if (pt->getIncoming()) - cout << " i: C" << pt->getIncoming()->id() << endl; - } - } - } // for p - } - } // for S - } - cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl; - if (depth >= 1) - { - // Frames ======================================================================================= - for (auto F : getTrajectory()->getFrameList()) - { - cout << (F->isEstimated() ? (F->isImportant() ? " KF" : " EF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); - if (constr_by) - { - cout << " <-- "; - for (auto cby : F->getConstrainedByList()) - cout << "c" << cby->id() << " \t"; - } - cout << endl; - if (metric) - { - cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5) - << F->getTimeStamp().get(); - cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )"; - cout << endl; - } - if (state_blocks) - { - cout << " sb:"; - for (auto sb : F->getStateBlockVec()) - if (sb != nullptr) - cout << " " << (sb->isFixed() ? "Fix" : "Est"); - cout << endl; - } - if (depth >= 2) - { - // Captures ======================================================================================= - for (auto C : F->getCaptureList()) - { - cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType(); - - if(C->getSensor() != nullptr) - { - cout << " -> S" << C->getSensor()->id(); - cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, "); - cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]"); - } - else - cout << " -> S-"; - if (C->isMotion()) - { - auto CM = std::static_pointer_cast<CaptureMotion>(C); - if (CM->getOriginFrame()) - cout << " -> OF" << CM->getOriginFrame()->id(); - } - - cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : ""); - if (constr_by) - { - cout << " <-- "; - for (auto cby : C->getConstrainedByList()) - cout << "c" << cby->id() << " \t"; - } - cout << endl; - - if (state_blocks) - for(auto sb : C->getStateBlockVec()) - if(sb != nullptr) - { - cout << " sb: "; - cout << (sb->isFixed() ? "Fix" : "Est"); - if (metric) - cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )"; - cout << endl; - } - - if (C->isMotion() ) - { - CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C); - cout << " buffer size : " << CM->getBuffer().get().size() << endl; - if ( metric && ! CM->getBuffer().get().empty()) - { - cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl; - if (CM->hasCalibration()) - { - cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl; - cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl; - cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl; - cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl; - } - } - } - - if (depth >= 3) - { - // Features ======================================================================================= - for (auto f : C->getFeatureList()) - { - cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : ""); - if (constr_by) - { - cout << " <--\t"; - for (auto cby : f->getConstrainedByList()) - cout << "c" << cby->id() << " \t"; - } - cout << endl; - if (metric) - cout << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose() - << " )" << endl; - if (depth >= 4) - { - // Factors ======================================================================================= - for (auto c : f->getFactorList()) - { - cout << " c" << c->id() << " " << c->getType() << " -->"; - if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr) - cout << " A"; - if (c->getFrameOther() != nullptr) - cout << " F" << c->getFrameOther()->id(); - if (c->getCaptureOther() != nullptr) - cout << " C" << c->getCaptureOther()->id(); - if (c->getFeatureOther() != nullptr) - cout << " f" << c->getFeatureOther()->id(); - if (c->getLandmarkOther() != nullptr) - cout << " L" << c->getLandmarkOther()->id(); - cout << endl; - } // for c - } - } // for f - } - } // for C - } - } // for F - } - cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl; - if (depth >= 1) - { - // Landmarks ======================================================================================= - for (auto L : getMap()->getLandmarkList()) - { - cout << " L" << L->id() << " " << L->getType(); - if (constr_by) - { - cout << "\t<-- "; - for (auto cby : L->getConstrainedByList()) - cout << "c" << cby->id() << " \t"; - } - cout << endl; - if (metric) - { - cout << (L->isFixed() ? " Fix" : " Est"); - cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )"; - cout << endl; - } - if (state_blocks) - { - cout << " sb:"; - for (auto sb : L->getStateBlockVec()) - if (sb != nullptr) - cout << (sb->isFixed() ? " Fix" : " Est"); - cout << endl; - } - } // for L - } - cout << "-----------------------------------------" << endl; - cout << endl; -} - -FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) -{ - return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); -} - -bool Problem::check(int verbose_level) -{ - using std::cout; - using std::endl; - - bool is_consistent = true; // true if all checks passed; false if any check fails. - - if (verbose_level) cout << endl; - if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl; - auto P_raw = this; - if (verbose_level > 0) - { - cout << "P @ " << P_raw << endl; - } - // ------------------------ - // HARDWARE branch - // ------------------------ - auto H = hardware_ptr_; - if (verbose_level > 0) - { - cout << "H @ " << H.get() << endl; - } - // check pointer to Problem - is_consistent = is_consistent && (H->getProblem().get() == P_raw); - - // Sensors ======================================================================================= - for (auto S : H->getSensorList()) - { - if (verbose_level > 0) - { - cout << " S" << S->id() << " @ " << S.get() << endl; - cout << " -> P @ " << S->getProblem().get() << endl; - cout << " -> H @ " << S->getHardware().get() << endl; - for (auto sb : S->getStateBlockVec()) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - // check problem and hardware pointers - is_consistent = is_consistent && (S->getProblem().get() == P_raw); - is_consistent = is_consistent && (S->getHardware() == H); - - // Processors ======================================================================================= - for (auto p : S->getProcessorList()) - { - if (verbose_level > 0) - { - cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl; - cout << " -> P @ " << p->getProblem().get() << endl; - cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl; - } - // check problem and sensor pointers - is_consistent = is_consistent && (p->getProblem().get() == P_raw); - is_consistent = is_consistent && (p->getSensor() == S); - } - } - // ------------------------ - // TRAJECTORY branch - // ------------------------ - auto T = trajectory_ptr_; - if (verbose_level > 0) - { - cout << "T @ " << T.get() << endl; - } - // check pointer to Problem - is_consistent = is_consistent && (T->getProblem().get() == P_raw); - - // Frames ======================================================================================= - for (auto F : T->getFrameList()) - { - if (verbose_level > 0) - { - cout << (F->isEstimated() ? " KF" : " F") << F->id() << " @ " << F.get() << endl; - cout << " -> P @ " << F->getProblem().get() << endl; - cout << " -> T @ " << F->getTrajectory().get() << endl; - for (auto sb : F->getStateBlockVec()) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - // check problem and trajectory pointers - is_consistent = is_consistent && (F->getProblem().get() == P_raw); - is_consistent = is_consistent && (F->getTrajectory() == T); - for (auto cby : F->getConstrainedByList()) - { - if (verbose_level > 0) - { - cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl; - } - // check constrained_by pointer to this frame - is_consistent = is_consistent && (cby->getFrameOther() == F); - for (auto sb : cby->getStateBlockPtrVector()) - { - if (verbose_level > 0) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - } - - // Captures ======================================================================================= - for (auto C : F->getCaptureList()) - { - if (verbose_level > 0) - { - cout << " C" << C->id() << " @ " << C.get() << " -> S"; - if (C->getSensor()) cout << C->getSensor()->id(); - else cout << "-"; - cout << endl; - cout << " -> P @ " << C->getProblem().get() << endl; - cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl; - for (auto sb : C->getStateBlockVec()) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - // check problem and frame pointers - is_consistent = is_consistent && (C->getProblem().get() == P_raw); - is_consistent = is_consistent && (C->getFrame() == F); - - // Features ======================================================================================= - for (auto f : C->getFeatureList()) - { - if (verbose_level > 0) - { - cout << " f" << f->id() << " @ " << f.get() << endl; - cout << " -> P @ " << f->getProblem().get() << endl; - cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get() - << endl; - } - // check problem and capture pointers - is_consistent = is_consistent && (f->getProblem().get() == P_raw); - is_consistent = is_consistent && (f->getCapture() == C); - - for (auto cby : f->getConstrainedByList()) - { - if (verbose_level > 0) - { - cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl; - } - // check constrained_by pointer to this feature - is_consistent = is_consistent && (cby->getFeatureOther() == f); - } - - // Factors ======================================================================================= - for (auto c : f->getFactorList()) - { - if (verbose_level > 0) - cout << " c" << c->id() << " @ " << c.get(); - - auto Fo = c->getFrameOther(); - auto Co = c->getCaptureOther(); - auto fo = c->getFeatureOther(); - auto Lo = c->getLandmarkOther(); - - if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE: - { - if (verbose_level > 0) - cout << " --> Abs." << endl; - } - - // find constrained_by pointer in constrained frame - if ( Fo ) // case FRAME: - { - if (verbose_level > 0) - cout << " --> F" << Fo->id() << " <- "; - bool found = false; - for (auto cby : Fo->getConstrainedByList()) - { - if (verbose_level > 0) - cout << " c" << cby->id(); - found = found || (c == cby); - } - if (verbose_level > 0) - cout << endl; - // check constrained_by pointer in constrained frame - is_consistent = is_consistent && found; - } - - // find constrained_by pointer in constrained capture - if ( Co ) // case CAPTURE: - { - if (verbose_level > 0) - cout << " --> C" << Co->id() << " <- "; - bool found = false; - for (auto cby : Co->getConstrainedByList()) - { - if (verbose_level > 0) - cout << " c" << cby->id(); - found = found || (c == cby); - } - if (verbose_level > 0) - cout << endl; - // check constrained_by pointer in constrained frame - is_consistent = is_consistent && found; - } - - // find constrained_by pointer in constrained feature - if ( fo ) // case FEATURE: - { - if (verbose_level > 0) - cout << " --> f" << fo->id() << " <- "; - bool found = false; - for (auto cby : fo->getConstrainedByList()) - { - if (verbose_level > 0) - cout << " c" << cby->id(); - found = found || (c == cby); - } - if (verbose_level > 0) - cout << endl; - // check constrained_by pointer in constrained feature - is_consistent = is_consistent && found; - } - - // find constrained_by pointer in constrained landmark - if ( Lo ) // case LANDMARK: - { - if (verbose_level > 0) - cout << " --> L" << Lo->id() << " <- "; - bool found = false; - for (auto cby : Lo->getConstrainedByList()) - { - if (verbose_level > 0) - cout << " c" << cby->id(); - found = found || (c == cby); - } - if (verbose_level > 0) - cout << endl; - // check constrained_by pointer in constrained landmark - is_consistent = is_consistent && found; - } - if (verbose_level > 0) - { - cout << " -> P @ " << c->getProblem().get() << endl; - cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl; - } - // check problem and feature pointers - is_consistent = is_consistent && (c->getProblem().get() == P_raw); - is_consistent = is_consistent && (c->getFeature() == f); - - // find state block pointers in all constrained nodes - SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb - for (auto sb : c->getStateBlockPtrVector()) - { - bool found = false; - if (verbose_level > 0) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - } - // find in own Frame - found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end()); - // find in own Capture - found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end()); - // find in own Sensor - if (S) - found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end()); - // find in constrained Frame - if (Fo) - found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end()); - // find in constrained Capture - if (Co) - found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end()); - // find in constrained Feature - if (fo) - { - // find in constrained feature's Frame - FrameBasePtr foF = fo->getFrame(); - found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end()); - // find in constrained feature's Capture - CaptureBasePtr foC = fo->getCapture(); - found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end()); - // find in constrained feature's Sensor - SensorBasePtr foS = fo->getCapture()->getSensor(); - found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end()); - } - // find in constrained landmark - if (Lo) - found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end()); - if (verbose_level > 0) - { - if (found) - cout << " found"; - else - cout << " NOT FOUND !"; - cout << endl; - } - // check that all state block pointers were found - is_consistent = is_consistent && found; - } - } - } - } - } - // ------------------------ - // MAP branch - // ------------------------ - auto M = map_ptr_; - if (verbose_level > 0) - cout << "M @ " << M.get() << endl; - // check pointer to Problem - is_consistent = is_consistent && (M->getProblem().get() == P_raw); - - // Landmarks ======================================================================================= - for (auto L : M->getLandmarkList()) - { - if (verbose_level > 0) - { - cout << " L" << L->id() << " @ " << L.get() << endl; - cout << " -> P @ " << L->getProblem().get() << endl; - cout << " -> M @ " << L->getMap().get() << endl; - for (auto sb : L->getStateBlockVec()) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - // check problem and map pointers - is_consistent = is_consistent && (L->getProblem().get() == P_raw); - is_consistent = is_consistent && (L->getMap() == M); - for (auto cby : L->getConstrainedByList()) - { - if (verbose_level > 0) - cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl; - // check constrained_by pointer to this landmark - is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id()); - for (auto sb : cby->getStateBlockPtrVector()) - { - if (verbose_level > 0) - { - cout << " sb @ " << sb.get(); - if (sb) - { - auto lp = sb->getLocalParametrization(); - if (lp) - cout << " (lp @ " << lp.get() << ")"; - } - cout << endl; - } - } - } - } - - if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl; - if (verbose_level) cout << endl; - - return is_consistent; -} - -void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) -{ - if (depth.compare("T") == 0) - print(0, constr_by, metric, state_blocks); - else if (depth.compare("F") == 0) - print(1, constr_by, metric, state_blocks); - else if (depth.compare("C") == 0) - print(2, constr_by, metric, state_blocks); - else if (depth.compare("f") == 0) - print(3, constr_by, metric, state_blocks); - else if (depth.compare("c") == 0) - print(4, constr_by, metric, state_blocks); - else - print(0, constr_by, metric, state_blocks); -} - -} // namespace wolf diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 3f5214a6f120001d6e37627fabb2f97ad9b9c103..bb913e042a5b14a4e7e1c3bfe9c1f8fbdf858e13 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -127,7 +127,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // // setting the origin in all processor motion if origin already setted if (prc_ptr->isMotion() && prior_is_set_) - (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastImportantFrame()); + (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); // setting the main processor motion if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) @@ -251,8 +251,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state) { if (processor_motion_ptr_ != nullptr) processor_motion_ptr_->getCurrentState(state); - else if (trajectory_ptr_->getLastImportantFrame() != nullptr) - trajectory_ptr_->getLastImportantFrame()->getState(state); + else if (trajectory_ptr_->getLastEstimatedFrame() != nullptr) + trajectory_ptr_->getLastEstimatedFrame()->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_->getLastImportantFrame() != nullptr) + else if (trajectory_ptr_->getLastEstimatedFrame() != nullptr) { - trajectory_ptr_->getLastImportantFrame()->getTimeStamp(ts); - trajectory_ptr_->getLastImportantFrame()->getState(state); + trajectory_ptr_->getLastEstimatedFrame()->getTimeStamp(ts); + trajectory_ptr_->getLastEstimatedFrame()->getState(state); } else state = zeroState(); @@ -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); @@ -583,7 +607,13 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) { - FrameBasePtr frm = getLastImportantFrame(); + FrameBasePtr frm = getLastKeyFrame(); + return getFrameCovariance(frm, cov); +} + +bool Problem::getLastEstimatedFrameCovariance(Eigen::MatrixXs& cov) +{ + FrameBasePtr frm = getLastEstimatedFrame(); return getFrameCovariance(frm, cov); } @@ -644,9 +674,9 @@ FrameBasePtr Problem::getLastFrame() const return trajectory_ptr_->getLastFrame(); } -FrameBasePtr Problem::getLastImportantFrame() const +FrameBasePtr Problem::getLastKeyFrame() const { - return trajectory_ptr_->getLastImportantFrame(); + return trajectory_ptr_->getLastKeyFrame(); } FrameBasePtr Problem::getLastEstimatedFrame() const @@ -654,9 +684,9 @@ FrameBasePtr Problem::getLastEstimatedFrame() const return trajectory_ptr_->getLastEstimatedFrame(); } -FrameBasePtr Problem::closestImportantFrameToTimeStamp(const TimeStamp& _ts) const +FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const { - return trajectory_ptr_->closestImportantFrameToTimeStamp(_ts); + return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts); } FrameBasePtr Problem::closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const @@ -669,7 +699,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: if ( ! prior_is_set_ ) { // Create origin frame - FrameBasePtr origin_keyframe = emplaceFrame(IMPORTANT, _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 @@ -778,10 +808,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()->isEstimated() ? (pm->getOrigin()->getFrame()->isImportant() ? " KF" : " EF" ) : " F") + cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isEstimated() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F") << pm->getOrigin()->getFrame()->id() << endl; if (pm->getLast()) - cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? (pm->getLast()->getFrame()->isImportant() ? " KF" : " EF") : " F") + cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F") << pm->getLast()->getFrame()->id() << endl; if (pm->getIncoming()) cout << " i: C" << pm->getIncoming()->id() << endl; @@ -796,14 +826,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()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isImportant() ? " KF" : " EF") : " 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()->isEstimated() ? (pt->getOrigin()->getFrame()->isImportant() ? " KF" : " EF") : " F") + cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F") << pt->getOrigin()->getFrame()->id() << endl; if (pt->getLast()) - cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? (pt->getLast()->getFrame()->isImportant() ? " KF" : " EF") : " F") + cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F") << pt->getLast()->getFrame()->id() << endl; if (pt->getIncoming()) cout << " i: C" << pt->getIncoming()->id() << endl; @@ -819,7 +849,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) // Frames ======================================================================================= for (auto F : getTrajectory()->getFrameList()) { - cout << (F->isEstimated() ? (F->isImportant() ? " KF" : " EF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); + cout << (F->isEstimated() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : ""); if (constr_by) { cout << " <-- "; @@ -1055,7 +1085,7 @@ bool Problem::check(int verbose_level) { if (verbose_level > 0) { - cout << (F->isEstimated() ? (F->isImportant() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl; + cout << (F->isEstimated() ? (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_base.cpp b/src/processor/processor_base.cpp index 9de484dfa474baf1cc8bc9bc56b4b20eed7aeb89..260f416988b32c8a9ffc8ccbcbc014a4d0be021a 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -28,7 +28,7 @@ bool ProcessorBase::permittedKeyFrame() FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr) { - std::cout << "Making " << (_type == ESTIMATED? "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_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp index ee1d2e03e79b6c71439a112e6afe6f8d9115417f..7c4e63a75031de399473c9fa1c6e764fa28331a9 100644 --- a/src/processor/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -72,7 +72,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / // check for LC just if frame is key frame // Assert that the evaluated KF has a capture of the // same sensor as this processor - if (key_it->isEstimated() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr) + if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr) { // Check if the two frames currently evaluated are already // constrained one-another. @@ -82,7 +82,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / for (const FactorBasePtr& crt : fac_list) { // Are the two frames constrained one-another ? - if (crt->getFrameOther() == problem_ptr->getLastImportantFrame()) + if (crt->getFrameOther() == problem_ptr->getLastKeyFrame()) { // By this very processor ? if (crt->getProcessor() == shared_from_this()) @@ -127,7 +127,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / Eigen::Vector5s frame_covariance, current_covariance; if (!computeEllipse2D(key_it, frame_covariance)) continue; - if (!computeEllipse2D(getProblem()->getLastImportantFrame(), + if (!computeEllipse2D(getProblem()->getLastKeyFrame(), current_covariance)) continue; found_possible_candidate = ellipse2DIntersect(frame_covariance, current_covariance); @@ -160,7 +160,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / Eigen::Vector10s frame_covariance, current_covariance; if (!computeEllipsoid3D(key_it, frame_covariance)) continue; - if (!computeEllipsoid3D(getProblem()->getLastImportantFrame(), + if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(), frame_covariance)) continue; found_possible_candidate = ellipsoid3DIntersect(frame_covariance, current_covariance); @@ -170,7 +170,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& / case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE: { found_possible_candidate = insideMahalanisDistance(key_it, - problem_ptr->getLastImportantFrame()); + problem_ptr->getLastKeyFrame()); break; } @@ -496,7 +496,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP //############################################################################## bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr) { - FrameBasePtr keyframe = getProblem()->getLastImportantFrame(); + FrameBasePtr keyframe = getProblem()->getLastKeyFrame(); if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ )) return false; else diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 622c766d2352833020a757ae1b66dfda42a50ec1..755b6c95091c88ad7c5f3362caa3950714046d0f 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -193,11 +193,11 @@ 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(); - key_frame_ptr->setEstimated(); + key_frame_ptr->setKey(); // create motion feature and add it to the key_capture auto key_feature_ptr = emplaceFeature(last_ptr_); @@ -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(ESTIMATED, _x_origin, _ts_origin); + FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin); setOrigin(key_frame_ptr); return key_frame_ptr; @@ -299,7 +299,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr."); assert(_origin_frame->getTrajectory() != nullptr && "ProcessorMotion::setOrigin: origin frame must be in the trajectory."); - assert(_origin_frame->isEstimated() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME."); + assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME."); // -------- ORIGIN --------- // emplace (empty) origin Capture diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 87be1401e46bb0493b33c1dd99cfdb2a64653f5c..5d97526e108fccced45d966bd49c3e5d795c9b84 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(ESTIMATED, incoming_ptr_->getTimeStamp()); + FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp()); kfrm->addCapture(incoming_ptr_); // Process info @@ -171,7 +171,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // set KF on last last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); - last_ptr_->getFrame()->setEstimated(); + last_ptr_->getFrame()->setKey(); // make F; append incoming to new F FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()); @@ -180,9 +180,6 @@ void ProcessorTracker::process(CaptureBasePtr const _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())); @@ -258,7 +255,7 @@ void ProcessorTracker::computeProcessingStep() if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance)) { - if (last_ptr_->getFrame()->isEstimated()) + if (last_ptr_->getFrame()->isKey()) { WOLF_WARN("||*||"); WOLF_INFO(" ... It seems you missed something!"); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 26cd97067e33d1a3546e1422e3527d31f8ac9e54..209dc12e04ed770ec797f2d10b27173b17fe0458 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -226,7 +226,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) FrameBaseRevIter frame_rev_it = frame_list.rbegin(); while (frame_rev_it != frame_list.rend()) { - if ((*frame_rev_it)->isEstimated()) + if ((*frame_rev_it)->isKey()) { capture = (*frame_rev_it)->getCaptureOf(shared_from_this()); if (capture) diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 735e4187079038eed700c8a499b1b6a4ae4b85fb..ca899deef67722885fd6dab673ec834410237ebb 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -6,7 +6,7 @@ namespace wolf { TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) : NodeBase("TRAJECTORY", "Base"), frame_structure_(_frame_structure), - last_important_frame_ptr_(nullptr), + last_key_frame_ptr_(nullptr), last_estimated_frame_ptr_(nullptr) { // WOLF_DEBUG("constructed T"); diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index 202f566a6eca49236165a2a2c8a10bb111a4b20d..7c65c1a341a691aaf3135f7c206717d3056634b0 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(ESTIMATED, 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 906d810cfb843b91606c7c32afb79d66e59a4fe3..ad8852abc92a528381ed5f8b859f9909309617f0 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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 b445e4b1a77eb3f64d43c135ecebdec2855bf16d..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(ESTIMATED, 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 3b64eafe603c25edbc91070e0718874c3044b8ea..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 (ESTIMATED, pose1, 1.0); + F1 = problem->emplaceFrame (KEY, pose1, 1.0); - F2 = problem->emplaceFrame (ESTIMATED, 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 5eb632976ba8ca2793226992b366b1e499971d5b..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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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 b7c8e5899d8036e0cf8338793482c3fec700059c..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(ESTIMATED, problem_ptr->zeroState(), TimeStamp(0)); -FrameBasePtr frm1 = problem_ptr->emplaceFrame(ESTIMATED, 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 aa1b2db5d9d1c2a64c0a28ba53bffb4fe0eb130a..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(ESTIMATED, 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 e7f91dc2d995debc25bb97215cc295039b20971e..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(ESTIMATED, 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 3cc3a029bba96de511e7b22de6e1f0a30e6b7ae8..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>(ESTIMATED, 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 @@ -119,9 +119,9 @@ TEST_F(FeatureIMU_test, check_frame) origin_frame->getTimeStamp(ts); ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl; - ASSERT_TRUE(origin_frame->isEstimated()); - ASSERT_TRUE(last_frame->isEstimated()); - ASSERT_TRUE(left_frame->isEstimated()); + ASSERT_TRUE(origin_frame->isKey()); + ASSERT_TRUE(last_frame->isKey()); + ASSERT_TRUE(left_frame->isKey()); wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr; origin_pptr = origin_frame->getP(); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 0e0ff058e4cd43c481f68880ccdb430460504f6e..fc87237511b807fb40e7fa7fd2f4f6a0bf52519f 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -31,6 +31,7 @@ TEST(FrameBase, GettersAndSetters) F->getTimeStamp(t); ASSERT_EQ(t, 1); ASSERT_FALSE(F->isFixed()); + ASSERT_EQ(F->isKey(), false); ASSERT_EQ(F->isEstimated(), false); } @@ -121,7 +122,8 @@ TEST(FrameBase, LinksToTree) ASSERT_TRUE(F1->isFixed()); // set key - F1->setEstimated(); + F1->setKey(); + ASSERT_TRUE(F1->isKey()); ASSERT_TRUE(F1->isEstimated()); // Unlink diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 7043b1be1b46e838db1ea6905f808a0ad43732ce..51dbc519a84fee9b38d393250ac90af0d42e9594 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -75,7 +75,7 @@ void show(const ProblemPtr& problem) for (FrameBasePtr F : problem->getTrajectory()->getFrameList()) { - if (F->isEstimated()) + if (F->isKey()) { cout << "----- Key Frame " << F->id() << " -----" << endl; if (!F->getCaptureList().empty()) @@ -131,7 +131,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) // KF1 and motion from KF0 t += dt; - FrameBasePtr F1 = Pr->emplaceFrame(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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(ESTIMATED, 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 17e02e54087f0dc7ec6d5f017245e254bcbabd89..4fe1cbcd4ff0bfca536939d9cf97b77e6bd113d0 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -112,7 +112,7 @@ TEST(Problem, SetOrigin_PO_2D) ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 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(); @@ -130,8 +130,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); } @@ -177,9 +177,9 @@ TEST(Problem, emplaceFrame_factory) { ProblemPtr P = Problem::create("PO 2D"); - FrameBasePtr f0 = P->emplaceFrame("PO 2D", ESTIMATED, VectorXs(3), TimeStamp(0.0)); - FrameBasePtr f1 = P->emplaceFrame("PO 3D", ESTIMATED, VectorXs(7), TimeStamp(1.0)); - FrameBasePtr f2 = P->emplaceFrame("POV 3D", ESTIMATED, 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; @@ -223,7 +223,7 @@ TEST(Problem, StateBlocks) ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); // 2 state blocks, estimated - P->emplaceFrame("PO 3D", ESTIMATED, xs, 0); + P->emplaceFrame("PO 3D", KEY, xs, 0); ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call // P->print(4,1,1,1); @@ -256,7 +256,7 @@ TEST(Problem, Covariances) // 4 state blocks, estimated St->unfixExtrinsics(); - FrameBasePtr F = P->emplaceFrame("PO 3D", ESTIMATED, 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()); diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 0a3a0ffacb2f58249dfde5d48b14693fc5de19af..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 Important Frame creations (callback calls) + // Sequence to test Key Frame creations (callback calls) // initialize TimeStamp t(0.0); @@ -89,7 +89,7 @@ TEST(ProcessorBase, KeyFrameCallback) // problem->print(4,1,1,0); // Only odom creating KFs - ASSERT_TRUE( problem->getLastImportantFrame()->getType().compare("PO 2D")==0 ); + ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 ); } } diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index 9bb2238ed38ef20e0a9fd51734a3968fdfe81945..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::ESTIMATED, + F1 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr1, stateblock_optr1); - F2 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED, + F2 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr2, stateblock_optr2); - F3 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED, + F3 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr3, stateblock_optr3); - F4 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED, + F4 = std::make_shared<wolf::FrameBase>(wolf::KEY, 1, stateblock_pptr4, stateblock_optr4); @@ -147,7 +147,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated) incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY", 1.2, sensor_ptr); - // Make 3rd frame last Important frame + // Make 3rd frame last Key frame F3->setTimeStamp(wolf::TimeStamp(2.0)); problem->getTrajectory()->sortFrame(F3); @@ -181,7 +181,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) problem->addCovarianceBlock( F4->getP(), F4->getP(), position_covariance_matrix); - // Make 3rd frame last Important frame + // Make 3rd frame last Key frame F3->setTimeStamp(wolf::TimeStamp(2.0)); problem->getTrajectory()->sortFrame(F3); @@ -194,7 +194,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance) ASSERT_EQ(testloops[0]->id(), (unsigned int) 1); ASSERT_EQ(testloops[1]->id(), (unsigned int) 2); - // Make 4th frame last Important frame + // Make 4th frame last Key frame F4->setTimeStamp(wolf::TimeStamp(3.0)); problem->getTrajectory()->sortFrame(F4); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 962572c3de17c2de464829d48e65f8d12088027d..6792b5d84e8889ea4064cbd4f17e972715b76e17 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -499,7 +499,7 @@ TEST(SolverManager, AddFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(ESTIMATED, 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))); @@ -521,7 +521,7 @@ TEST(SolverManager, RemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(ESTIMATED, 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))); @@ -549,7 +549,7 @@ TEST(SolverManager, AddRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(ESTIMATED, 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))); @@ -576,7 +576,7 @@ TEST(SolverManager, DoubleRemoveFactor) StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); // Create (and add) factor point 2d - FrameBasePtr F = P->emplaceFrame(ESTIMATED, 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 7b2849760750077590692300c3c13823d1ac46ea..f5dd60c2cad9552167220fc7faf34f27fb53020b 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -60,6 +60,47 @@ struct DummyNotificationProcessor /// Set to true if you want debug info bool debug = false; +TEST(TrajectoryBase, ClosestKeyFrame) +{ + + ProblemPtr P = Problem::create("PO 2D"); + TrajectoryBasePtr T = P->getTrajectory(); + + // Trajectory status: + // 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, ClosestEstimatedFrame) { @@ -67,33 +108,33 @@ TEST(TrajectoryBase, ClosestEstimatedFrame) 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>(ESTIMATED, 1, nullptr, nullptr); - FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED, 2, nullptr, nullptr); - FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 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->closestEstimatedFrameToTimeStamp(-0.8); // before all keyframes --> return f0 - ASSERT_EQ(kf->id(), f1->id()); // same id! + KF = T->closestEstimatedFrameToTimeStamp(-0.8); // before all keyframes --> return f0 + ASSERT_EQ(KF->id(), F1->id()); // same id! - kf = T->closestEstimatedFrameToTimeStamp(1.1); // between keyframes --> return f1 - ASSERT_EQ(kf->id(), f1->id()); // same id! + KF = T->closestEstimatedFrameToTimeStamp(1.1); // between keyframes --> return F1 + ASSERT_EQ(KF->id(), F1->id()); // same id! - kf = T->closestEstimatedFrameToTimeStamp(1.9); // between keyframes --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! + KF = T->closestEstimatedFrameToTimeStamp(1.9); // between keyframes --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - kf = T->closestEstimatedFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! + KF = T->closestEstimatedFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! - kf = T->closestEstimatedFrameToTimeStamp(3.2); // after the last frame --> return f2 - ASSERT_EQ(kf->id(), f2->id()); // same id! + KF = T->closestEstimatedFrameToTimeStamp(3.2); // after the last frame --> return F2 + ASSERT_EQ(KF->id(), F2->id()); // same id! } TEST(TrajectoryBase, Add_Remove_Frame) @@ -106,36 +147,36 @@ TEST(TrajectoryBase, Add_Remove_Frame) DummyNotificationProcessor N(P); // Trajectory status: - // kf1 kf2 f3 frames + // KF1 KF2 F3 frames // 1 2 3 time stamps // --+-----+-----+---> time - FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED, 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 + 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(), (unsigned int) 1); ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 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(), (unsigned int) 2); 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(), (unsigned int) 3); ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id()); std::cout << __LINE__ << std::endl; N.update(); @@ -143,24 +184,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(), (unsigned int) 2); ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastFrame()->id(), f3->id()); - ASSERT_EQ(T->getLastImportantFrame()->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(), (unsigned int) 1); std::cout << __LINE__ << std::endl; - ASSERT_EQ(T->getLastImportantFrame()->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(), (unsigned int) 0); std::cout << __LINE__ << std::endl; @@ -179,70 +220,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>(ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed - FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED, 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 + 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->getLastImportantFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->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->getLastImportantFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->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->getLastImportantFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); - f3->setEstimated(); // set KF3 + F3->setKey(); // set KF3 if (debug) P->print(2,0,0,0); - ASSERT_EQ(T->getLastFrame() ->id(), f3->id()); - ASSERT_EQ(T->getLastImportantFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - FrameBasePtr f4 = std::make_shared<FrameBase>(NON_ESTIMATED, 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->getLastImportantFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F4->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->id(), F3->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id()); - f4->setEstimated(); + 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->getLastImportantFrame()->id(), f3->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F3->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->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->getLastImportantFrame()->id(), f2->id()); + ASSERT_EQ(T->getLastFrame() ->id(), F2->id()); + ASSERT_EQ(T->getLastEstimatedFrame()->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->getLastEstimatedFrame()->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->getLastEstimatedFrame()->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->getLastEstimatedFrame()->id(), F5->id()); + ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id()); }