orbslam/include/Cache.h

//
// Created by lifu on 17-1-31.
//

#ifndef ORB_SLAM2_CACHE_H
#define ORB_SLAM2_CACHE_H

#include "Frame.h"
#include "KeyFrame.h"
#include "MapPoint.h"
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
#include "Map.h"
#include "SerializeObject.h"

/*
 * Cache class is the data layer which organize all the persistent data, such as keyframe, mappoint, all kinds of map.
 * Cache class provide the all metheds which access the data layer such as basic read and store methods
 *
 */

namespace ORB_SLAM2 {

    class Map;

    // class ORBVocabulary;
    class KeyFrameDatabase;

    class KeyFrame;

    class Cache {

    public:
        //construct function
        Cache( const int maxKFs, const int maxMPs );

        //load the ORBVocabulary from the file
        bool loadORBVocabulary(const string &strVocFile);

        void createKeyFrameDatabase();

        void createMap();

        void run();

        // operate about keyframes

        // add one keyFrame to map
        void AddKeyFrameToMap(KeyFrame *pKF);

        //erase one particular keyFrame in map
        void EraseKeyFrameFromMap(KeyFrame *pKF);

        //get the number of keyframes in map
        const int getKeyFramesInMap();

        //get all keyframes in the map
        vector<KeyFrame *> getAllKeyFramesInMap();

        long unsigned int GetMaxKFidInMap();

        KeyFrame *getKeyFrameById(long unsigned int pId);

        MapPoint *getMapPointById(long unsigned int pId);

        KeyFrame *getKeyFrameFromServer(long unsigned int pId);

        MapPoint *getMapPointFromServer(long unsigned int pId);

        // operate about mappoint
        //add one MapPoint to map
        void AddMapPointToMap(MapPoint *pMP);

        // erase the paticular mapPoint from the map
        void EraseMapPointFromMap(MapPoint *pMP);

        // get all MapPoints in the map
        std::vector<MapPoint *> GetAllMapPointsFromMap();

        //set ReferenceMapPoint in the map
        void SetReferenceMapPointsToMap(std::vector<MapPoint *> pLocalMapPoints);

        //push back keyFrame to mvpKeyFrameOrigin in Map
        void SetmvpKeyFrameOrigins(KeyFrame *pKF);

        //get mvpKeyFrameOrigins from map
        vector<KeyFrame *> getmvpKeyFrameOrigins();

        //Clear map
        void clearMap();

        //operate about keyframeDatabase
        //erase one keyfrmae from keyFramedatabase
        void EraseKeyFrameFromDB(KeyFrame *pKF);

        //KeyFrameDatabase detectRelocalizationCandidates keyframes
        vector<KeyFrame *> DetectRelocalizationCandidatesFromDB(Frame *F);

        //KeyFrameDatabase detect the loopCandi keyframes
        vector<KeyFrame *> DetectLoopCandidatesFromDB(KeyFrame *pKF, float minScore);

        //add one keyFrame to keyFrameDatabase
        void addKeyFrametoDB(KeyFrame *pKF);

        //clear KeyFrameDatabase
        void clearKeyframeDatabase();

        //get and set functions
        //get ORBVocabulary point
        ORBVocabulary *getMpVocabulary() const {
            return mpVocabulary;
        }

        //get the keyFrameDatabase point
        KeyFrameDatabase *getMpKeyFrameDatabase() const {
            return mpKeyFrameDatabase;
        }

        //get the map point
        Map *getMpMap() const {
            return mpMap;
        }

        std::map<long unsigned int, MapPoint *> getlMPToMPmap();

        void SaveMap(const string &filename);

        void LoadMap(const string &filename);

        void RequestFinish();

        bool isFinished();

    private:
        /*  cache organize function   */

        bool CheckNewKeyFrames();
        void ProcessNewKeyFrame();
        bool CheckFullCache();
        void ProcessFullCache();
        bool CheckFullMapPoint();
        void ProcessFullMapPoint();

        bool checkInCache( LightKeyFrame tLKF );
        bool checkMpNeedTrans( MapPoint * pMP);
        bool Stop();
        bool isStopped();
        bool CheckFinish();
        void SetFinish();

        long unsigned int getTime();

        //when cache full, need decide which keyfrmaes should to be transfered to the server
        vector<KeyFrame *> getNeedReplacedKeyFrame( const int tn);

        set<MapPoint *> getNeedReplacedMapPoint();

        //transfer the keyframes to server and
        //get the keyframe from server using ros service
        void transKeyFrameToServer( std::vector<KeyFrame*> vKF);
        void tramsKeyFrameFromServer( std::set<LightKeyFrame> vLKF);
        void transMapPointToServer( std::vector<MapPoint*> vMP);
        void transMapPointFromServer( std::set<LightMapPoint> vLMP);

        //replace strategy
        vector<KeyFrame *> getRLUKeyFrames( const int tn);

    private:

        // ORB vocabulary used for place recognition and feature matching.
        ORBVocabulary *mpVocabulary;

        // KeyFrame database for place recognition (relocalization and loop detection).
        KeyFrameDatabase *mpKeyFrameDatabase;

        // Map structure that stores the pointers to all KeyFrames and MapPoints.
        Map *mpMap;

        std::mutex mMutexCacheTime;
        long unsigned int cacheTime;
        std::mutex mMutexKFStayTime;
        std::map<long unsigned int, long unsigned int> KFStayTime;

        std::mutex mMutexlKFToKFmap;
        std::map<long unsigned int, KeyFrame *> lKFToKFmap;

        std::mutex mMutexMPToMPmap;
        std::map<long unsigned int, MapPoint *> lMPToMPmap;

        //cache orgnize data

        const int maxKFinCache;
        const int maxMPinCache;

        std::mutex mMutexNTPB;
        std::set<MapPoint *> needToPutBack;

        bool mbFinished;

        std::mutex mMutexNewKFs;
        vector<KeyFrame* > mlNewKeyFrames;

        std::mutex mMutexStop;
        bool mbStopRequested;
        bool mbNotStop;
        bool mbStopped;

        std::mutex mMutexFinish;
        bool mbFinishRequested;

        // for debug the function
        std::map<long unsigned int, KeyFrame*> KFDataBase;
        std::map<long unsigned int, MapPoint*> MPDataBase;


    };

} //namespace ORB_SLAM
#endif //ORB_SLAM2_CACHE_H