KeyFrame.h

#pragma once
#include "DLLInfo.h"
#include <NDEVR/RWLock.h>
#include "OrbSLAM/Headers/MapPoint.h"
#include "OrbSLAM/Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "OrbSLAM/Headers/ORBVocabulary.h"
#include "OrbSLAM/Headers/ORBextractor.h"
#include "OrbSLAM/Headers/Frame.h"
#include <NDEVR/IntegratedRotation.h>
#include <NDEVR/GlobalPosition.h>
#include "GraphOptimization/Headers/GeometricCamera.h"
#include "Base/Headers/Buffer.hpp"
#include "Base/Headers/Set.h"
#include <mutex>
namespace NDEVR
{
    class Map;
    class MapPoint;
    class Frame;
    class KeyFrameDatabase;
    class InfoPipe;
    class GeometricCamera;

    class KeyFrame : public Frame
    {
    public:
        KeyFrame(Frame& F, Map* pMap, KeyFrameDatabase* pKFDB);
        void setPose(const Sophus::SE3<g_type>& Tcw) final override;
        void setLinearVelocity(const Eigen::Vector3<g_type>& Vw_) final override;
        bool doNotReduce() const { return m_do_not_reduce; }
        void setDoNotReduce(bool do_not_reduce) const { m_do_not_reduce = do_not_reduce; }
        Sophus::SE3<g_type> pose() const override;
        Map* map() const
        {
            return mpMap;
        }
        Sophus::SE3<g_type> inversePose() const override
        {
            std::unique_lock<std::mutex> lock(m_mutex_pose);
            return mTwc;
        }
        Sophus::SE3<g_type> poseInverse() const
        {
            std::unique_lock<std::mutex> lock(m_mutex_pose);
            if (!isBad())
                return mTwc;
            //computer for bad
            const KeyFrame* pKF = this;
            Sophus::SE3<g_type> Trw;
            // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
            while (pKF && pKF->isBad())
            {
                Trw = Trw * pKF->mTcp;
                pKF = pKF->parent();
            }
            if(pKF)
                Trw = Trw * pKF->mTcw;
            auto lit = mTcw * mpReferenceKF->mTwc;
            Sophus::SE3<g_type> Tcw = lit * Trw;
            return Tcw.inverse();
        }
        std::mutex& poseMutex() const { return m_mutex_pose; }
        Eigen::Vector3<g_type> cameraCenter() const
        {
            std::unique_lock<std::mutex> lock(m_mutex_pose);
            return mTwc.translation();
        }
 
        Eigen::Vector3<g_type> GetImuPosition() const final override;
        Eigen::Matrix3<g_type> GetImuRotation() const final override;
        Sophus::SE3<g_type> imuPose() const final override;
        Eigen::Matrix3<g_type> GetRotation() const final override
        {
            std::unique_lock<std::mutex> lock(m_mutex_pose);
            return mTcw.rotationMatrix();
        }
        Eigen::Vector3<g_type> GetTranslation() const final override
        {
            std::unique_lock<std::mutex> lock(m_mutex_pose);
            return mTcw.translation();
        }
        Eigen::Vector3<g_type> linearVelocity() const final override;
        bool isVelocitySet() const;
        void addConnection(KeyFrame* pKF, uint04 weight);
        void EraseConnection(KeyFrame* pKF);

        void updateConnections();
        void updateBestCovisibles();
        Set<KeyFrame*> GetConnectedKeyFrames();
        void bestCovisibilityKeyFrames(Buffer<KeyFrame*>& frames, uint04 N = Constant<uint04>::Max) const;
        void covisiblesByWeight(uint04 w, Buffer<KeyFrame*>& frames) const;
        uint04 weight(KeyFrame* pKF) const;

        void AddChild(KeyFrame* pKF);
        void EraseChild(KeyFrame* pKF);
        void ChangeParent(KeyFrame* pKF);
        Set<KeyFrame*> GetChilds();
        KeyFrame* parent() const
        {
            return mpParent;
        }

        bool hasChild(KeyFrame* pKF);
        void SetFirstConnection(bool bFirst);
        bool isFirstConnection() const
        {
            return mbFirstConnection;
        }
        void AddLoopEdge(KeyFrame* pKF);
        Set<KeyFrame*> GetLoopEdges();
        void eraseMapPoint(MapPoint* pMP);

        void AddMergeEdge(KeyFrame* pKF);
        Set<KeyFrame*> GetMergeEdges();
        bool IsInImage(const float& x, const float& y) const;

