Tracking.h

#pragma once
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "OrbTrackingInfo.h"
#include "OrbSLAM/Headers/Optimizer.h"
#include "OrbSLAM/Headers/Frame.h"
#include "OrbSLAM/Headers/SLAMParameters.h"
#include "OrbSLAM/Headers/ORBmatcher.h"
#include "Base/Headers/Vector.hpp"
#include <NDEVR/GlobalPosition.h>
#include <mutex>
 
namespace NDEVR
{
    struct FrameInfo;
    class Atlas;
    class LocalMapping;
    class LoopClosing;
    class OrbSLAM;
    class Settings;
    class MapDrawer;
    class SLAMParameters;
    class KeyFrameDatabase;
    class Map;
    class InfoPipe;
    class Tracking
    {
    public:
        enum eTrackingState
        {
            SYSTEM_NOT_READY = -1,  
            NO_IMAGES_YET = 0,      
            NOT_INITIALIZED = 1,    
            OK = 2,                 
            RECENTLY_LOST = 3,      
            LOST = 4,               
            OK_KLT = 5              
        };

        enum GlobalFrameAlignmentState
        {
            NO_MEAS_YET,            
            FIRST_GLOBAL_MEAS_SET,  
            ALIGNING,               
            ALIGNED                 
        };
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        Tracking(OrbSLAM* pSys, ORBVocabulary* pVoc, Atlas* pAtlas
            , KeyFrameDatabase* pKFDB, const SLAMParameters& params, LogPtr log);

        ~Tracking();
        void setParams(const SLAMParameters& params);
        bool parseCamParamFile(const SLAMParameters& params);
        bool parseORBParamFile(const SLAMParameters& params);
        bool parseIMUParamFile(const SLAMParameters& params);

        Sophus::SE3f GrabImageStereo(OrbTrackingInfo& info);
        Sophus::SE3f GrabImageRGBD(OrbTrackingInfo& info);
        Sophus::SE3f GrabImageMonocular(OrbTrackingInfo& info);
        void appendImuData(Buffer<IMU::Point> imu_measurements, fltp08 time_offset);
        void appendGPSData(const Buffer<GlobalPosition*> gps_measurements, fltp08 time_offset);

        void SetLocalMapper(LocalMapping* pLocalMapper);
        void SetLoopClosing(LoopClosing* pLoopClosing);
        eTrackingState state() const { return m_state; };
        void setState(eTrackingState state) { m_state = state; };

        void InformOnlyTracking(bool flag);

        void UpdateFrameIMU(fltp04 scale, const IMU::Bias& b, KeyFrame* pCurrentKeyFrame);
        KeyFrame* GetLastKeyFrame() { return m_last_keyframe; }
        const Sensor& sensor() const { return m_sensor; }
        void CreateMapInAtlas();
        int GetMatchesInliers() { return mnMatchesInliers; };
        bool ensureValidData(const cv::Mat& imRectLeft) const;
 
    public:
 
        eTrackingState m_state;
 
        // Input sensor
        Sensor m_sensor;
 
        // Current Frame
        Frame m_pending_frame;
        Frame m_current_frame;
        Frame m_last_frame;
        Frame m_mono_initial_frame;
 
        // Initialization Variables (Monocular)
        Buffer<uint04> m_mono_initial_matches;
        Buffer<cv::Point2f> m_mono_prev_matched;
        Buffer<cv::Point3f> mvIniP3D;
 
        // Lists used to recover the full camera trajectory at the end of the execution.
        // Basically we store the reference keyframe for each frame and its relative transformation
        Buffer<Sophus::SE3f> mlRelativeFramePoses;
        Buffer<KeyFrame*> mlpReferences;
        Buffer<bool> mlbLost;
 
        // frames with estimated pose
        bool mbStep = false;
 
        // True if local mapping is deactivated and we are performing only localization
        bool mbOnlyTracking = false;
 
        void reset(bool bLocMap = false);
        void ResetActiveMap(bool bLocMap = false);
 
        double t0 = 0.0; // time-stamp of first read frame
        double t0IMU = 0.0; // time-stamp of IMU initialization
        bool mFastInit = false;
 
 
        Buffer<MapPoint*> GetLocalMapMPS();
    protected:
 
        bool hugechangeornot(const cv::Mat& depth1, const cv::Mat& depth2);
        bool TrackWithMotionSensor(OrbTrackingInfo& info);
        bool TrackLocalMapwithSensor(OrbTrackingInfo& info);
        void Track_withsensor(OrbTrackingInfo& info);
        void mergeImg(cv::Mat& dst, cv::Mat& src1, cv::Mat& src2);
 
protected:
    //addby ruyu
    void RGBDInitialiuliu();
    void updateStateIMU();
 
        // Main tracking function. It is independent of the input sensor.
        void Track(OrbTrackingInfo& info);
        //Tracks and creates new keypoints
        bool trackWithLocalMapping(Map* map, OrbTrackingInfo& info);
        //Tracks using existing keypoints
        bool trackWithoutLocalMapping(Map* map, OrbTrackingInfo& info);
        // Map initialization for stereo and RGB-t_dim
        void StereoInitialization();
 
