Frame.h

#pragma once
#include "DLLInfo.h"
#include "../../GraphOptimization/Headers/DLLInfo.h"
#include "OrbSLAM/Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "OrbSLAM/Thirdparty/Sophus/sophus/se3.hpp"
#include "OrbSLAM/Headers/ORBVocabulary.h"
#include <NDEVR/IntegratedRotation.h>
#include <NDEVR/RWLock.h>
#include "Base/Headers/Buffer.hpp"
#include <mutex>
#include <opencv2/opencv.hpp>
#include <functional>
 
namespace NDEVR
{
    constexpr uint04 FRAME_GRID_ROWS = 48;  
    constexpr uint04 FRAME_GRID_COLS = 64;  
 
    class MapPoint;
    class KeyFrame;
    class InfoPipe;
    class ConstraintPoseImu;
    class GeometricCamera;
    class ORBextractor;
    class BinaryFile;
    struct FrameView
    {
    public:
        FrameView() {};
        FrameView(ORBextractor* extractor, GeometricCamera* camera);
        FrameView(const FrameView& other);
        void init(ORBextractor* extractor, GeometricCamera* camera);
        FrameView& operator=(const FrameView& other);
        Dictionary<Vector<2, uint04>, Buffer<uint04>> grid; 
        PrimitiveAlignedBuffer<cv::KeyPoint, 32> keys;          
        Buffer<uint04> match_points;                            
        cv::Mat descriptors;                                    
        ORBextractor* extractor = nullptr;                      
        GeometricCamera* camera = nullptr;                      
        uint04 keypoint_count = 0U;                             
        uint04 mono_count = 0U;                                 
    };

    struct FrameInfo
    {
        float mfGridElementWidthInv;    
        float mfGridElementHeightInv;   
        float fx;                       
        float fy;                       
        float cx;                       
        float cy;                       
        float invfx;                    
        float invfy;                    
        float mbf = 0.0f;               
        float mb = 0.0f;                
        const cv::Mat K;                
        const cv::Mat mDistCoef;        
        fltp04 thDepth;                 
        GeometricCamera* pCamera = nullptr; 
        FrameInfo(const Vector<2, uint04>& size, const cv::Mat& mDistCoef, const cv::Mat& mK, GeometricCamera* camera, fltp04 thDepth, fltp04 mbf);
 
        Bounds<2, fltp04> image_bounds; 
        Vector<2, uint04> image_size;   
        LogPtr log = LogPtr();          
    };

    struct IMUData
    {
        IMU::Calib mImuCalib;   
        IMU::Bias mImuBias;     
    };

    class Frame
    {
    public:
        Frame() = default;

        Frame(const Frame &frame);
        Frame(Frame&& frame) noexcept;
        Frame& operator=(const Frame& frame) noexcept;
        Frame& operator=(Frame&& frame) noexcept;

        Frame(FrameInfo* frame_info, const cv::Mat& imLeft, const cv::Mat& imRight, fltp08 timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, const IMU::Calib &ImuCalib = IMU::Calib());
        Frame(FrameInfo* frame_info, const cv::Mat& imLeft, const cv::Mat& imRight, fltp08 timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, GeometricCamera* pCamera2, Sophus::SE3<g_type>& Tlr, const IMU::Calib& ImuCalib = IMU::Calib());
        Frame(FrameInfo* frame_info, const cv::Mat &imGray, const cv::Mat& imDepth, fltp08 timeStamp, ORBextractor* extractor, ORBVocabulary* voc, const IMU::Calib &ImuCalib = IMU::Calib());
        Frame(FrameInfo* frame_info, const cv::Mat &imGray, fltp08 timeStamp, ORBextractor* extractor,ORBVocabulary* voc, const IMU::Calib &ImuCalib = IMU::Calib());
        ~Frame();

        void init(FrameInfo* info, const cv::Mat& imGray, const cv::Mat& imDepth, fltp08 timeStamp, ORBextractor* extractor
            , ORBVocabulary* voc, const IMU::Calib& ImuCalib = IMU::Calib());
        void extractORB(uint01 flag, const cv::Mat &im, uint04 x0, uint04 x1);

