MapPoint.h

#pragma once
#include "DLLInfo.h"
#include "Base/Headers/Set.h"
#include "Base/Headers/Dictionary.h"
#include "GraphOptimization/Headers/GeometricCamera.h"
#include <Eigen/Core>
#include <NDEVR/RWLock.h>
#include <opencv2/core/core.hpp>
#include <mutex>
namespace NDEVR
{
 
    class KeyFrame;
    class Map;
    class Frame;

    class ORBSLAM_API MapPoint
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        MapPoint(const Eigen::Vector3<g_type>& Pos, KeyFrame* pRefKF, Map* pMap);
        MapPoint(const Eigen::Vector3<g_type>& Pos, Map* pMap, Frame* pFrame, uint04 idxF);
        void SetWorldPos(const Eigen::Vector3<g_type>& Pos);
        Eigen::Vector3<g_type> worldPos() const
        {
            //std::unique_lock<std::mutex> lock(mMutexPos);
            return m_world_pos;
        }
        Eigen::Vector3f GetNormal() const
        {
            //std::unique_lock<std::mutex> lock(mMutexPos);
            return mNormalVector;
        }
        void SetNormalVector(const Eigen::Vector3f& normal);
 
        KeyFrame* GetReferenceKeyFrame();
 
        const Dictionary<KeyFrame*, Vector<2, uint04>>& observationsRef();
        uint04 observationCount() const;
 
        void AddObservation(KeyFrame* pKF, uint04 idx);
        void eraseObservation(KeyFrame* pKF);
 
        Vector<2, uint04> GetIndexInKeyFrame(KeyFrame* pKF);
        bool IsInKeyFrame(KeyFrame* pKF);
 
        void setBadFlag();
        bool isBad();
 
        void Replace(MapPoint* pMP);
        MapPoint* GetReplaced();
 
        void increaseVisible(uint04 n = 1U);
        void increaseFound(uint04 n = 1U);
        float GetFoundRatio();
        inline int GetFound() {
            return mnFound;
        }
 
        void ComputeDistinctiveDescriptors();
        RWLock& featureLockPtr() { return m_feature_lock; };
        const cv::Mat& distinctDescriptor() const;
 
        void UpdateNormalAndDepth();
 
        float GetMinDistanceInvariance();
        float GetMaxDistanceInvariance();
        uint04 predictScale(const Frame& kF) const;
        uint04 predictScale(float currentDist, const Frame& kF) const;
 
        Map* GetMap();
        void UpdateMap(Map* pMap);
        uint04 id() const { return m_id; }
        void setLocalTrackedFrame(uint04 frame_idx) { m_last_frame_locally_tracked = frame_idx; };
        uint04 getLocalTrackedFrame() const { return m_last_frame_locally_tracked; };
    protected:
        static volatile uint04 s_id_increment;
    public:
        static std::mutex mGlobalMutex;
        
    protected:// Mutex
        // Keyframes observing the point and associated index in keyframe
        Dictionary<KeyFrame*, Vector<2, uint04>> m_observations;
        RWLock m_feature_lock;
        // Best descriptor to fast matching
        cv::Mat m_descriptor;
    public:
        uint04 mnFirstKFid = 0U;
 
        // Variables used by local mapping
        uint04 mnBALocalForKF = Constant<uint04>::Invalid;
 
        // Variables used by loop closing
        uint04 mnCorrectedByKF = Constant<uint04>::Invalid;
        uint04 mnCorrectedReference = Constant<uint04>::Invalid;
        Eigen::Vector3f mPosGBA;
        uint04 mnBAGlobalForKF = Constant<uint04>::Invalid;
        uint04 mnBALocalForMerge = Constant<uint04>::Invalid;
 
        // Variable used by merging
        Eigen::Vector3f mPosMerge;
        Eigen::Vector3f mNormalVectorMerge;
 
        uint04 m_observation_count = 0U;
    protected:
        uint04 m_id;
        uint04 m_last_frame_locally_tracked = Constant<uint04>::Invalid;
        // Position in absolute coordinates
        Eigen::Vector3<g_type> m_world_pos;
 
        
 
        // Mean viewing direction
        Eigen::Vector3<g_type> mNormalVector;
 
        
 
        // Reference KeyFrame
        KeyFrame* mpRefKF = nullptr;
 
        // Tracking counters
        uint04 mnVisible = 0;
        uint04 mnFound = 0;
        MapPoint* mpReplaced = nullptr;
 
        // Scale invariance distances
        float mfMinDistance = 0.0f;
        float mfMaxDistance = 0.0f;
 
        Map* mpMap = nullptr;
        bool mbBad = false;
        
 
    };
 
}