diff --git a/src/factory.h b/src/factory.h index 0c1060b2cb8f37bbd2f44dbd77935d66cb58d9b2..6d32d3d278cc13f7d95a570aef4e68aa2abc2b17 100644 --- a/src/factory.h +++ b/src/factory.h @@ -349,7 +349,7 @@ inline std::string LandmarkFactory::getClass() // Frames class FrameBase; class TimeStamp; -typedef Factory<FrameBase, const FrameKeyType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory; +typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory; template<> inline std::string FrameFactory::getClass() { diff --git a/src/frame_base.cpp b/src/frame_base.cpp index c76546b419ecac3f2b2f2ff170a2d709a676c9e2..c312b1a7aa6f3d12c5fe46b0b8dd15f318018c5f 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -15,7 +15,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ trajectory_ptr_(), state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), - type_id_(NON_KEY_FRAME), + type_(NON_KEY_FRAME), status_(ST_ESTIMATED), time_stamp_(_ts) { @@ -29,12 +29,12 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ std::cout << "constructed +F" << id() << std::endl; } -FrameBase::FrameBase(const FrameKeyType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : +FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "BASE"), trajectory_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), - type_id_(_tp), + type_(_tp), status_(ST_ESTIMATED), time_stamp_(_ts) { @@ -104,7 +104,7 @@ void FrameBase::removeStateBlocks() auto sbp = getStateBlockPtr(i); if (sbp != nullptr) { - if (getProblem() != nullptr && type_id_ == KEY_FRAME) + if (getProblem() != nullptr && type_ == KEY_FRAME) { getProblem()->removeStateBlockPtr(sbp); } @@ -115,9 +115,9 @@ void FrameBase::removeStateBlocks() void FrameBase::setKey() { - if (type_id_ != KEY_FRAME) + if (type_ != KEY_FRAME) { - type_id_ = KEY_FRAME; + type_ = KEY_FRAME; registerNewStateBlocks(); if (getTrajectoryPtr()->getLastKeyFramePtr() == nullptr || getTrajectoryPtr()->getLastKeyFramePtr()->getTimeStamp() < time_stamp_) @@ -302,7 +302,7 @@ void FrameBase::setStatus(StateStatus _st) } } -FrameBasePtr FrameBase::create_PO_2D(const FrameKeyType & _tp, +FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) { @@ -314,7 +314,7 @@ FrameBasePtr FrameBase::create_PO_2D(const FrameKeyType & _tp, f->setType("PO 2D"); return f; } -FrameBasePtr FrameBase::create_PO_3D(const FrameKeyType & _tp, +FrameBasePtr FrameBase::create_PO_3D(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) { @@ -326,7 +326,7 @@ FrameBasePtr FrameBase::create_PO_3D(const FrameKeyType & _tp, f->setType("PO 3D"); return f; } -FrameBasePtr FrameBase::create_POV_3D(const FrameKeyType & _tp, +FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) { diff --git a/src/frame_base.h b/src/frame_base.h index aef6762b49bed9094ef7e307996c7313b723311e..b7a2aaa1fe8bab7361f2b6bf13fdc742d88cbe17 100644 --- a/src/frame_base.h +++ b/src/frame_base.h @@ -25,13 +25,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase TrajectoryBaseWPtr trajectory_ptr_; CaptureBaseList capture_list_; ConstraintBaseList constrained_by_list_; - std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, V. + std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity. static unsigned int frame_id_count_; protected: unsigned int frame_id_; - FrameKeyType type_id_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h) + FrameType type_; ///< type of frame. Either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h) StateStatus status_; ///< status of the estimation of the frame state TimeStamp time_stamp_; ///< frame time stamp @@ -55,7 +55,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase * \param _o_ptr StateBlock pointer to the orientation (default: nullptr) * \param _v_ptr StateBlock pointer to the velocity (default: nullptr). **/ - FrameBase(const FrameKeyType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); + FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); virtual ~FrameBase(); void remove(); @@ -135,13 +135,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase void setStatus(StateStatus _st); public: - static FrameBasePtr create_PO_2D (const FrameKeyType & _tp, + static FrameBasePtr create_PO_2D (const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(3)); - static FrameBasePtr create_PO_3D (const FrameKeyType & _tp, + static FrameBasePtr create_PO_3D (const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(7)); - static FrameBasePtr create_POV_3D(const FrameKeyType & _tp, + static FrameBasePtr create_POV_3D(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10)); }; @@ -180,7 +180,7 @@ inline unsigned int FrameBase::id() inline bool FrameBase::isKey() const { - return (type_id_ == KEY_FRAME); + return (type_ == KEY_FRAME); } inline void FrameBase::fix() diff --git a/src/frame_imu.cpp b/src/frame_imu.cpp index f2bee3bf137c1cc3bc52fdc5ff8107a45f850c38..e07fcaabedd929aaa10047f20f164790060a08f4 100644 --- a/src/frame_imu.cpp +++ b/src/frame_imu.cpp @@ -17,7 +17,7 @@ FrameIMU::FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ setType("IMU"); } -FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr, +FrameIMU::FrameIMU(const FrameType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr, StateQuaternionPtr _q_ptr, StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) : FrameBase(_tp, _ts, _p_ptr, _q_ptr, _v_ptr) { @@ -27,7 +27,7 @@ FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr setType("IMU"); } -FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) : +FrameIMU::FrameIMU(const FrameType& _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) : FrameBase(_tp, _ts, std::make_shared<StateBlock>(3), std::make_shared<StateQuaternion>(), std::make_shared<StateBlock>(3)) { resizeStateBlockVec(5); // could have done push_back, but prefer more explicit locations for the StateBlocks @@ -213,7 +213,7 @@ FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, const Eigen::V -FrameBasePtr FrameIMU::create(const FrameKeyType & _tp, +FrameBasePtr FrameIMU::create(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) { diff --git a/src/frame_imu.h b/src/frame_imu.h index 83fbe273f51f22c27cf243af93317e8ee171102f..412c53ebf8862130362f8cc0891226242b501527 100644 --- a/src/frame_imu.h +++ b/src/frame_imu.h @@ -47,7 +47,7 @@ namespace wolf { * \param _ba_ptr StateBlock pointer to the acceleration bias (default: nullptr). * \param _bg_ptr StateBlock pointer to the gyrometer bias (default: nullptr). **/ - FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr = nullptr, + FrameIMU(const FrameType& _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_ptr = nullptr, StateQuaternionPtr _q_ptr = nullptr, StateBlockPtr _ba_ptr = nullptr, StateBlockPtr _bg_ptr = nullptr); /** \brief Constructor with type, time stamp and state vector @@ -55,7 +55,7 @@ namespace wolf { * \param _ts is the time stamp associated to this frame, provided in seconds * \param _x state vector of size 16, organized as [position, quaternion, velocity, acc_bias, gyro_bias] **/ - FrameIMU(const FrameKeyType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x); + FrameIMU(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x); virtual ~FrameIMU(); // Frame values ------------------------------------------------ @@ -72,7 +72,7 @@ namespace wolf { **/ void setStatus(StateStatus _st); public: - static FrameBasePtr create(const FrameKeyType & _tp, + static FrameBasePtr create(const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(16)); }; diff --git a/src/problem.cpp b/src/problem.cpp index ec568d91e10e403bf814f36be1f192664bc5ce6b..4d6f338ded1ff6e262593d93116319622e51aa72 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -172,12 +172,12 @@ void Problem::setProcessorMotion(ProcessorMotion::Ptr _processor_motion_ptr) processor_motion_ptr_ = _processor_motion_ptr; } -FrameBasePtr Problem::createFrame(FrameKeyType _frame_type, const TimeStamp& _time_stamp) +FrameBasePtr Problem::createFrame(FrameType _frame_type, const TimeStamp& _time_stamp) { return createFrame(_frame_type, getStateAtTimeStamp(_time_stamp), _time_stamp); } -FrameBasePtr Problem::createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state, +FrameBasePtr Problem::createFrame(FrameType _frame_key_type, const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp) { //std::cout << "Problem::createFrame" << std::endl; diff --git a/src/problem.h b/src/problem.h index 5c67b30d9c182d4597a5a7cd2bea7624b2ee5484..62b8bef241e096e885f9295262227e161c59c23f 100644 --- a/src/problem.h +++ b/src/problem.h @@ -147,13 +147,13 @@ class Problem : public std::enable_shared_from_this<Problem> * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem */ - FrameBasePtr createFrame(FrameKeyType _frame_key_type, const TimeStamp& _time_stamp); + FrameBasePtr createFrame(FrameType _frame_key_type, const TimeStamp& _time_stamp); /** \brief Create Frame from vector * * This acts as a Frame factory, but also takes care to update related lists in WolfProblem */ - FrameBasePtr createFrame(FrameKeyType _frame_key_type, const Eigen::VectorXs& _frame_state, + FrameBasePtr createFrame(FrameType _frame_key_type, const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp); Eigen::VectorXs getCurrentState(); diff --git a/src/processor_base.cpp b/src/processor_base.cpp index 94158c29f7f0a1c31348c0df805c929fd2e2b7f7..886d959c00935812b87c005774a896e6bb738076 100644 --- a/src/processor_base.cpp +++ b/src/processor_base.cpp @@ -10,7 +10,6 @@ ProcessorBase::ProcessorBase(const std::string& _type, const Scalar& _time_toler NodeBase("PROCESSOR"), sensor_ptr_(), processor_id_(++processor_id_count_), -// type_id_(_tp), time_tolerance_(_time_tolerance) { std::cout << "constructed +p" << id() << std::endl; @@ -28,7 +27,7 @@ bool ProcessorBase::permittedKeyFrame() return getProblem()->permitKeyFrame(shared_from_this()); } -FrameBasePtr ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, FrameKeyType _type) +FrameBasePtr ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, FrameType _type) { std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl; @@ -39,7 +38,7 @@ FrameBasePtr ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, FrameKeyType return new_frame_ptr; } -FrameBasePtr ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type) +FrameBasePtr ProcessorBase::makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameType _type) { // We need to create the new free Frame to hold what will become the last Capture std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl; diff --git a/src/processor_base.h b/src/processor_base.h index fe742577ae032e4a6658a3b45e4e05a0c30b4419..fc8405c3a2260adb65f4869561da8f2b80fe081e 100644 --- a/src/processor_base.h +++ b/src/processor_base.h @@ -54,8 +54,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce /**\brief make a Frame with the provided Capture */ - FrameBasePtr makeFrame(CaptureBasePtr _capture_ptr, FrameKeyType _type = NON_KEY_FRAME); - FrameBasePtr makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameKeyType _type = NON_KEY_FRAME); + FrameBasePtr makeFrame(CaptureBasePtr _capture_ptr, FrameType _type = NON_KEY_FRAME); + FrameBasePtr makeFrame(CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state, FrameType _type = NON_KEY_FRAME); virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0; diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index af6bd304f0abafe8e29304b3b7d46054949bae18..188a26a93d9c95e95b0e0c892588ac751dd353b0 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -12,7 +12,6 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc hardware_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. sensor_id_(++sensor_id_count_), // simple ID factory -// type_id_(_tp), extrinsic_dynamic_(_extr_dyn), noise_std_(_noise_size), noise_cov_(_noise_size, _noise_size) @@ -30,7 +29,6 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc hardware_ptr_(), state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed. sensor_id_(++sensor_id_count_), // simple ID factory -// type_id_(_tp), extrinsic_dynamic_(_extr_dyn), noise_std_(_noise_std), noise_cov_(_noise_std.size(), _noise_std.size()) diff --git a/src/wolf.h b/src/wolf.h index 5cda4674369e786e9e111fa5d07c44b12c28c81e..0712ccd43a094aae4ce0c26b6db52dc0e91a98c6 100644 --- a/src/wolf.h +++ b/src/wolf.h @@ -102,15 +102,13 @@ typedef Rotation2D<wolf::Scalar> Rotation2Ds; ///< Rotation2D of r namespace wolf { -/** \brief Enumeration of all possible frames - * - * You may add items to this list as needed. Be concise with names, and document your entries. +/** \brief Enumeration of frame types: key-frame or non-key-frame */ typedef enum { NON_KEY_FRAME = 0, ///< regular frame. It does play at optimizations but it will be discarded from the window once a newer frame arrives. KEY_FRAME = 1 ///< key frame. It will stay in the frames window and play at optimizations. -} FrameKeyType; +} FrameType; /** \brief Enumeration of all possible frames *