frame.h

Go to the documentation of this file.

#ifndef FRAME_H
#define FRAME_H
 
#include "isaeslam/data/features/AFeature2D.h"
#include "isaeslam/data/landmarks/ALandmark.h"
#include "isaeslam/typedefs.h"
 
namespace isae {
 
class ImageSensor;
class ASensor;
class IMU;
 
class Frame : public std::enable_shared_from_this<Frame> {
  public:
    Frame() = default;
 
    static int _frame_count; 
    int _id;                 
 
    void init(const std::vector<std::shared_ptr<ImageSensor>> &sensors, unsigned long long timestamp);
 
    void init(std::shared_ptr<IMU> &imu, unsigned long long timestamp);
 
    void init(const std::vector<std::shared_ptr<ASensor>> &sensors, unsigned long long timestamp);
 
    std::vector<std::shared_ptr<ImageSensor>> getSensors() const { return _sensors; }
    std::shared_ptr<IMU> getIMU() const { return _imu; }
    void setIMU(std::shared_ptr<IMU> &imu, Eigen::Affine3d T_s_f);
 
    void cleanSensors() {
        _imu = nullptr;
        _sensors.clear();
    }
 
    void setWorld2FrameTransform(Eigen::Affine3d T_f_w) {
        std::lock_guard<std::mutex> lock(_frame_mtx);
        _T_f_w = T_f_w;
    }
 
    Eigen::Affine3d getWorld2FrameTransform() const {
        std::lock_guard<std::mutex> lock(_frame_mtx);
        return _T_f_w;
    }
 
    Eigen::Affine3d getFrame2WorldTransform() const {
        std::lock_guard<std::mutex> lock(_frame_mtx);
        return _T_f_w.inverse();
    }
 
    void addLandmark(std::shared_ptr<ALandmark> ldmk) { _landmarks[ldmk->_label].push_back(ldmk); }
    void addLandmarks(isae::typed_vec_landmarks ldmks) {
        for (auto typed_ldmks : ldmks) {
            for (auto l : typed_ldmks.second)
                _landmarks[typed_ldmks.first].push_back(l);
        }
    }
    typed_vec_landmarks getLandmarks() const { return _landmarks; }
 
    uint getInMapLandmarksNumber() const;
    uint getLandmarksNumber() const {
        uint N = 0;
        for (const auto &typeldmk : _landmarks)
            N += typeldmk.second.size();
        return N;
    }
    void cleanLandmarks();
 
    unsigned long long getTimestamp() const { return _timestamp; }
 
    void setKeyFrame() { _is_kf = true; }
    void unsetKeyFrame() { _is_kf = false; }
 
    bool isKeyFrame() const { return _is_kf; }
 
    // Handles a prior on T_f_w
    bool hasPrior() const { return _has_prior; }
    void setPrior(Eigen::Affine3d T_prior, Vector6d inf_prior) {
        _has_prior   = true;
        _T_prior     = T_prior;
        _inf_T_prior = inf_prior;
    }
    Eigen::Affine3d getPrior() const { return _T_prior; }
    Vector6d getInfPrior() const { return _inf_T_prior; }
 
    // Handles relative pose covariance
    // The convention is wrt the previous frame
    void setdTCov(Eigen::MatrixXd dT_cov) { _dT_cov = dT_cov; }
    Eigen::MatrixXd getdTCov() const { return _dT_cov; }
 
  private:
    Eigen::Affine3d _T_f_w = Eigen::Affine3d::Identity(); 
    typed_vec_landmarks _landmarks;                       
 
    unsigned long long _timestamp;                      
    std::vector<std::shared_ptr<ImageSensor>> _sensors; 
    std::shared_ptr<IMU> _imu;                          
 
    bool _is_kf     = false; 
    bool _has_prior = false; 
 
    Eigen::Affine3d _T_prior = Eigen::Affine3d::Identity(); 
    Vector6d _inf_T_prior    = Vector6d::Zero();            
    Eigen::MatrixXd _dT_cov =
        Eigen::MatrixXd::Identity(6, 6); 
 
    mutable std::mutex _frame_mtx;
};
 
} // namespace isae
 
#endif // FRAME_H