v1

include/KeyFrame.h

@@ -0,0 +1,238 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef KEYFRAME_H
+#define KEYFRAME_H
+
+#include "MapPoint.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "ORBVocabulary.h"
+#include "ORBextractor.h"
+#include "Frame.h"
+#include "KeyFrameDatabase.h"
+
+#include <mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Map;
+class MapPoint;
+class Frame;
+class KeyFrameDatabase;
+
+class KeyFrame
+{
+public:
+    KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);
+
+    // Pose functions
+    void SetPose(const cv::Mat &Tcw);
+    cv::Mat GetPose();
+    cv::Mat GetPoseInverse();
+    cv::Mat GetCameraCenter();
+    cv::Mat GetStereoCenter();
+    cv::Mat GetRotation();
+    cv::Mat GetTranslation();
+
+    // Bag of Words Representation
+    void ComputeBoW();
+
+    // Covisibility graph functions
+    void AddConnection(KeyFrame* pKF, const int &weight);
+    void EraseConnection(KeyFrame* pKF);
+    void UpdateConnections();
+    void UpdateBestCovisibles();
+    std::set<KeyFrame *> GetConnectedKeyFrames();
+    std::vector<KeyFrame* > GetVectorCovisibleKeyFrames();
+    std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N);
+    std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w);
+    int GetWeight(KeyFrame* pKF);
+
+    // Spanning tree functions
+    void AddChild(KeyFrame* pKF);
+    void EraseChild(KeyFrame* pKF);
+    void ChangeParent(KeyFrame* pKF);
+    std::set<KeyFrame*> GetChilds();
+    KeyFrame* GetParent();
+    bool hasChild(KeyFrame* pKF);
+
+    // Loop Edges
+    void AddLoopEdge(KeyFrame* pKF);
+    std::set<KeyFrame*> GetLoopEdges();
+
+    // MapPoint observation functions
+    void AddMapPoint(MapPoint* pMP, const size_t &idx);
+    void EraseMapPointMatch(const size_t &idx);
+    void EraseMapPointMatch(MapPoint* pMP);
+    void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP);
+    std::set<MapPoint*> GetMapPoints();
+    std::vector<MapPoint*> GetMapPointMatches();
+    int TrackedMapPoints(const int &minObs);
+    MapPoint* GetMapPoint(const size_t &idx);
+
+    // KeyPoint functions
+    std::vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r) const;
+    cv::Mat UnprojectStereo(int i);
+
+    // Image
+    bool IsInImage(const float &x, const float &y) const;
+
+    // Enable/Disable bad flag changes
+    void SetNotErase();
+    void SetErase();
+
+    // Set/check bad flag
+    void SetBadFlag();
+    bool isBad();
+
+    // Compute Scene Depth (q=2 median). Used in monocular.
+    float ComputeSceneMedianDepth(const int q);
+
+    static bool weightComp( int a, int b){
+        return a>b;
+    }
+
+    static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){
+        return pKF1->mnId<pKF2->mnId;
+    }
+
+
+    // The following variables are accesed from only 1 thread or never change (no mutex needed).
+public:
+
+    static long unsigned int nNextId;
+    long unsigned int mnId;
+    const long unsigned int mnFrameId;
+
+    const double mTimeStamp;
+
+    // Grid (to speed up feature matching)
+    const int mnGridCols;
+    const int mnGridRows;
+    const float mfGridElementWidthInv;
+    const float mfGridElementHeightInv;
+
+    // Variables used by the tracking
+    long unsigned int mnTrackReferenceForFrame;
+    long unsigned int mnFuseTargetForKF;
+
+    // Variables used by the local mapping
+    long unsigned int mnBALocalForKF;
+    long unsigned int mnBAFixedForKF;
+
+    // Variables used by the keyframe database
+    long unsigned int mnLoopQuery;
+    int mnLoopWords;
+    float mLoopScore;
+    long unsigned int mnRelocQuery;
+    int mnRelocWords;
+    float mRelocScore;
+
+    // Variables used by loop closing
+    cv::Mat mTcwGBA;
+    cv::Mat mTcwBefGBA;
+    long unsigned int mnBAGlobalForKF;
+
+    // Calibration parameters
+    const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
+
+    // Number of KeyPoints
+    const int N;
+
+    // KeyPoints, stereo coordinate and descriptors (all associated by an index)
+    const std::vector<cv::KeyPoint> mvKeys;
+    const std::vector<cv::KeyPoint> mvKeysUn;
+    const std::vector<float> mvuRight; // negative value for monocular points
+    const std::vector<float> mvDepth; // negative value for monocular points
+    const cv::Mat mDescriptors;
+
+    //BoW
+    DBoW2::BowVector mBowVec;
+    DBoW2::FeatureVector mFeatVec;
+
+    // Pose relative to parent (this is computed when bad flag is activated)
+    cv::Mat mTcp;
+
+    // Scale
+    const int mnScaleLevels;
+    const float mfScaleFactor;
+    const float mfLogScaleFactor;
+    const std::vector<float> mvScaleFactors;
+    const std::vector<float> mvLevelSigma2;
+    const std::vector<float> mvInvLevelSigma2;
+
+    // Image bounds and calibration
+    const int mnMinX;
+    const int mnMinY;
+    const int mnMaxX;
+    const int mnMaxY;
+    const cv::Mat mK;
+
+
+    // The following variables need to be accessed trough a mutex to be thread safe.
+protected:
+
+    // SE3 Pose and camera center
+    cv::Mat Tcw;
+    cv::Mat Twc;
+    cv::Mat Ow;
+
+    cv::Mat Cw; // Stereo middel point. Only for visualization
+
+    // MapPoints associated to keypoints
+    std::vector<MapPoint*> mvpMapPoints;
+
+    // BoW
+    KeyFrameDatabase* mpKeyFrameDB;
+    ORBVocabulary* mpORBvocabulary;
+
+    // Grid over the image to speed up feature matching
+    std::vector< std::vector <std::vector<size_t> > > mGrid;
+
+    std::map<KeyFrame*,int> mConnectedKeyFrameWeights;
+    std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
+    std::vector<int> mvOrderedWeights;
+
+    // Spanning Tree and Loop Edges
+    bool mbFirstConnection;
+    KeyFrame* mpParent;
+    std::set<KeyFrame*> mspChildrens;
+    std::set<KeyFrame*> mspLoopEdges;
+
+    // Bad flags
+    bool mbNotErase;
+    bool mbToBeErased;
+    bool mbBad;    
+
+    float mHalfBaseline; // Only for visualization
+
+    Map* mpMap;
+
+    std::mutex mMutexPose;
+    std::mutex mMutexConnections;
+    std::mutex mMutexFeatures;
+};
+
+} //namespace ORB_SLAM
+
+#endif // KEYFRAME_H