        const cv::KeyPoint& keypoint(uint04 i) const;
        const cv::KeyPoint& keypoint(uint04 i, bool is_right) const;
        void eraseMapPointMatch(uint04);
        void setMapPointMatch(uint04, MapPoint* pMP);
        void computeBoW();
        void clearBoW();

        virtual void setPose(const Sophus::SE3<g_type>& Tcw);

        virtual void setLinearVelocity(const Eigen::Vector3<g_type>& Vw);

        void setImuPoseVelocity(const Eigen::Matrix3<g_type>& Rwb, const Eigen::Vector3<g_type>& twb, const Eigen::Vector3<g_type>& Vwb);

        virtual Eigen::Matrix<g_type,3,1> GetImuPosition() const;
        virtual Eigen::Matrix<g_type,3,3> GetImuRotation() const;
        virtual Sophus::SE3<g_type> imuPose() const;
        uint04 observationCount(uint04 idx) const;
        uint04 mapPointCount() const { return m_map_points.size(); }
        MapPoint* mapPoint(uint04 idx) const { return m_map_points[idx]; }
        Sophus::SE3<g_type> GetRelativePoseTrl() const;
        Sophus::SE3<g_type> GetRelativePoseTlr() const;
        Eigen::Matrix3<g_type> GetRelativePoseTlr_rotation() const;
        Eigen::Vector3<g_type> GetRelativePoseTlr_translation() const;

        virtual Eigen::Matrix3<g_type> GetRotation() const;
        virtual Eigen::Vector3<g_type> GetTranslation() const;
        virtual Eigen::Vector3<g_type> linearVelocity() const;
        void SetNewBias(const IMU::Bias &b);
        bool hasCamera(uint01 index) const;
        bool isMono() const;
        fltp04 trackDepth(const Eigen::Vector3<g_type>& global_position) const;
        Eigen::Vector3<g_type> inRefCoordinates(Eigen::Vector3<g_type> pCw) const;

        void getFeaturesInArea(Buffer<uint04>& indices, fltp04 x, fltp04 y, fltp04 r, uint04 min_level = 0U, uint04 max_level = Constant<uint04>::Max, const bool bRight = false) const;

        void computeStereoMatches();
        const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& undistortedPoints() const { return m_undistorted_points; }
        const cv::KeyPoint& undistortedPoint(uint04 idx) const { return m_undistorted_points[idx]; }
        uint04 undistortedPointCount() const { return m_undistorted_points.size(); }
        void computeStereoFromRGBD(const cv::Mat &imDepth);

        Eigen::Vector3<g_type> unproject(uint04 i);
 

        bool imuIsPreintegrated() const;
        void setIntegrated();
        bool isSet() const { return mbIsSet; }

        void updatePoseMatrices();

        inline const Eigen::Vector3<g_type>& cameraCenter() const {
            return mTwc.translation();
        }

        const Eigen::Matrix<g_type, 3, 1>& tcw() const {
            return mTcw.translation();
        }

        inline Eigen::Matrix3<g_type> cameraRotation() const
        {
            return mTwc.rotationMatrix();
        }

        bool isKeyFrame() { return m_is_keyframe; }
        virtual inline Sophus::SE3<g_type> pose() const {
            return mTcw;
        }

        virtual inline Sophus::SE3<g_type> inversePose() const {
            return mTwc;
        }

        inline bool HasPose() const {
            return mbHasPose;
        }

        inline bool HasVelocity() const {
            return mbHasVelocity;
        }

        RWLock& mapPointsLockPtr() const { return m_feature_lock; };
        Eigen::Vector3<g_type> stereo3DPoint(uint04 idx) const;
        fltp04 depth(uint04 idx) const { return m_depth[idx]; }
        fltp04 right(uint04 idx) const { return m_right[idx]; }
        LogPtr log() { return m_frame_info->log; }

