123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374 |
- #ifndef FRAME_H
- #define FRAME_H
- #include<vector>
- #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
- #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
- #include "Thirdparty/Sophus/sophus/geometry.hpp"
- #include "ImuTypes.h"
- #include "ORBVocabulary.h"
- #include "Converter.h"
- #include "Settings.h"
- #include <mutex>
- #include <opencv2/opencv.hpp>
- #include "Eigen/Core"
- #include "sophus/se3.hpp"
- namespace ORB_SLAM3
- {
- #define FRAME_GRID_ROWS 48
- #define FRAME_GRID_COLS 64
- class MapPoint;
- class KeyFrame;
- class ConstraintPoseImu;
- class GeometricCamera;
- class ORBextractor;
- class Frame
- {
- public:
- Frame();
-
- Frame(const Frame &frame);
-
- Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
-
- Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
-
- Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
-
-
-
- void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1);
-
- void ComputeBoW();
-
- void SetPose(const Sophus::SE3<float> &Tcw);
-
- void SetVelocity(Eigen::Vector3f Vw);
- Eigen::Vector3f GetVelocity() const;
-
- void SetImuPoseVelocity(const Eigen::Matrix3f &Rwb, const Eigen::Vector3f &twb, const Eigen::Vector3f &Vwb);
- Eigen::Matrix<float,3,1> GetImuPosition() const;
- Eigen::Matrix<float,3,3> GetImuRotation();
- Sophus::SE3<float> GetImuPose();
- Sophus::SE3f GetRelativePoseTrl();
- Sophus::SE3f GetRelativePoseTlr();
- Eigen::Matrix3f GetRelativePoseTlr_rotation();
- Eigen::Vector3f GetRelativePoseTlr_translation();
- void SetNewBias(const IMU::Bias &b);
-
-
- bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
- bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
- Eigen::Vector3f inRefCoordinates(Eigen::Vector3f pCw);
-
- bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
- vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const;
-
-
- void ComputeStereoMatches();
-
- void ComputeStereoFromRGBD(const cv::Mat &imDepth);
-
- bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D);
- ConstraintPoseImu* mpcpi;
- bool imuIsPreintegrated();
- void setIntegrated();
- bool isSet() const;
-
- void UpdatePoseMatrices();
-
- inline Eigen::Vector3f GetCameraCenter(){
- return mOw;
- }
-
- inline Eigen::Matrix3f GetRotationInverse(){
- return mRwc;
- }
- inline Sophus::SE3<float> GetPose() const {
-
- return mTcw;
- }
- inline Eigen::Matrix3f GetRwc() const {
- return mRwc;
- }
- inline Eigen::Vector3f GetOw() const {
- return mOw;
- }
- inline bool HasPose() const {
- return mbHasPose;
- }
- inline bool HasVelocity() const {
- return mbHasVelocity;
- }
- private:
-
- Sophus::SE3<float> mTcw;
- Eigen::Matrix<float,3,3> mRwc;
- Eigen::Matrix<float,3,1> mOw;
- Eigen::Matrix<float,3,3> mRcw;
- Eigen::Matrix<float,3,1> mtcw;
- bool mbHasPose;
-
-
-
- Sophus::SE3<float> mTlr, mTrl;
- Eigen::Matrix<float,3,3> mRlr;
- Eigen::Vector3f mtlr;
-
- Eigen::Vector3f mVw;
- bool mbHasVelocity;
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- ORBVocabulary* mpORBvocabulary;
-
- ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
-
- double mTimeStamp;
-
- cv::Mat mK;
- Eigen::Matrix3f mK_;
- static float fx;
- static float fy;
- static float cx;
- static float cy;
- static float invfx;
- static float invfy;
- cv::Mat mDistCoef;
-
- float mbf;
-
- float mb;
-
-
- float mThDepth;
-
- int N;
-
-
-
- std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
- std::vector<cv::KeyPoint> mvKeysUn;
-
- std::vector<MapPoint*> mvpMapPoints;
-
- std::vector<float> mvuRight;
- std::vector<float> mvDepth;
-
- DBoW2::BowVector mBowVec;
- DBoW2::FeatureVector mFeatVec;
-
- cv::Mat mDescriptors, mDescriptorsRight;
-
-
- std::vector<bool> mvbOutlier;
- int mnCloseMPs;
-
- static float mfGridElementWidthInv;
- static float mfGridElementHeightInv;
- std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
- IMU::Bias mPredBias;
-
- IMU::Bias mImuBias;
-
- IMU::Calib mImuCalib;
-
- IMU::Preintegrated* mpImuPreintegrated;
- KeyFrame* mpLastKeyFrame;
-
- Frame* mpPrevFrame;
- IMU::Preintegrated* mpImuPreintegratedFrame;
-
- static long unsigned int nNextId;
- long unsigned int mnId;
-
- KeyFrame* mpReferenceKF;
-
- int mnScaleLevels;
- float mfScaleFactor;
- float mfLogScaleFactor;
- vector<float> mvScaleFactors;
- vector<float> mvInvScaleFactors;
- vector<float> mvLevelSigma2;
- vector<float> mvInvLevelSigma2;
-
- static float mnMinX;
- static float mnMaxX;
- static float mnMinY;
- static float mnMaxY;
- static bool mbInitialComputations;
- map<long unsigned int, cv::Point2f> mmProjectPoints;
- map<long unsigned int, cv::Point2f> mmMatchedInImage;
- string mNameFile;
- int mnDataset;
- #ifdef REGISTER_TIMES
- double mTimeORB_Ext;
- double mTimeStereoMatch;
- #endif
- private:
-
-
-
- void UndistortKeyPoints();
-
- void ComputeImageBounds(const cv::Mat &imLeft);
-
- void AssignFeaturesToGrid();
- bool mbIsSet;
- bool mbImuPreintegrated;
- std::mutex *mpMutexImu;
- public:
- GeometricCamera* mpCamera, *mpCamera2;
-
- int Nleft, Nright;
-
- int monoLeft, monoRight;
-
- std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;
-
- static cv::BFMatcher BFmatcher;
-
-
- std::vector<Eigen::Vector3f> mvStereo3Dpoints;
-
- std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
- Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
-
- void ComputeStereoFishEyeMatches();
- bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false);
- Eigen::Vector3f UnprojectStereoFishEye(const int &i);
- cv::Mat imgLeft, imgRight;
- void PrintPointDistribution(){
- int left = 0, right = 0;
- int Nlim = (Nleft != -1) ? Nleft : N;
- for(int i = 0; i < N; i++){
- if(mvpMapPoints[i] && !mvbOutlier[i]){
- if(i < Nlim) left++;
- else right++;
- }
- }
- cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl;
- }
- Sophus::SE3<double> T_test;
- };
- }
- #endif
|