        void SetNotErase();
        void SetErase();
        void processGlobalPositionMeas();
        Buffer<const GlobalPosition*> getGlobalPositionMeas() const;
        void SetBadFlag();
        bool isBad() const
        {
            return mbBad;
        }

        float ComputeSceneMedianDepth(const int q);
 
        static bool weightComp(const std::pair<uint04, KeyFrame*>& a, const std::pair<uint04, KeyFrame*>& b)
        {
            return a.first > b.first;
        }
 
        static bool lId(KeyFrame* pKF1, KeyFrame* pKF2) { return pKF1->id < pKF2->id; }
        Map* GetMap();
        void UpdateMap(Map* pMap);
        void addGlobalPositionMeas(const GlobalPosition* meas);
        void SetNewBias(const IMU::Bias& b);
        Eigen::Vector3f GetGyroBias();
        Eigen::Vector3f GetAccBias();
        IMU::Bias GetImuBias();
        void SetORBVocabulary(ORBVocabulary* pORBVoc);
        void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
 
        bool bImu = false;  
 
    public:
 
        uint04 mnFuseTargetForKF = 0U;  
 
        uint04 mnBALocalForKF = 0U;     
        uint04 mnBAFixedForKF = 0U;     
        struct KeyFrameScore
        {
            uint04 query_id = Constant<uint04>::Invalid;    
            uint04 word_count = 0;                          
            fltp04 score = 0.0f;                            
        };

        enum ScoreEnum
        {
              e_merge               
            , e_loop                
            , e_relocation          
            , e_place_recognition   
        };
        KeyFrameScore scores[4];    
        // Variables used by loop closing
        Sophus::SE3<g_type> mTcwGBA;
        Sophus::SE3<g_type> mTcwBefGBA;
        Eigen::Vector3<g_type> mVwbGBA;
        IMU::Bias mBiasGBA;
        uint04 mnBAGlobalForKF = 0U;
 
        // Variables used by merging
        Sophus::SE3<g_type> mTcwMerge;
        Sophus::SE3<g_type> mTcwBefMerge;
        Sophus::SE3<g_type> mTwcBefMerge;
        Eigen::Vector3<g_type> mVwbMerge;
        Eigen::Vector3<g_type> mVwbBefMerge;
        IMU::Bias mBiasMerge;
        uint04 mnBALocalForMerge = 0U;
 
        static volatile uint04 s_next_kf_id;    
 
 
        Sophus::SE3<g_type> mTcp;   
 
 
        KeyFrame* mPrevKF = nullptr;    
        KeyFrame* mNextKF = nullptr;    
    protected:
        bool m_has_global_position_measurement = false;
        Buffer<const GlobalPosition*> m_global_position_meas;
        Buffer<const GlobalPosition*> m_global_position_meas_before_prop;
        // BoW
        KeyFrameDatabase* mpKeyFrameDB = nullptr;
 
        // Grid over the image to speed up feature matching
 
        Dictionary<KeyFrame*, uint04> m_connected_keyframe_weights;
        Buffer<std::pair<uint04, KeyFrame*>> m_ordered_connected_key_frames;
 
        // Spanning Tree and Loop Edges
        bool mbFirstConnection = false;
        KeyFrame* mpParent = nullptr;
        Set<KeyFrame*> m_child_frames;
        Set<KeyFrame*> mspLoopEdges;
        Set<KeyFrame*> mspMergeEdges;
 
        std::map<double, Sophus::SE3f> frameRelativePoses;
        std::map<double, Sophus::SE3f> posesRelativeToKF;
        // Bad flags
        bool mbNotErase = false;
        bool mbToBeErased = false;
        bool mbBad = false;
        mutable bool m_do_not_reduce = false;
        Map* mpMap = nullptr;
 
 
        // Mutex
        mutable std::mutex m_mutex_gps;
        mutable std::mutex m_mutex_pose; // for pose, velocity and biases
        mutable std::mutex mMutexConnections;
        mutable std::mutex mMutexMap;
 
    public:
        uint04 frame_id = Constant<uint04>::Invalid;        
        uint04 keyframe_id = Constant<uint04>::Invalid; 

        Sophus::SE3f GetRelativePoseTrl();
        Sophus::SE3f GetRelativePoseTlr();

        Sophus::SE3<float> GetRightPose();
        Sophus::SE3<float> GetRightPoseInverse();

        Eigen::Vector3f GetRightCameraCenter();
        Eigen::Matrix<float, 3, 3> GetRightRotation();
        Eigen::Vector3f GetRightTranslation();
 
    };
 
}