        void ComputeStereoFishEyeMatches();
        const Buffer<MapPoint*>& mapPoints() const { return m_map_points; }
        void setMapPoints(const Buffer<MapPoint*>& map_points) { m_map_points = map_points; }
        const Buffer<bool>& outliers() const { return m_outlier; }
        void setOutliers(const Buffer<bool>& map_points) { m_outlier = map_points; }
        bool isOutlier(uint04 idx) const { return m_outlier[idx]; }
        void setOutlier(uint04 idx, bool outlier);
        const Buffer<MapPoint*>& mapPointMatches();
        uint04 trackedMapPoints(uint04 minObs) const;
        MapPoint* mapPoint(uint04 idx);
        void clearMapPoints();
        Vector<2, uint04> discardOutliers();
        const Buffer<float>& scaleFactors() const;
        const Buffer<float>& invScaleFactors() const;
        const Buffer<float>& levelSigma2() const;
        const Buffer<float>& invLevelSigma2() const;
        uint04 levelCount() const;
        fltp04 scaleFactor() const;
        fltp04 nearFarThreshold() const { return m_frame_info->thDepth; }
        const FrameInfo& info() const { return *m_frame_info; }
        fltp08 timestamp() const { return m_timestamp; }
        const BowVector& bow() const { return m_bow; }
        const Dictionary<uint04, Buffer<uint04>>& featureIndices() const { return m_features_indices; }
    private:
        void undistortKeyPoints();
        void AssignFeaturesToGrid();
    public:
        Eigen::Vector3<g_type> mOwb;            
        IMUData imu_data;                       
        Sophus::SE3<g_type> sensor_pose;        
    protected:
        Sophus::SE3<g_type> mTcw;   
        Sophus::SE3<g_type> mTwc;   
        Sophus::SE3<g_type> mTlr;   
        Sophus::SE3<g_type> mTrl;   
 
        PrimitiveAlignedBuffer<cv::KeyPoint, 32> m_undistorted_points;  
        Buffer<Vector<2, fltp04>> m_original_points;                    
        Buffer<Eigen::Vector3<g_type>> m_stereo_3D_points;              
 
        Buffer<MapPoint*> m_map_points; 
        Buffer<fltp04> m_right;         
        Buffer<fltp04> m_depth;         
        Buffer<bool> m_outlier;         
        BowVector m_bow;                
    public:
        Dictionary<uint04, Buffer<uint04>> m_features_indices;  
 
        ORBVocabulary* mpORBvocabulary = nullptr;           
        ConstraintPoseImu* mpcpi = nullptr;                 
        KeyFrame* mpReferenceKF = nullptr;                  
        IMU::Preintegrated* mpImuPreintegrated = nullptr;   
        KeyFrame* mpLastKeyFrame = nullptr;                 
 
        FrameView* m_view[2] = { nullptr, nullptr };        
 
        IMU::Preintegrated* mpImuPreintegratedFrame = nullptr;  
    protected:
        mutable RWLock m_feature_lock;                                      
        Eigen::Vector3<g_type> m_linear_velocity = Eigen::Vector3<g_type>(0,0,0);   
    public:
        fltp08 m_timestamp = 0.0;                   
        uint04 id = Constant<uint04>::Invalid;      
    public:
        ORBextractor* m_extractor;                  
        FrameInfo* m_frame_info = nullptr;          
        uint04 N = 0;                               
        bool mbHasPose = false;                     
        bool mbHasVelocity = false;                 
        bool mbIsSet = false;                       
        bool mbImuPreintegrated = false;            
        bool m_is_keyframe = false;                 
    public:
        ORBSLAM_API static void Set3DCallback(std::function<Vector<3, fltp04>(Frame* frame, Vector<3, fltp04>)> callback);
        static std::function<Vector<3, fltp04>(Frame* frame, Vector<3, fltp04>)> s_point_to_3d_callback;    
        static cv::BFMatcher BFmatcher;         
        static volatile uint04 s_next_id;       
    };
 
}