ORBmatcher.h

 
#pragma once
#include "Base/Headers/Buffer.hpp"
#include "Base/Headers/Set.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "OrbSLAM/Thirdparty/Sophus/sophus/sim3.hpp"
#include "OrbSLAM/Headers/MapPointProjection.h"
#include <utility>
namespace cv
{
    class Mat;
}
 
namespace NDEVR
{
    class MapPoint;
    class Frame;
    class KeyFrame;
    class ORBmatcher
    {
    public:

        ORBmatcher(float nnratio=0.6f, bool checkOri=true);
        void setup(float mfNNratio = 0.6f, bool checkOri = true);
        static uint01 DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
 
        // Search matches between Frame keypoints and projected MapPoints. Returns number of matches
        // Used to track the local map (Tracking)
        uint04 searchByProjection(Frame &F, const Buffer<MapPointProjection> &vpMapPoints, const fltp04 th=3.0f, const bool bFarPoints = false, const float thFarPoints = 50.0f);
 
        // Project MapPoints tracked in last frame into the current frame and search matches.
        // Used to track from previous frame (Tracking)
        uint04 searchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const fltp04 th, const bool bMono);
 
        // Project MapPoints seen in KeyFrame into the Frame and search matches.
        // Used in relocalisation (Tracking)
        uint04 searchByProjection(Frame &frame, KeyFrame* pKF, const Set<MapPoint*> &sAlreadyFound, const float th, uint04 ORBdist);
 
        // Project MapPoints using a Similarity Transformation and search matches.
        // Used in loop detection (Loop Closing)
        uint04 searchByProjection(KeyFrame* pKF, Sophus::Sim3<g_type>& Scw, const Buffer<MapPoint*> &vpPoints, Buffer<MapPoint*> &vpMatched, uint04 th, fltp04 ratioHamming=1.0f);
 
        // Project MapPoints using a Similarity Transformation and search matches.
        // Used in Place Recognition (Loop Closing and Merging)
        uint04 searchByProjection(KeyFrame* pKF, Sophus::Sim3<g_type>& Scw, const Buffer<MapPoint*> &vpPoints, const Buffer<KeyFrame*> &vpPointsKFs, Buffer<MapPoint*> &vpMatched, Buffer<KeyFrame*> &vpMatchedKF, fltp04 th, fltp04 ratioHamming=1.0f);
 
        // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
        // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level)
        // Used in Relocalisation and Loop Detection
        uint04 searchByBoW(KeyFrame *pKF, const Frame &F, Buffer<MapPoint*>& vpMapPointMatches);
        uint04 searchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, Buffer<MapPoint*> &vpMatches12);
        
        // Matching for the Map Initialization (only used in the monocular case)
        uint04 searchForInitialization(Frame &F1, Frame &F2, Buffer<cv::Point2f> &vbPrevMatched, Buffer<uint04> &vnMatches12, int windowSize=10);
 
        // Matching to triangulate new MapPoints. Check Epipolar Constraint.
        uint04 searchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, Buffer<Vector<2, uint04>>& vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false);
 
        // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12]
        // In the stereo and RGB-t_dim case, s12=1
        // int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, Buffer<MapPoint *> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th);
        int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, Buffer<MapPoint *> &vpMatches12, const Sophus::Sim3<g_type>& S12, const float th);
 
        // Project MapPoints into KeyFrame and search for duplicated MapPoints.
        int Fuse(KeyFrame* pKF, const Buffer<MapPoint *> &vpMapPoints, const float th=3.0, const bool bRight = false);
 
        // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints.
        int Fuse(KeyFrame* pKF, Sophus::Sim3<g_type>& Scw, const Buffer<MapPoint*> &vpPoints, float th, Buffer<MapPoint *> &vpReplacePoint);
    public:
 
        static constexpr uint04 TH_LOW = 50U;
        static constexpr uint04 TH_HIGH = 100U;
        static constexpr uint04 HISTO_LENGTH = 30U;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    protected:
        
        static float RadiusByViewingCos(fltp04 viewCos);
        //checks orientation and returns removed amount
        void computeOrientationIdx(const cv::KeyPoint& k0, const cv::KeyPoint& k1, uint04 idx);
        uint04 checkOrientation(Frame& frame);
        uint04 checkOrientation(Buffer<MapPoint*>& vpMatches12);
        uint04 checkOrientation(Buffer<uint04>& vpMatches12);
        void computeMaxima(uint04& ind1, uint04& ind2, uint04& ind3) const;
        void resetRotationHistory();
        Buffer<uint04> m_rotational_history[HISTO_LENGTH];
        Buffer<uint04> m_indices;
        Buffer<uint04> m_v_indices;
        float mfNNratio = 0.0f;
        bool m_check_orientation = false;
    };
 
}