        // Map initialization for monocular
        void MonocularInitialization();
        //void CreateNewMapPoints();
        void CreateInitialMapMonocular();
 
        void CheckReplacedInLastFrame();
        bool TrackReferenceKeyFrame(OrbTrackingInfo& info);
        void UpdateLastFrame();
        bool TrackWithMotionModel(OrbTrackingInfo& info);
        bool PredictStateIMU();
 
        bool relocalization(OrbTrackingInfo& info);
 
        void UpdateLocalMap();
        void UpdateLocalPoints();
        void UpdateLocalKeyFrames();
 
        bool TrackLocalMap(OrbTrackingInfo& info);
        void SearchLocalPoints();
 
        bool needsNewKeyFrame();
        void createNewKeyFrame();
 
        // Perform preintegration from last frame
        void preIntegrateIMU();
 
        // reset IMU biases and compute frame velocity
        void ResetFrameIMU();
    protected:
        Optimizer m_optimizer;
        fltp08 m_last_sensor_pose_time = Constant<fltp08>::Invalid;
        fltp08 m_sensor_pose_time = Constant<fltp08>::Invalid;
        Sophus::SE3<g_type> m_sensor_pose;
        Sophus::SE3<g_type> m_last_sensor_pose;
        ORBmatcher m_matcher;
        bool mbMapUpdated = false;
 
        // Imu preintegration from last frame
        IMU::Preintegrated* mpImuPreintegratedFromLastKF;
 
        // Queue of IMU measurements between frames
        Buffer<IMU::Point> m_imu_queue;
        std::mutex m_mutex_imu_queue;
        std::mutex m_mutex_gps_queue;
        std::mutex m_mutex_current_image;
        // Imu calibration parameters
        IMU::Calib* m_imu_calib;
 
        // Last Bias Estimation (at keyframe creation)
        IMU::Bias mLastBias;
 
        // In case of performing only localization, this flag is true when there are no matches to
        // points in the map. Still tracking will continue if there are enough matches with temporal points.
        // In that case we are doing visual odometry. The system will try to do relocalization to recover
        // "zero-drift" localization to the map.
        bool mbVO = false;
 
        //Other Thread Pointers
        LocalMapping* mpLocalMapper;
        LoopClosing* mpLoopClosing;
 
        //ORB
        Vector<2, ORBextractor*> m_orb_extractor;
 
        //BoW
        ORBVocabulary* mpORBVocabulary;
        KeyFrameDatabase* mpKeyFrameDB;
 
        // Initialization (only for monocular)
        bool m_mono_ready_to_initialize = false;
 
        //Local Map
        KeyFrame* m_reference_keyframe;
        Buffer<KeyFrame*> m_local_key_frames;
        Buffer<MapPoint*> mvpLocalMapPoints;
        Buffer<MapPoint*> m_map_point_matches;
        Buffer<MapPointProjection> m_local_projected_points;
        // System
        OrbSLAM* mpSystem = nullptr;
 
        //Atlas
        Atlas* m_atlas = nullptr;
 
        //Calibration matrix
        cv::Mat mK;
        cv::Mat mDistCoef;
        fltp04 mbf = 0.0f;
 
        fltp08 mImuPer = 0.0f;
        //New KeyFrame rules (according to fps)
        uint04 mMinFrames = 0;
        uint04 mMaxFrames = 0;
 
        uint04 mnFramesToResetIMU = 0;
 
        // Threshold close/far points
        // Points seen as close by the stereo/RGBD sensor are considered reliable
        // and inserted from just one frame. Far points requiere a match in two keyframes.
        fltp04 near_far_threshold = 0.0f;
 
        // Depth map scale factor (converts raw depth values to meters)
        fltp04 m_depth_map_factor = 1.0f;
 
        //Current matches in frame
        uint04 mnMatchesInliers = 0;
 
        //Last Frame, KeyFrame and Relocalisation Info
        KeyFrame* m_last_keyframe = nullptr;
        uint04 m_frame_id_of_last_kf = 0U;
        uint04 mnLastRelocFrameId = 0U;
        fltp08 m_time_stamp_lost = 0.0;
        fltp08 time_recently_lost = 0.0;
 
        uint04 mn_first_frame_ID = 0U;
        uint04 mnInitialFrameId = 0U;
 
        bool mbCreatedMap = false;
 
        //Motion Model
        bool mbVelocity = false;
        Sophus::SE3<g_type>::Tangent mVelocity;
 
        Buffer<MapPoint*> mlpTemporalPoints;
 
        Buffer<const GlobalPosition*> dataToAlign;
        const GlobalPosition* globalPositionOrigin;
        bool m_set_global_pos_origin = false;
        GPSCalib mpGlobalMeasCalib;
        GlobalFrameAlignmentState m_global_frame_alignment_state;
        //int nMapChangeIndex;
        LogPtr m_log;
        FrameInfo* m_frame_info = nullptr;
        GeometricCamera* mpCamera = nullptr;
        GeometricCamera* mpCamera2 = nullptr;
 
        uint04 initID = 0;
        uint04 m_last_monocular_ID = 0;
 
        Sophus::SE3f m_stereo_tlr;
    protected:
        bool mInsertKFsLost = false;
    };
 
}