├── README.md ├── include ├── ORBVocabulary.h ├── Converter.h ├── MapDrawer.h ├── Viewer.h ├── FrameDrawer.h ├── KeyFrameDatabase.h ├── ORBextractor.h ├── Sim3Solver.h ├── TwoViewReconstruction.h ├── Initializer.h ├── CameraModels │ ├── GeometricCamera.h │ ├── Pinhole.h │ └── KannalaBrandt8.h ├── LocalMapping.h ├── Atlas.h ├── ORBmatcher.h ├── Map.h ├── LoopClosing.h ├── PnPsolver.h ├── Optimizer.h ├── OptimizableTypes.h ├── System.h ├── MapPoint.h ├── ImuTypes.h ├── MLPnPsolver.h ├── Tracking.h └── Frame.h └── src ├── Converter.cc ├── CameraModels └── Pinhole.cpp ├── Atlas.cc ├── OptimizableTypes.cpp ├── Viewer.cc └── Sim3Solver.cc /README.md: -------------------------------------------------------------------------------- 1 | # ORB-SLAM3-Note 2 | ORB-SLAM3源码注释，注释了大部分代码。 3 | 4 | 结合代码整理总结了视觉SLAM中的一些重要知识点，给出理论推导。如果有理解上的错误，请您指正。 5 | 6 | :) 如果对您有帮助，帮我点个star呦~ 7 | 8 | ## 目录 9 | - [ORB-SLAM3知识点(一)：词袋模型BoW](https://zhuanlan.zhihu.com/p/354616831) 10 | - [ORB-SLAM3知识点(二)：ORB提取](https://zhuanlan.zhihu.com/p/355441452) 11 | - [ORB-SLAM3知识点(三)：2d-2d匹配](https://zhuanlan.zhihu.com/p/355445588) 12 | - [ORB-SLAM3知识点(四)：3d-2d匹配](https://zhuanlan.zhihu.com/p/355848913) 13 | - [ORB-SLAM3知识点(五)：3d-3d匹配](https://zhuanlan.zhihu.com/p/356147588) 14 | - ORB-SLAM3知识点(六)：BA优化、Jacobian 15 | - ORB-SLAM3知识点(七)：... 16 | 17 | 可以到知乎上查看详细内容~ 18 | -------------------------------------------------------------------------------- /include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef ORBVOCABULARY_H 21 | #define ORBVOCABULARY_H 22 | 23 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 24 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 25 | 26 | namespace ORB_SLAM3 27 | { 28 | 29 | typedef DBoW2::TemplatedVocabulary 30 | ORBVocabulary; 31 | 32 | } //namespace ORB_SLAM 33 | 34 | #endif // ORBVOCABULARY_H 35 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef CONVERTER_H 21 | #define CONVERTER_H 22 | 23 | #include 24 | 25 | #include 26 | #include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 27 | #include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 28 | 29 | namespace ORB_SLAM3 30 | { 31 | 32 | class Converter 33 | { 34 | public: 35 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 36 | 37 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 38 | static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3); 39 | 40 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 41 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 42 | static cv::Mat toCvMat(const Eigen::Matrix &m); 43 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 44 | static cv::Mat toCvMat(const Eigen::Matrix &m); 45 | static cv::Mat toCvMat(const Eigen::MatrixXd &m); 46 | 47 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 48 | static cv::Mat tocvSkewMatrix(const cv::Mat &v); 49 | 50 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 51 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 52 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 53 | static Eigen::Matrix toMatrix4d(const cv::Mat &cvMat4); 54 | static std::vector toQuaternion(const cv::Mat &M); 55 | 56 | static bool isRotationMatrix(const cv::Mat &R); 57 | static std::vector toEuler(const cv::Mat &R); 58 | 59 | }; 60 | 61 | }// namespace ORB_SLAM 62 | 63 | #endif // CONVERTER_H 64 | -------------------------------------------------------------------------------- /include/MapDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef MAPDRAWER_H 21 | #define MAPDRAWER_H 22 | 23 | #include"Atlas.h" 24 | #include"MapPoint.h" 25 | #include"KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | namespace ORB_SLAM3 31 | { 32 | 33 | class MapDrawer 34 | { 35 | public: 36 | MapDrawer(Atlas* pAtlas, const string &strSettingPath); 37 | 38 | Atlas* mpAtlas; 39 | 40 | void DrawMapPoints(); 41 | void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const bool bDrawInertialGraph); 42 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 43 | void SetCurrentCameraPose(const cv::Mat &Tcw); 44 | void SetReferenceKeyFrame(KeyFrame *pKF); 45 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw); 46 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw, pangolin::OpenGlMatrix &MTwwp); 47 | 48 | private: 49 | 50 | bool ParseViewerParamFile(cv::FileStorage &fSettings); 51 | 52 | float mKeyFrameSize; 53 | float mKeyFrameLineWidth; 54 | float mGraphLineWidth; 55 | float mPointSize; 56 | float mCameraSize; 57 | float mCameraLineWidth; 58 | 59 | cv::Mat mCameraPose; 60 | 61 | std::mutex mMutexCamera; 62 | 63 | float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f}, 64 | {0.8f, 0.4f, 1.0f}, 65 | {1.0f, 0.2f, 0.4f}, 66 | {0.6f, 0.0f, 1.0f}, 67 | {1.0f, 1.0f, 0.0f}, 68 | {0.0f, 1.0f, 1.0f}}; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif // MAPDRAWER_H 74 | -------------------------------------------------------------------------------- /include/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef VIEWER_H 21 | #define VIEWER_H 22 | 23 | #include "FrameDrawer.h" 24 | #include "MapDrawer.h" 25 | #include "Tracking.h" 26 | #include "System.h" 27 | 28 | #include 29 | 30 | namespace ORB_SLAM3 31 | { 32 | 33 | class Tracking; 34 | class FrameDrawer; 35 | class MapDrawer; 36 | class System; 37 | 38 | class Viewer 39 | { 40 | public: 41 | Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath); 42 | 43 | // Main thread function. Draw points, keyframes, the current camera pose and the last processed 44 | // frame. Drawing is refreshed according to the camera fps. We use Pangolin. 45 | void Run(); 46 | 47 | void RequestFinish(); 48 | 49 | void RequestStop(); 50 | 51 | bool isFinished(); 52 | 53 | bool isStopped(); 54 | 55 | bool isStepByStep(); 56 | 57 | void Release(); 58 | 59 | void SetTrackingPause(); 60 | 61 | bool both; 62 | private: 63 | 64 | bool ParseViewerParamFile(cv::FileStorage &fSettings); 65 | 66 | bool Stop(); 67 | 68 | System* mpSystem; 69 | FrameDrawer* mpFrameDrawer; 70 | MapDrawer* mpMapDrawer; 71 | Tracking* mpTracker; 72 | 73 | // 1/fps in ms 74 | double mT; 75 | float mImageWidth, mImageHeight; 76 | 77 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 78 | 79 | bool CheckFinish(); 80 | void SetFinish(); 81 | bool mbFinishRequested; 82 | bool mbFinished; 83 | std::mutex mMutexFinish; 84 | 85 | bool mbStopped; 86 | bool mbStopRequested; 87 | std::mutex mMutexStop; 88 | 89 | bool mbStopTrack; 90 | 91 | }; 92 | 93 | } 94 | 95 | 96 | #endif // VIEWER_H 97 | 98 | 99 | -------------------------------------------------------------------------------- /include/FrameDrawer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef FRAMEDRAWER_H 21 | #define FRAMEDRAWER_H 22 | 23 | #include "Tracking.h" 24 | #include "MapPoint.h" 25 | #include "Atlas.h" 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | 34 | namespace ORB_SLAM3 35 | { 36 | 37 | class Tracking; 38 | class Viewer; 39 | 40 | class FrameDrawer 41 | { 42 | public: 43 | FrameDrawer(Atlas* pAtlas); 44 | 45 | // Update info from the last processed frame. 46 | void Update(Tracking *pTracker); 47 | 48 | // Draw last processed frame. 49 | cv::Mat DrawFrame(bool bOldFeatures=true); 50 | cv::Mat DrawRightFrame(); 51 | 52 | bool both; 53 | 54 | protected: 55 | 56 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 57 | 58 | // Info of the frame to be drawn 59 | cv::Mat mIm, mImRight; 60 | int N; 61 | vector mvCurrentKeys,mvCurrentKeysRight; 62 | vector mvbMap, mvbVO; 63 | bool mbOnlyTracking; 64 | int mnTracked, mnTrackedVO; 65 | vector mvIniKeys; 66 | vector mvIniMatches; 67 | int mState; 68 | 69 | Atlas* mpAtlas; 70 | 71 | std::mutex mMutex; 72 | vector > mvTracks; 73 | 74 | Frame mCurrentFrame; 75 | vector mvpLocalMap; 76 | vector mvMatchedKeys; 77 | vector mvpMatchedMPs; 78 | vector mvOutlierKeys; 79 | vector mvpOutlierMPs; 80 | 81 | map mmProjectPoints; 82 | map mmMatchedInImage; 83 | 84 | }; 85 | 86 | } //namespace ORB_SLAM 87 | 88 | #endif // FRAMEDRAWER_H 89 | -------------------------------------------------------------------------------- /include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef KEYFRAMEDATABASE_H 21 | #define KEYFRAMEDATABASE_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "KeyFrame.h" 28 | #include "Frame.h" 29 | #include "ORBVocabulary.h" 30 | #include "Map.h" 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace ORB_SLAM3 40 | { 41 | 42 | class KeyFrame; 43 | class Frame; 44 | class Map; 45 | 46 | 47 | class KeyFrameDatabase 48 | { 49 | friend class boost::serialization::access; 50 | 51 | template 52 | void serialize(Archive& ar, const unsigned int version) 53 | { 54 | ar & mvBackupInvertedFileId; 55 | } 56 | 57 | public: 58 | 59 | KeyFrameDatabase(const ORBVocabulary &voc); 60 | 61 | void add(KeyFrame* pKF); 62 | 63 | void erase(KeyFrame* pKF); 64 | 65 | void clear(); 66 | void clearMap(Map* pMap); 67 | 68 | // Loop Detection(DEPRECATED) 69 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 70 | 71 | // Loop and Merge Detection 72 | void DetectCandidates(KeyFrame* pKF, float minScore,vector& vpLoopCand, vector& vpMergeCand); 73 | void DetectBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nMinWords); 74 | void DetectNBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nNumCandidates); 75 | 76 | // Relocalization 77 | std::vector DetectRelocalizationCandidates(Frame* F, Map* pMap); 78 | 79 | void PreSave(); 80 | void PostLoad(map mpKFid); 81 | void SetORBVocabulary(ORBVocabulary* pORBVoc); 82 | 83 | protected: 84 | 85 | // Associated vocabulary 86 | const ORBVocabulary* mpVoc; 87 | 88 | // Inverted file 89 | // 每一个worldId下面对应kf集合 90 | std::vector > mvInvertedFile; 91 | 92 | // For save relation without pointer, this is necessary for save/load function 93 | std::vector > mvBackupInvertedFileId; 94 | 95 | // Mutex 96 | std::mutex mMutex; 97 | }; 98 | 99 | } //namespace ORB_SLAM 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef ORBEXTRACTOR_H 20 | #define ORBEXTRACTOR_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace ORB_SLAM3 28 | { 29 | 30 | class ExtractorNode 31 | { 32 | public: 33 | ExtractorNode():bNoMore(false){} 34 | 35 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 36 | 37 | std::vector vKeys; 38 | cv::Point2i UL, UR, BL, BR; 39 | std::list::iterator lit; 40 | bool bNoMore; 41 | }; 42 | 43 | class ORBextractor 44 | { 45 | public: 46 | 47 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 48 | 49 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 50 | int iniThFAST, int minThFAST); 51 | 52 | ~ORBextractor(){} 53 | 54 | // Compute the ORB features and descriptors on an image. 55 | // ORB are dispersed on the image using an octree. 56 | // Mask is ignored in the current implementation. 57 | int operator()( cv::InputArray _image, cv::InputArray _mask, 58 | std::vector& _keypoints, 59 | cv::OutputArray _descriptors, std::vector &vLappingArea); 60 | 61 | int inline GetLevels(){ 62 | return nlevels;} 63 | 64 | float inline GetScaleFactor(){ 65 | return scaleFactor;} 66 | 67 | std::vector inline GetScaleFactors(){ 68 | return mvScaleFactor; 69 | } 70 | 71 | std::vector inline GetInverseScaleFactors(){ 72 | return mvInvScaleFactor; 73 | } 74 | 75 | std::vector inline GetScaleSigmaSquares(){ 76 | return mvLevelSigma2; 77 | } 78 | 79 | std::vector inline GetInverseScaleSigmaSquares(){ 80 | return mvInvLevelSigma2; 81 | } 82 | 83 | std::vector mvImagePyramid; 84 | 85 | protected: 86 | 87 | void ComputePyramid(cv::Mat image); 88 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 89 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 90 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 91 | 92 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 93 | std::vector pattern; 94 | 95 | int nfeatures; 96 | double scaleFactor; 97 | int nlevels; 98 | int iniThFAST; 99 | int minThFAST; 100 | 101 | std::vector mnFeaturesPerLevel; 102 | 103 | std::vector umax; 104 | 105 | std::vector mvScaleFactor; 106 | std::vector mvInvScaleFactor; 107 | std::vector mvLevelSigma2; 108 | std::vector mvInvLevelSigma2; 109 | }; 110 | 111 | } //namespace ORB_SLAM 112 | 113 | #endif 114 | 115 | -------------------------------------------------------------------------------- /include/Sim3Solver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef SIM3SOLVER_H 21 | #define SIM3SOLVER_H 22 | 23 | #include 24 | #include 25 | 26 | #include "KeyFrame.h" 27 | 28 | 29 | 30 | namespace ORB_SLAM3 31 | { 32 | 33 | class Sim3Solver 34 | { 35 | public: 36 | 37 | Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12, const bool bFixScale = true, 38 | const vector vpKeyFrameMatchedMP = vector()); 39 | 40 | void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); 41 | 42 | cv::Mat find(std::vector &vbInliers12, int &nInliers); 43 | 44 | cv::Mat iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); 45 | cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge); 46 | 47 | cv::Mat GetEstimatedRotation(); 48 | cv::Mat GetEstimatedTranslation(); 49 | float GetEstimatedScale(); 50 | 51 | 52 | protected: 53 | 54 | void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C); 55 | 56 | void ComputeSim3(cv::Mat &P1, cv::Mat &P2); 57 | 58 | void CheckInliers(); 59 | 60 | void Project(const std::vector &vP3Dw, std::vector &vP2D, cv::Mat Tcw, GeometricCamera* pCamera); 61 | void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, GeometricCamera* pCamera); 62 | 63 | 64 | protected: 65 | 66 | // KeyFrames and matches 67 | KeyFrame* mpKF1; 68 | KeyFrame* mpKF2; 69 | 70 | std::vector mvX3Dc1; 71 | std::vector mvX3Dc2; 72 | std::vector mvpMapPoints1; 73 | std::vector mvpMapPoints2; 74 | std::vector mvpMatches12; 75 | std::vector mvnIndices1; 76 | std::vector mvSigmaSquare1; 77 | std::vector mvSigmaSquare2; 78 | std::vector mvnMaxError1; 79 | std::vector mvnMaxError2; 80 | 81 | int N; 82 | int mN1; 83 | 84 | // Current Estimation 85 | cv::Mat mR12i; 86 | cv::Mat mt12i; 87 | float ms12i; 88 | cv::Mat mT12i; 89 | cv::Mat mT21i; 90 | std::vector mvbInliersi; 91 | int mnInliersi; 92 | 93 | // Current Ransac State 94 | int mnIterations; 95 | std::vector mvbBestInliers; 96 | int mnBestInliers; 97 | cv::Mat mBestT12; 98 | cv::Mat mBestRotation; 99 | cv::Mat mBestTranslation; 100 | float mBestScale; 101 | 102 | // Scale is fixed to 1 in the stereo/RGBD case 103 | bool mbFixScale; 104 | 105 | // Indices for random selection 106 | std::vector mvAllIndices; 107 | 108 | // Projections 109 | std::vector mvP1im1; 110 | std::vector mvP2im2; 111 | 112 | // RANSAC probability 113 | double mRansacProb; 114 | 115 | // RANSAC min inliers 116 | int mRansacMinInliers; 117 | 118 | // RANSAC max iterations 119 | int mRansacMaxIts; 120 | 121 | // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2 122 | float mTh; 123 | float mSigma2; 124 | 125 | // Calibration 126 | cv::Mat mK1; 127 | cv::Mat mK2; 128 | 129 | GeometricCamera* pCamera1, *pCamera2; 130 | }; 131 | 132 | } //namespace ORB_SLAM 133 | 134 | #endif // SIM3SOLVER_H 135 | -------------------------------------------------------------------------------- /include/TwoViewReconstruction.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef TwoViewReconstruction_H 20 | #define TwoViewReconstruction_H 21 | 22 | #include 23 | 24 | #include 25 | 26 | namespace ORB_SLAM3 27 | { 28 | 29 | class TwoViewReconstruction 30 | { 31 | typedef std::pair Match; 32 | 33 | public: 34 | 35 | // Fix the reference frame 36 | TwoViewReconstruction(cv::Mat& k, float sigma = 1.0, int iterations = 200); 37 | 38 | // Computes in parallel a fundamental matrix and a homography 39 | // Selects a model and tries to recover the motion and the structure from motion 40 | bool Reconstruct(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, 41 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); 42 | 43 | private: 44 | 45 | void FindHomography(std::vector &vbMatchesInliers, float &score, cv::Mat &H21); 46 | void FindFundamental(std::vector &vbInliers, float &score, cv::Mat &F21); 47 | 48 | cv::Mat ComputeH21(const std::vector &vP1, const std::vector &vP2); 49 | cv::Mat ComputeF21(const std::vector &vP1, const std::vector &vP2); 50 | 51 | float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, std::vector &vbMatchesInliers, float sigma); 52 | 53 | float CheckFundamental(const cv::Mat &F21, std::vector &vbMatchesInliers, float sigma); 54 | 55 | bool ReconstructF(std::vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, 56 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated, float minParallax, int minTriangulated); 57 | 58 | bool ReconstructH(std::vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, 59 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D,std:: vector &vbTriangulated, float minParallax, int minTriangulated); 60 | 61 | void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); 62 | 63 | void Normalize(const std::vector &vKeys, std::vector &vNormalizedPoints, cv::Mat &T); 64 | 65 | 66 | int CheckRT(const cv::Mat &R, const cv::Mat &t, const std::vector &vKeys1, const std::vector &vKeys2, 67 | const std::vector &vMatches12, std::vector &vbInliers, 68 | const cv::Mat &K, std::vector &vP3D, float th2, std::vector &vbGood, float ¶llax); 69 | 70 | void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); 71 | 72 | 73 | // Keypoints from Reference Frame (Frame 1) 74 | std::vector mvKeys1; 75 | 76 | // Keypoints from Current Frame (Frame 2) 77 | std::vector mvKeys2; 78 | 79 | // Current Matches from Reference to Current 80 | std::vector mvMatches12; 81 | std::vector mvbMatched1; 82 | 83 | // Calibration 84 | cv::Mat mK; 85 | 86 | // Standard Deviation and Variance 87 | float mSigma, mSigma2; 88 | 89 | // Ransac max iterations 90 | int mMaxIterations; 91 | 92 | // Ransac sets 93 | std::vector > mvSets; 94 | 95 | }; 96 | 97 | } //namespace ORB_SLAM 98 | 99 | #endif // TwoViewReconstruction_H 100 | -------------------------------------------------------------------------------- /include/Initializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef INITIALIZER_H 21 | #define INITIALIZER_H 22 | 23 | #include 24 | #include "Frame.h" 25 | 26 | #include 27 | 28 | namespace ORB_SLAM3 29 | { 30 | 31 | class Map; 32 | 33 | // THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE. 34 | class Initializer 35 | { 36 | typedef pair Match; 37 | 38 | public: 39 | 40 | // Fix the reference frame 41 | Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200); 42 | 43 | // Computes in parallel a fundamental matrix and a homography 44 | // Selects a model and tries to recover the motion and the structure from motion 45 | bool Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, 46 | cv::Mat &t21, vector &vP3D, vector &vbTriangulated); 47 | 48 | private: 49 | 50 | void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21); 51 | void FindFundamental(vector &vbInliers, float &score, cv::Mat &F21); 52 | 53 | cv::Mat ComputeH21(const vector &vP1, const vector &vP2); 54 | cv::Mat ComputeF21(const vector &vP1, const vector &vP2); 55 | 56 | float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma); 57 | 58 | float CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma); 59 | 60 | bool ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, 61 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 62 | 63 | bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, 64 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 65 | 66 | void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); 67 | 68 | void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); 69 | // void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); 70 | 71 | int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2, 72 | const vector &vMatches12, vector &vbInliers, 73 | const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax); 74 | 75 | void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); 76 | 77 | 78 | // Keypoints from Reference Frame (Frame 1) 79 | vector mvKeys1; 80 | 81 | // Keypoints from Current Frame (Frame 2) 82 | vector mvKeys2; 83 | 84 | // Current Matches from Reference to Current 85 | vector mvMatches12; 86 | vector mvbMatched1; 87 | 88 | // Calibration 89 | cv::Mat mK; 90 | 91 | // Standard Deviation and Variance 92 | float mSigma, mSigma2; 93 | 94 | // Ransac max iterations 95 | int mMaxIterations; 96 | 97 | // Ransac sets 98 | vector > mvSets; 99 | 100 | GeometricCamera* mpCamera; 101 | 102 | }; 103 | 104 | } //namespace ORB_SLAM 105 | 106 | #endif // INITIALIZER_H 107 | -------------------------------------------------------------------------------- /include/CameraModels/GeometricCamera.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef CAMERAMODELS_GEOMETRICCAMERA_H 20 | #define CAMERAMODELS_GEOMETRICCAMERA_H 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace ORB_SLAM3 { 37 | class GeometricCamera { 38 | 39 | friend class boost::serialization::access; 40 | 41 | template 42 | void serialize(Archive& ar, const unsigned int version) 43 | { 44 | ar & mnId; 45 | ar & mnType; 46 | ar & mvParameters; 47 | } 48 | 49 | 50 | public: 51 | GeometricCamera() {} 52 | GeometricCamera(const std::vector &_vParameters) : mvParameters(_vParameters) {} 53 | ~GeometricCamera() {} 54 | 55 | virtual cv::Point2f project(const cv::Point3f &p3D) = 0; 56 | virtual cv::Point2f project(const cv::Mat& m3D) = 0; 57 | virtual Eigen::Vector2d project(const Eigen::Vector3d & v3D) = 0; 58 | virtual cv::Mat projectMat(const cv::Point3f& p3D) = 0; 59 | 60 | virtual float uncertainty2(const Eigen::Matrix &p2D) = 0; 61 | 62 | virtual cv::Point3f unproject(const cv::Point2f &p2D) = 0; 63 | virtual cv::Mat unprojectMat(const cv::Point2f &p2D) = 0; 64 | 65 | virtual cv::Mat projectJac(const cv::Point3f &p3D) = 0; 66 | virtual Eigen::Matrix projectJac(const Eigen::Vector3d& v3D) = 0; 67 | 68 | virtual cv::Mat unprojectJac(const cv::Point2f &p2D) = 0; 69 | 70 | virtual bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, 71 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated) = 0; 72 | 73 | virtual cv::Mat toK() = 0; 74 | 75 | virtual bool epipolarConstrain(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc) = 0; 76 | 77 | float getParameter(const int i){return mvParameters[i];} 78 | void setParameter(const float p, const size_t i){mvParameters[i] = p;} 79 | 80 | size_t size(){return mvParameters.size();} 81 | 82 | virtual bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, 83 | cv::Mat& Tcw1, cv::Mat& Tcw2, 84 | const float sigmaLevel1, const float sigmaLevel2, 85 | cv::Mat& x3Dtriangulated) = 0; 86 | 87 | unsigned int GetId() { return mnId; } 88 | 89 | unsigned int GetType() { return mnType; } 90 | 91 | const unsigned int CAM_PINHOLE = 0; 92 | const unsigned int CAM_FISHEYE = 1; 93 | 94 | static long unsigned int nNextId; 95 | 96 | protected: 97 | std::vector mvParameters; 98 | 99 | unsigned int mnId; 100 | 101 | unsigned int mnType; 102 | }; 103 | } 104 | 105 | 106 | #endif //CAMERAMODELS_GEOMETRICCAMERA_H 107 | -------------------------------------------------------------------------------- /include/CameraModels/Pinhole.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef CAMERAMODELS_PINHOLE_H 20 | #define CAMERAMODELS_PINHOLE_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "GeometricCamera.h" 32 | 33 | #include "TwoViewReconstruction.h" 34 | 35 | namespace ORB_SLAM3 { 36 | class Pinhole : public GeometricCamera { 37 | 38 | friend class boost::serialization::access; 39 | 40 | template 41 | void serialize(Archive& ar, const unsigned int version) 42 | { 43 | ar & boost::serialization::base_object(*this); 44 | } 45 | 46 | public: 47 | Pinhole() { 48 | mvParameters.resize(4); 49 | mnId=nNextId++; 50 | mnType = CAM_PINHOLE; 51 | } 52 | Pinhole(const std::vector _vParameters) : GeometricCamera(_vParameters), tvr(nullptr) { 53 | assert(mvParameters.size() == 4); 54 | mnId=nNextId++; 55 | mnType = CAM_PINHOLE; 56 | } 57 | 58 | Pinhole(Pinhole* pPinhole) : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) { 59 | assert(mvParameters.size() == 4); 60 | mnId=nNextId++; 61 | mnType = CAM_PINHOLE; 62 | } 63 | 64 | 65 | ~Pinhole(){ 66 | if(tvr) delete tvr; 67 | } 68 | 69 | cv::Point2f project(const cv::Point3f &p3D); 70 | cv::Point2f project(const cv::Mat &m3D); 71 | Eigen::Vector2d project(const Eigen::Vector3d & v3D); 72 | cv::Mat projectMat(const cv::Point3f& p3D); 73 | 74 | float uncertainty2(const Eigen::Matrix &p2D); 75 | 76 | cv::Point3f unproject(const cv::Point2f &p2D); 77 | cv::Mat unprojectMat(const cv::Point2f &p2D); 78 | 79 | cv::Mat projectJac(const cv::Point3f &p3D); 80 | Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); 81 | 82 | cv::Mat unprojectJac(const cv::Point2f &p2D); 83 | 84 | bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, 85 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); 86 | 87 | cv::Mat toK(); 88 | 89 | bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc); 90 | 91 | bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, 92 | cv::Mat& Tcw1, cv::Mat& Tcw2, 93 | const float sigmaLevel1, const float sigmaLevel2, 94 | cv::Mat& x3Dtriangulated) { return false;} 95 | 96 | friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph); 97 | friend std::istream& operator>>(std::istream& os, Pinhole& ph); 98 | private: 99 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 100 | 101 | //Parameters vector corresponds to 102 | // [fx, fy, cx, cy] 103 | 104 | TwoViewReconstruction* tvr; 105 | }; 106 | } 107 | 108 | //BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole) 109 | 110 | #endif //CAMERAMODELS_PINHOLE_H 111 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef LOCALMAPPING_H 21 | #define LOCALMAPPING_H 22 | 23 | #include "KeyFrame.h" 24 | #include "Atlas.h" 25 | #include "LoopClosing.h" 26 | #include "Tracking.h" 27 | #include "KeyFrameDatabase.h" 28 | #include "Initializer.h" 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM3 34 | { 35 | 36 | class System; 37 | class Tracking; 38 | class LoopClosing; 39 | class Atlas; 40 | 41 | class LocalMapping 42 | { 43 | public: 44 | LocalMapping(System* pSys, Atlas* pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName=std::string()); 45 | 46 | void SetLoopCloser(LoopClosing* pLoopCloser); 47 | 48 | void SetTracker(Tracking* pTracker); 49 | 50 | // Main function 51 | void Run(); 52 | 53 | void InsertKeyFrame(KeyFrame* pKF); 54 | void EmptyQueue(); 55 | 56 | // Thread Synch 57 | void RequestStop(); 58 | void RequestReset(); 59 | void RequestResetActiveMap(Map* pMap); 60 | bool Stop(); 61 | void Release(); 62 | bool isStopped(); 63 | bool stopRequested(); 64 | bool AcceptKeyFrames(); 65 | void SetAcceptKeyFrames(bool flag); 66 | bool SetNotStop(bool flag); 67 | 68 | void InterruptBA(); 69 | 70 | void RequestFinish(); 71 | bool isFinished(); 72 | 73 | int KeyframesInQueue(){ 74 | unique_lock lock(mMutexNewKFs); 75 | return mlNewKeyFrames.size(); 76 | } 77 | 78 | bool IsInitializing(); 79 | double GetCurrKFTime(); 80 | KeyFrame* GetCurrKF(); 81 | 82 | std::mutex mMutexImuInit; 83 | 84 | Eigen::MatrixXd mcovInertial; 85 | Eigen::Matrix3d mRwg; 86 | Eigen::Vector3d mbg; 87 | Eigen::Vector3d mba; 88 | double mScale; 89 | double mInitTime; 90 | double mCostTime; 91 | bool mbNewInit; 92 | unsigned int mInitSect; 93 | unsigned int mIdxInit; 94 | unsigned int mnKFs; 95 | double mFirstTs; 96 | int mnMatchesInliers; 97 | 98 | // For debugging (erase in normal mode) 99 | int mInitFr; 100 | int mIdxIteration; 101 | string strSequence; 102 | 103 | bool mbNotBA1; 104 | bool mbNotBA2; 105 | bool mbBadImu; 106 | 107 | bool mbWriteStats; 108 | 109 | // not consider far points (clouds) 110 | bool mbFarPoints; 111 | float mThFarPoints; 112 | protected: 113 | 114 | bool CheckNewKeyFrames(); 115 | void ProcessNewKeyFrame(); 116 | void CreateNewMapPoints(); 117 | 118 | void MapPointCulling(); 119 | void SearchInNeighbors(); 120 | void KeyFrameCulling(); 121 | 122 | cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); 123 | 124 | cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 125 | 126 | System *mpSystem; 127 | 128 | bool mbMonocular; 129 | bool mbInertial; 130 | 131 | void ResetIfRequested(); 132 | bool mbResetRequested; 133 | bool mbResetRequestedActiveMap; 134 | Map* mpMapToReset; 135 | std::mutex mMutexReset; 136 | 137 | bool CheckFinish(); 138 | void SetFinish(); 139 | bool mbFinishRequested; 140 | bool mbFinished; 141 | std::mutex mMutexFinish; 142 | 143 | Atlas* mpAtlas; 144 | 145 | LoopClosing* mpLoopCloser; 146 | Tracking* mpTracker; 147 | 148 | std::list mlNewKeyFrames; 149 | 150 | KeyFrame* mpCurrentKeyFrame; 151 | 152 | std::list mlpRecentAddedMapPoints; 153 | 154 | std::mutex mMutexNewKFs; 155 | 156 | bool mbAbortBA; 157 | 158 | bool mbStopped; 159 | bool mbStopRequested; 160 | bool mbNotStop; 161 | std::mutex mMutexStop; 162 | 163 | bool mbAcceptKeyFrames; 164 | std::mutex mMutexAccept; 165 | 166 | void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false); 167 | void ScaleRefinement(); 168 | 169 | bool bInitializing; 170 | 171 | Eigen::MatrixXd infoInertial; 172 | int mNumLM; 173 | int mNumKFCulling; 174 | 175 | float mTinit; 176 | 177 | int countRefinement; 178 | 179 | //DEBUG 180 | ofstream f_lm; 181 | }; 182 | 183 | } //namespace ORB_SLAM 184 | 185 | #endif // LOCALMAPPING_H 186 | -------------------------------------------------------------------------------- /include/Atlas.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef ATLAS_H 20 | #define ATLAS_H 21 | 22 | #include "Map.h" 23 | #include "MapPoint.h" 24 | #include "KeyFrame.h" 25 | #include "GeometricCamera.h" 26 | #include "Pinhole.h" 27 | #include "KannalaBrandt8.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | namespace ORB_SLAM3 36 | { 37 | class Viewer; 38 | class Map; 39 | class MapPoint; 40 | class KeyFrame; 41 | class KeyFrameDatabase; 42 | class Frame; 43 | class KannalaBrandt8; 44 | class Pinhole; 45 | 46 | //BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") 47 | //BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") 48 | 49 | class Atlas 50 | { 51 | friend class boost::serialization::access; 52 | 53 | template 54 | void serialize(Archive &ar, const unsigned int version) 55 | { 56 | //ar.template register_type(); 57 | //ar.template register_type(); 58 | 59 | // Save/load the set of maps, the set is broken in libboost 1.58 for ubuntu 16.04 60 | //ar & mspMaps; 61 | ar & mvpBackupMaps; 62 | ar & mvpCameras; 63 | //ar & mvpBackupCamPin; 64 | //ar & mvpBackupCamKan; 65 | // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map 66 | ar & Map::nNextId; 67 | ar & Frame::nNextId; 68 | ar & KeyFrame::nNextId; 69 | ar & MapPoint::nNextId; 70 | ar & GeometricCamera::nNextId; 71 | ar & mnLastInitKFidMap; 72 | } 73 | 74 | public: 75 | Atlas(); 76 | Atlas(int initKFid); // When its initialization the first map is created 77 | ~Atlas(); 78 | 79 | void CreateNewMap(); 80 | void ChangeMap(Map* pMap); 81 | 82 | unsigned long int GetLastInitKFid(); 83 | 84 | void SetViewer(Viewer* pViewer); 85 | 86 | // Method for change components in the current map 87 | void AddKeyFrame(KeyFrame* pKF); 88 | void AddMapPoint(MapPoint* pMP); 89 | //void EraseMapPoint(MapPoint* pMP); 90 | //void EraseKeyFrame(KeyFrame* pKF); 91 | 92 | void AddCamera(GeometricCamera* pCam); 93 | 94 | /* All methods without Map pointer work on current map */ 95 | void SetReferenceMapPoints(const std::vector &vpMPs); 96 | void InformNewBigChange(); 97 | int GetLastBigChangeIdx(); 98 | 99 | long unsigned int MapPointsInMap(); 100 | long unsigned KeyFramesInMap(); 101 | 102 | // Method for get data in current map 103 | std::vector GetAllKeyFrames(); 104 | std::vector GetAllMapPoints(); 105 | std::vector GetReferenceMapPoints(); 106 | 107 | vector GetAllMaps(); 108 | 109 | int CountMaps(); 110 | 111 | void clearMap(); 112 | 113 | void clearAtlas(); 114 | 115 | Map* GetCurrentMap(); 116 | 117 | void SetMapBad(Map* pMap); 118 | void RemoveBadMaps(); 119 | 120 | bool isInertial(); 121 | void SetInertialSensor(); 122 | void SetImuInitialized(); 123 | bool isImuInitialized(); 124 | 125 | // Function for garantee the correction of serialization of this object 126 | void PreSave(); 127 | void PostLoad(); 128 | 129 | void SetKeyFrameDababase(KeyFrameDatabase* pKFDB); 130 | KeyFrameDatabase* GetKeyFrameDatabase(); 131 | 132 | void SetORBVocabulary(ORBVocabulary* pORBVoc); 133 | ORBVocabulary* GetORBVocabulary(); 134 | 135 | long unsigned int GetNumLivedKF(); 136 | 137 | long unsigned int GetNumLivedMP(); 138 | 139 | protected: 140 | 141 | std::set mspMaps; 142 | std::set mspBadMaps; 143 | // Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer 144 | std::vector mvpBackupMaps; 145 | Map* mpCurrentMap; 146 | 147 | std::vector mvpCameras; 148 | std::vector mvpBackupCamKan; 149 | std::vector mvpBackupCamPin; 150 | 151 | //Pinhole testCam; 152 | std::mutex mMutexAtlas; 153 | 154 | unsigned long int mnLastInitKFidMap; 155 | 156 | Viewer* mpViewer; 157 | bool mHasViewer; 158 | 159 | // Class references for the map reconstruction from the save file 160 | KeyFrameDatabase* mpKeyFrameDB; 161 | ORBVocabulary* mpORBVocabulary; 162 | 163 | 164 | }; // class Atlas 165 | 166 | } // namespace ORB_SLAM3 167 | 168 | #endif // ATLAS_H 169 | -------------------------------------------------------------------------------- /include/CameraModels/KannalaBrandt8.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef CAMERAMODELS_KANNALABRANDT8_H 20 | #define CAMERAMODELS_KANNALABRANDT8_H 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include "GeometricCamera.h" 32 | 33 | #include "TwoViewReconstruction.h" 34 | 35 | namespace ORB_SLAM3 { 36 | class KannalaBrandt8 final : public GeometricCamera { 37 | 38 | friend class boost::serialization::access; 39 | 40 | template 41 | void serialize(Archive& ar, const unsigned int version) 42 | { 43 | ar & boost::serialization::base_object(*this); 44 | ar & const_cast(precision); 45 | } 46 | 47 | public: 48 | KannalaBrandt8() : precision(1e-6) { 49 | mvParameters.resize(8); 50 | mnId=nNextId++; 51 | mnType = CAM_FISHEYE; 52 | } 53 | KannalaBrandt8(const std::vector _vParameters) : GeometricCamera(_vParameters), precision(1e-6), mvLappingArea(2,0) ,tvr(nullptr) { 54 | assert(mvParameters.size() == 8); 55 | mnId=nNextId++; 56 | mnType = CAM_FISHEYE; 57 | } 58 | 59 | KannalaBrandt8(const std::vector _vParameters, const float _precision) : GeometricCamera(_vParameters), 60 | precision(_precision), mvLappingArea(2,0) { 61 | assert(mvParameters.size() == 8); 62 | mnId=nNextId++; 63 | mnType = CAM_FISHEYE; 64 | } 65 | KannalaBrandt8(KannalaBrandt8* pKannala) : GeometricCamera(pKannala->mvParameters), precision(pKannala->precision), mvLappingArea(2,0) ,tvr(nullptr) { 66 | assert(mvParameters.size() == 8); 67 | mnId=nNextId++; 68 | mnType = CAM_FISHEYE; 69 | } 70 | 71 | cv::Point2f project(const cv::Point3f &p3D); 72 | cv::Point2f project(const cv::Mat& m3D); 73 | Eigen::Vector2d project(const Eigen::Vector3d & v3D); 74 | cv::Mat projectMat(const cv::Point3f& p3D); 75 | 76 | float uncertainty2(const Eigen::Matrix &p2D); 77 | 78 | cv::Point3f unproject(const cv::Point2f &p2D); 79 | cv::Mat unprojectMat(const cv::Point2f &p2D); 80 | 81 | cv::Mat projectJac(const cv::Point3f &p3D); 82 | Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); 83 | 84 | cv::Mat unprojectJac(const cv::Point2f &p2D); 85 | 86 | bool ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, 87 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); 88 | 89 | cv::Mat toK(); 90 | 91 | bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc); 92 | 93 | float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc, cv::Mat& p3D); 94 | 95 | std::vector mvLappingArea; 96 | 97 | bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, 98 | cv::Mat& Tcw1, cv::Mat& Tcw2, 99 | const float sigmaLevel1, const float sigmaLevel2, 100 | cv::Mat& x3Dtriangulated); 101 | 102 | friend std::ostream& operator<<(std::ostream& os, const KannalaBrandt8& kb); 103 | friend std::istream& operator>>(std::istream& is, KannalaBrandt8& kb); 104 | private: 105 | const float precision; 106 | 107 | //Parameters vector corresponds to 108 | //[fx, fy, cx, cy, k0, k1, k2, k3] 109 | 110 | TwoViewReconstruction* tvr; 111 | 112 | void Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Mat &Tcw1, const cv::Mat &Tcw2,cv::Mat &x3D); 113 | }; 114 | } 115 | 116 | //BOOST_CLASS_EXPORT_KEY(ORBSLAM2::KannalaBrandt8) 117 | 118 | #endif //CAMERAMODELS_KANNALABRANDT8_H 119 | -------------------------------------------------------------------------------- /include/ORBmatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef ORBMATCHER_H 21 | #define ORBMATCHER_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include"MapPoint.h" 28 | #include"KeyFrame.h" 29 | #include"Frame.h" 30 | 31 | 32 | namespace ORB_SLAM3 33 | { 34 | 35 | class ORBmatcher 36 | { 37 | public: 38 | 39 | ORBmatcher(float nnratio=0.6, bool checkOri=true); 40 | 41 | // Computes the Hamming distance between two ORB descriptors 42 | static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 43 | 44 | // Search matches between Frame keypoints and projected MapPoints. Returns number of matches 45 | // Used to track the local map (Tracking) 46 | int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th=3, const bool bFarPoints = false, const float thFarPoints = 50.0f); 47 | 48 | // Project MapPoints tracked in last frame into the current frame and search matches. 49 | // Used to track from previous frame (Tracking) 50 | int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono); 51 | 52 | // Project MapPoints seen in KeyFrame into the Frame and search matches. 53 | // Used in relocalisation (Tracking) 54 | int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set &sAlreadyFound, const float th, const int ORBdist); 55 | 56 | // Project MapPoints using a Similarity Transformation and search matches. 57 | // Used in loop detection (Loop Closing) 58 | int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, std::vector &vpMatched, int th, float ratioHamming=1.0); 59 | 60 | // Project MapPoints using a Similarity Transformation and search matches. 61 | // Used in Place Recognition (Loop Closing and Merging) 62 | int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, const std::vector &vpPointsKFs, std::vector &vpMatched, std::vector &vpMatchedKF, int th, float ratioHamming=1.0); 63 | 64 | // Search matches between MapPoints in a KeyFrame and ORB in a Frame. 65 | // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) 66 | // Used in Relocalisation and Loop Detection 67 | int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); 68 | int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); 69 | 70 | // Matching for the Map Initialization (only used in the monocular case) 71 | int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); 72 | 73 | // Matching to triangulate new MapPoints. Check Epipolar Constraint. 74 | int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, 75 | std::vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false); 76 | 77 | int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12, 78 | vector > &vMatchedPairs, const bool bOnlyStereo, vector &vMatchedPoints); 79 | 80 | // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12] 81 | // In the stereo and RGB-D case, s12=1 82 | int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th); 83 | 84 | // Project MapPoints into KeyFrame and search for duplicated MapPoints. 85 | int Fuse(KeyFrame* pKF, const vector &vpMapPoints, const float th=3.0, const bool bRight = false); 86 | 87 | // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints. 88 | int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, float th, vector &vpReplacePoint); 89 | 90 | public: 91 | 92 | static const int TH_LOW; 93 | static const int TH_HIGH; 94 | static const int HISTO_LENGTH; 95 | 96 | 97 | protected: 98 | 99 | bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF, const bool b1=false); 100 | bool CheckDistEpipolarLine2(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF, const float unc); 101 | 102 | float RadiusByViewingCos(const float &viewCos); 103 | 104 | void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); 105 | 106 | float mfNNratio; 107 | bool mbCheckOrientation; 108 | }; 109 | 110 | }// namespace ORB_SLAM 111 | 112 | #endif // ORBMATCHER_H 113 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef MAP_H 21 | #define MAP_H 22 | 23 | #include "MapPoint.h" 24 | #include "KeyFrame.h" 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | 33 | namespace ORB_SLAM3 34 | { 35 | 36 | class MapPoint; 37 | class KeyFrame; 38 | class Atlas; 39 | class KeyFrameDatabase; 40 | 41 | class Map 42 | { 43 | friend class boost::serialization::access; 44 | 45 | template 46 | void serialize(Archive &ar, const unsigned int version) 47 | { 48 | ar & mnId; 49 | ar & mnInitKFid; 50 | ar & mnMaxKFid; 51 | ar & mnBigChangeIdx; 52 | // Set of KeyFrames and MapPoints, in this version the set serializator is not working 53 | //ar & mspKeyFrames; 54 | //ar & mspMapPoints; 55 | 56 | ar & mvpBackupKeyFrames; 57 | ar & mvpBackupMapPoints; 58 | 59 | ar & mvBackupKeyFrameOriginsId; 60 | 61 | ar & mnBackupKFinitialID; 62 | ar & mnBackupKFlowerID; 63 | 64 | ar & mbImuInitialized; 65 | ar & mbIsInertial; 66 | ar & mbIMU_BA1; 67 | ar & mbIMU_BA2; 68 | 69 | ar & mnInitKFid; 70 | ar & mnMaxKFid; 71 | ar & mnLastLoopKFid; 72 | } 73 | 74 | public: 75 | Map(); 76 | Map(int initKFid); 77 | ~Map(); 78 | 79 | void AddKeyFrame(KeyFrame* pKF); 80 | void AddMapPoint(MapPoint* pMP); 81 | void EraseMapPoint(MapPoint* pMP); 82 | void EraseKeyFrame(KeyFrame* pKF); 83 | void SetReferenceMapPoints(const std::vector &vpMPs); 84 | void InformNewBigChange(); 85 | int GetLastBigChangeIdx(); 86 | 87 | std::vector GetAllKeyFrames(); 88 | std::vector GetAllMapPoints(); 89 | std::vector GetReferenceMapPoints(); 90 | 91 | long unsigned int MapPointsInMap(); 92 | long unsigned KeyFramesInMap(); 93 | 94 | long unsigned int GetId(); 95 | 96 | long unsigned int GetInitKFid(); 97 | void SetInitKFid(long unsigned int initKFif); 98 | long unsigned int GetMaxKFid(); 99 | 100 | KeyFrame* GetOriginKF(); 101 | 102 | void SetCurrentMap(); 103 | void SetStoredMap(); 104 | 105 | bool HasThumbnail(); 106 | bool IsInUse(); 107 | 108 | void SetBad(); 109 | bool IsBad(); 110 | 111 | void clear(); 112 | 113 | int GetMapChangeIndex(); 114 | void IncreaseChangeIndex(); 115 | int GetLastMapChange(); 116 | void SetLastMapChange(int currentChangeId); 117 | 118 | void SetImuInitialized(); 119 | bool isImuInitialized(); 120 | 121 | void RotateMap(const cv::Mat &R); 122 | void ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel=false, const cv::Mat t=cv::Mat::zeros(cv::Size(1,3),CV_32F)); 123 | 124 | void SetInertialSensor(); 125 | bool IsInertial(); 126 | void SetIniertialBA1(); 127 | void SetIniertialBA2(); 128 | bool GetIniertialBA1(); 129 | bool GetIniertialBA2(); 130 | 131 | void PrintEssentialGraph(); 132 | bool CheckEssentialGraph(); 133 | void ChangeId(long unsigned int nId); 134 | 135 | unsigned int GetLowerKFID(); 136 | 137 | void PreSave(std::set &spCams); 138 | void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map& mpKeyFrameId, map &mpCams); 139 | 140 | void printReprojectionError(list &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder); 141 | 142 | vector mvpKeyFrameOrigins; 143 | vector mvBackupKeyFrameOriginsId; 144 | KeyFrame* mpFirstRegionKF; 145 | std::mutex mMutexMapUpdate; 146 | 147 | // This avoid that two points are created simultaneously in separate threads (id conflict) 148 | std::mutex mMutexPointCreation; 149 | 150 | bool mbFail; 151 | 152 | // Size of the thumbnail (always in power of 2) 153 | static const int THUMB_WIDTH = 512; 154 | static const int THUMB_HEIGHT = 512; 155 | 156 | static long unsigned int nNextId; 157 | 158 | protected: 159 | 160 | long unsigned int mnId; 161 | 162 | std::set mspMapPoints; 163 | std::set mspKeyFrames; 164 | 165 | std::vector mvpBackupMapPoints; 166 | std::vector mvpBackupKeyFrames; 167 | 168 | KeyFrame* mpKFinitial; 169 | KeyFrame* mpKFlowerID; 170 | 171 | unsigned long int mnBackupKFinitialID; 172 | unsigned long int mnBackupKFlowerID; 173 | 174 | std::vector mvpReferenceMapPoints; 175 | 176 | bool mbImuInitialized; 177 | 178 | int mnMapChange; 179 | int mnMapChangeNotified; 180 | 181 | long unsigned int mnInitKFid; 182 | long unsigned int mnMaxKFid; 183 | long unsigned int mnLastLoopKFid; 184 | 185 | // Index related to a big change in the map (loop closure, global BA) 186 | int mnBigChangeIdx; 187 | 188 | 189 | // View of the map in aerial sight (for the AtlasViewer) 190 | GLubyte* mThumbnail; 191 | 192 | bool mIsInUse; 193 | bool mHasTumbnail; 194 | bool mbBad = false; 195 | 196 | bool mbIsInertial; 197 | bool mbIMU_BA1; 198 | bool mbIMU_BA2; 199 | 200 | std::mutex mMutexMap; 201 | }; 202 | 203 | } //namespace ORB_SLAM3 204 | 205 | #endif // MAP_H 206 | -------------------------------------------------------------------------------- /src/Converter.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #include "Converter.h" 20 | 21 | namespace ORB_SLAM3 22 | { 23 | 24 | std::vector Converter::toDescriptorVector(const cv::Mat &Descriptors) 25 | { 26 | std::vector vDesc; 27 | vDesc.reserve(Descriptors.rows); 28 | for (int j=0;j R; 37 | R << cvT.at(0,0), cvT.at(0,1), cvT.at(0,2), 38 | cvT.at(1,0), cvT.at(1,1), cvT.at(1,2), 39 | cvT.at(2,0), cvT.at(2,1), cvT.at(2,2); 40 | 41 | Eigen::Matrix t(cvT.at(0,3), cvT.at(1,3), cvT.at(2,3)); 42 | 43 | return g2o::SE3Quat(R,t); 44 | } 45 | 46 | cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3) 47 | { 48 | Eigen::Matrix eigMat = SE3.to_homogeneous_matrix(); 49 | return toCvMat(eigMat); 50 | } 51 | 52 | cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) 53 | { 54 | Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix(); 55 | Eigen::Vector3d eigt = Sim3.translation(); 56 | double s = Sim3.scale(); 57 | return toCvSE3(s*eigR,eigt); 58 | } 59 | 60 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 61 | { 62 | cv::Mat cvMat(4,4,CV_32F); 63 | for(int i=0;i<4;i++) 64 | for(int j=0; j<4; j++) 65 | cvMat.at(i,j)=m(i,j); 66 | 67 | return cvMat.clone(); 68 | } 69 | 70 | cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m) 71 | { 72 | cv::Mat cvMat(3,3,CV_32F); 73 | for(int i=0;i<3;i++) 74 | for(int j=0; j<3; j++) 75 | cvMat.at(i,j)=m(i,j); 76 | 77 | return cvMat.clone(); 78 | } 79 | 80 | cv::Mat Converter::toCvMat(const Eigen::MatrixXd &m) 81 | { 82 | cv::Mat cvMat(m.rows(),m.cols(),CV_32F); 83 | for(int i=0;i(i,j)=m(i,j); 86 | 87 | return cvMat.clone(); 88 | } 89 | 90 | cv::Mat Converter::toCvMat(const Eigen::Matrix &m) 91 | { 92 | cv::Mat cvMat(3,1,CV_32F); 93 | for(int i=0;i<3;i++) 94 | cvMat.at(i)=m(i); 95 | 96 | return cvMat.clone(); 97 | } 98 | 99 | cv::Mat Converter::toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t) 100 | { 101 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F); 102 | for(int i=0;i<3;i++) 103 | { 104 | for(int j=0;j<3;j++) 105 | { 106 | cvMat.at(i,j)=R(i,j); 107 | } 108 | } 109 | for(int i=0;i<3;i++) 110 | { 111 | cvMat.at(i,3)=t(i); 112 | } 113 | 114 | return cvMat.clone(); 115 | } 116 | 117 | Eigen::Matrix Converter::toVector3d(const cv::Mat &cvVector) 118 | { 119 | Eigen::Matrix v; 120 | v << cvVector.at(0), cvVector.at(1), cvVector.at(2); 121 | 122 | return v; 123 | } 124 | 125 | Eigen::Matrix Converter::toVector3d(const cv::Point3f &cvPoint) 126 | { 127 | Eigen::Matrix v; 128 | v << cvPoint.x, cvPoint.y, cvPoint.z; 129 | 130 | return v; 131 | } 132 | 133 | Eigen::Matrix Converter::toMatrix3d(const cv::Mat &cvMat3) 134 | { 135 | Eigen::Matrix M; 136 | 137 | M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), 138 | cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), 139 | cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); 140 | 141 | return M; 142 | } 143 | 144 | Eigen::Matrix Converter::toMatrix4d(const cv::Mat &cvMat4) 145 | { 146 | Eigen::Matrix M; 147 | 148 | M << cvMat4.at(0,0), cvMat4.at(0,1), cvMat4.at(0,2), cvMat4.at(0,3), 149 | cvMat4.at(1,0), cvMat4.at(1,1), cvMat4.at(1,2), cvMat4.at(1,3), 150 | cvMat4.at(2,0), cvMat4.at(2,1), cvMat4.at(2,2), cvMat4.at(2,3), 151 | cvMat4.at(3,0), cvMat4.at(3,1), cvMat4.at(3,2), cvMat4.at(3,3); 152 | return M; 153 | } 154 | 155 | 156 | std::vector Converter::toQuaternion(const cv::Mat &M) 157 | { 158 | Eigen::Matrix eigMat = toMatrix3d(M); 159 | Eigen::Quaterniond q(eigMat); 160 | 161 | std::vector v(4); 162 | v[0] = q.x(); 163 | v[1] = q.y(); 164 | v[2] = q.z(); 165 | v[3] = q.w(); 166 | 167 | return v; 168 | } 169 | 170 | cv::Mat Converter::tocvSkewMatrix(const cv::Mat &v) 171 | { 172 | return (cv::Mat_(3,3) << 0, -v.at(2), v.at(1), 173 | v.at(2), 0,-v.at(0), 174 | -v.at(1), v.at(0), 0); 175 | } 176 | 177 | bool Converter::isRotationMatrix(const cv::Mat &R) 178 | { 179 | cv::Mat Rt; 180 | cv::transpose(R, Rt); 181 | cv::Mat shouldBeIdentity = Rt * R; 182 | cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type()); 183 | 184 | return cv::norm(I, shouldBeIdentity) < 1e-6; 185 | 186 | } 187 | 188 | std::vector Converter::toEuler(const cv::Mat &R) 189 | { 190 | assert(isRotationMatrix(R)); 191 | float sy = sqrt(R.at(0,0) * R.at(0,0) + R.at(1,0) * R.at(1,0) ); 192 | 193 | bool singular = sy < 1e-6; // If 194 | 195 | float x, y, z; 196 | if (!singular) 197 | { 198 | x = atan2(R.at(2,1) , R.at(2,2)); 199 | y = atan2(-R.at(2,0), sy); 200 | z = atan2(R.at(1,0), R.at(0,0)); 201 | } 202 | else 203 | { 204 | x = atan2(-R.at(1,2), R.at(1,1)); 205 | y = atan2(-R.at(2,0), sy); 206 | z = 0; 207 | } 208 | 209 | std::vector v_euler(3); 210 | v_euler[0] = x; 211 | v_euler[1] = y; 212 | v_euler[2] = z; 213 | 214 | return v_euler; 215 | } 216 | 217 | } //namespace ORB_SLAM 218 | -------------------------------------------------------------------------------- /include/LoopClosing.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef LOOPCLOSING_H 21 | #define LOOPCLOSING_H 22 | 23 | #include "KeyFrame.h" 24 | #include "LocalMapping.h" 25 | #include "Atlas.h" 26 | #include "ORBVocabulary.h" 27 | #include "Tracking.h" 28 | 29 | #include "KeyFrameDatabase.h" 30 | 31 | #include 32 | #include 33 | #include 34 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 35 | 36 | namespace ORB_SLAM3 37 | { 38 | 39 | class Tracking; 40 | class LocalMapping; 41 | class KeyFrameDatabase; 42 | class Map; 43 | 44 | 45 | class LoopClosing 46 | { 47 | public: 48 | 49 | typedef pair,int> ConsistentGroup; 50 | typedef map, 51 | Eigen::aligned_allocator > > KeyFrameAndPose; 52 | 53 | public: 54 | 55 | LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale); 56 | 57 | void SetTracker(Tracking* pTracker); 58 | 59 | void SetLocalMapper(LocalMapping* pLocalMapper); 60 | 61 | // Main function 62 | void Run(); 63 | 64 | void InsertKeyFrame(KeyFrame *pKF); 65 | 66 | void RequestReset(); 67 | void RequestResetActiveMap(Map* pMap); 68 | 69 | // This function will run in a separate thread 70 | void RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF); 71 | 72 | bool isRunningGBA(){ 73 | unique_lock lock(mMutexGBA); 74 | return mbRunningGBA; 75 | } 76 | bool isFinishedGBA(){ 77 | unique_lock lock(mMutexGBA); 78 | return mbFinishedGBA; 79 | } 80 | 81 | void RequestFinish(); 82 | 83 | bool isFinished(); 84 | 85 | Viewer* mpViewer; 86 | 87 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 88 | 89 | protected: 90 | 91 | bool CheckNewKeyFrames(); 92 | 93 | 94 | //Methods to implement the new place recognition algorithm 95 | bool NewDetectCommonRegions(); 96 | bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, 97 | std::vector &vpMPs, std::vector &vpMatchedMPs); 98 | bool DetectCommonRegionsFromBoW(std::vector &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, 99 | int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs); 100 | bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, 101 | std::vector &vpMPs, std::vector &vpMatchedMPs); 102 | int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw, 103 | set &spMatchedMPinOrigin, vector &vpMapPoints, 104 | vector &vpMatchedMapPoints); 105 | 106 | 107 | void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector &vpMapPoints); 108 | void SearchAndFuse(const vector &vConectedKFs, vector &vpMapPoints); 109 | 110 | void CorrectLoop(); 111 | 112 | void MergeLocal(); 113 | void MergeLocal2(); 114 | 115 | void CheckObservations(set &spKFsMap1, set &spKFsMap2); 116 | void printReprojectionError(set &spLocalWindowKFs, KeyFrame* mpCurrentKF, string &name); 117 | 118 | void ResetIfRequested(); 119 | bool mbResetRequested; 120 | bool mbResetActiveMapRequested; 121 | Map* mpMapToReset; 122 | std::mutex mMutexReset; 123 | 124 | bool CheckFinish(); 125 | void SetFinish(); 126 | bool mbFinishRequested; 127 | bool mbFinished; 128 | std::mutex mMutexFinish; 129 | 130 | Atlas* mpAtlas; 131 | Tracking* mpTracker; 132 | 133 | KeyFrameDatabase* mpKeyFrameDB; 134 | ORBVocabulary* mpORBVocabulary; 135 | 136 | LocalMapping *mpLocalMapper; 137 | 138 | std::list mlpLoopKeyFrameQueue; 139 | 140 | std::mutex mMutexLoopQueue; 141 | 142 | // Loop detector parameters 143 | float mnCovisibilityConsistencyTh; 144 | 145 | // Loop detector variables 146 | KeyFrame* mpCurrentKF; 147 | KeyFrame* mpLastCurrentKF; 148 | KeyFrame* mpMatchedKF; 149 | std::vector mvConsistentGroups; 150 | std::vector mvpEnoughConsistentCandidates; 151 | std::vector mvpCurrentConnectedKFs; 152 | std::vector mvpCurrentMatchedPoints; 153 | std::vector mvpLoopMapPoints; 154 | cv::Mat mScw; 155 | g2o::Sim3 mg2oScw; 156 | 157 | //------- 158 | Map* mpLastMap; 159 | 160 | bool mbLoopDetected; 161 | int mnLoopNumCoincidences; 162 | int mnLoopNumNotFound; 163 | KeyFrame* mpLoopLastCurrentKF; 164 | g2o::Sim3 mg2oLoopSlw; 165 | g2o::Sim3 mg2oLoopScw; 166 | KeyFrame* mpLoopMatchedKF; 167 | std::vector mvpLoopMPs; 168 | std::vector mvpLoopMatchedMPs; 169 | bool mbMergeDetected; 170 | int mnMergeNumCoincidences; 171 | int mnMergeNumNotFound; 172 | KeyFrame* mpMergeLastCurrentKF; 173 | g2o::Sim3 mg2oMergeSlw; 174 | g2o::Sim3 mg2oMergeSmw; 175 | g2o::Sim3 mg2oMergeScw; 176 | KeyFrame* mpMergeMatchedKF; 177 | std::vector mvpMergeMPs; 178 | std::vector mvpMergeMatchedMPs; 179 | std::vector mvpMergeConnectedKFs; 180 | 181 | g2o::Sim3 mSold_new; 182 | //------- 183 | 184 | long unsigned int mLastLoopKFid; 185 | 186 | // Variables related to Global Bundle Adjustment 187 | bool mbRunningGBA; 188 | bool mbFinishedGBA; 189 | bool mbStopGBA; 190 | std::mutex mMutexGBA; 191 | std::thread* mpThreadGBA; 192 | 193 | // Fix scale in the stereo/RGB-D case 194 | bool mbFixScale; 195 | 196 | 197 | bool mnFullBAIdx; 198 | 199 | 200 | 201 | vector vdPR_CurrentTime; 202 | vector vdPR_MatchedTime; 203 | vector vnPR_TypeRecogn; 204 | }; 205 | 206 | } //namespace ORB_SLAM 207 | 208 | #endif // LOOPCLOSING_H 209 | -------------------------------------------------------------------------------- /src/CameraModels/Pinhole.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #include "Pinhole.h" 20 | 21 | #include 22 | 23 | //BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole) 24 | 25 | namespace ORB_SLAM3 { 26 | //BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") 27 | 28 | long unsigned int GeometricCamera::nNextId=0; 29 | 30 | cv::Point2f Pinhole::project(const cv::Point3f &p3D) { 31 | return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2], 32 | mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); 33 | } 34 | 35 | cv::Point2f Pinhole::project(const cv::Mat &m3D) { 36 | const float* p3D = m3D.ptr(); 37 | 38 | return this->project(cv::Point3f(p3D[0],p3D[1],p3D[2])); 39 | } 40 | 41 | Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) { 42 | Eigen::Vector2d res; 43 | res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; 44 | res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; 45 | 46 | return res; 47 | } 48 | 49 | cv::Mat Pinhole::projectMat(const cv::Point3f &p3D) { 50 | cv::Point2f point = this->project(p3D); 51 | return (cv::Mat_(2,1) << point.x, point.y); 52 | } 53 | 54 | float Pinhole::uncertainty2(const Eigen::Matrix &p2D) 55 | { 56 | return 1.0; 57 | } 58 | 59 | cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) { 60 | return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], 61 | 1.f); 62 | } 63 | 64 | cv::Mat Pinhole::unprojectMat(const cv::Point2f &p2D){ 65 | cv::Point3f ray = this->unproject(p2D); 66 | return (cv::Mat_(3,1) << ray.x, ray.y, ray.z); 67 | } 68 | 69 | cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) { 70 | cv::Mat Jac(2, 3, CV_32F); 71 | Jac.at(0, 0) = mvParameters[0] / p3D.z; 72 | Jac.at(0, 1) = 0.f; 73 | Jac.at(0, 2) = -mvParameters[0] * p3D.x / (p3D.z * p3D.z); 74 | Jac.at(1, 0) = 0.f; 75 | Jac.at(1, 1) = mvParameters[1] / p3D.z; 76 | Jac.at(1, 2) = -mvParameters[1] * p3D.y / (p3D.z * p3D.z); 77 | 78 | return Jac; 79 | } 80 | 81 | Eigen::Matrix Pinhole::projectJac(const Eigen::Vector3d &v3D) { 82 | Eigen::Matrix Jac; 83 | Jac(0, 0) = mvParameters[0] / v3D[2]; 84 | Jac(0, 1) = 0.f; 85 | Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]); 86 | Jac(1, 0) = 0.f; 87 | Jac(1, 1) = mvParameters[1] / v3D[2]; 88 | Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]); 89 | 90 | return Jac; 91 | } 92 | 93 | cv::Mat Pinhole::unprojectJac(const cv::Point2f &p2D) { 94 | cv::Mat Jac(3, 2, CV_32F); 95 | Jac.at(0, 0) = 1 / mvParameters[0]; 96 | Jac.at(0, 1) = 0.f; 97 | Jac.at(1, 0) = 0.f; 98 | Jac.at(1, 1) = 1 / mvParameters[1]; 99 | Jac.at(2, 0) = 0.f; 100 | Jac.at(2, 1) = 0.f; 101 | 102 | return Jac; 103 | } 104 | 105 | /** 106 | * 2d-2d重建结构，得到R、t、三角化点 107 | */ 108 | bool Pinhole::ReconstructWithTwoViews(const std::vector& vKeys1, const std::vector& vKeys2, const std::vector &vMatches12, 109 | cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated){ 110 | if(!tvr){ 111 | cv::Mat K = this->toK(); 112 | tvr = new TwoViewReconstruction(K); 113 | } 114 | /** 115 | * 2d-2d重建结构 116 | * 1、准备好8对点ransac的数据 117 | * 2、开辟两个线程，分别计算单应矩阵H、基础矩阵F 118 | * 3、从H、F中选择打分较高的来恢复R、t，三角化 119 | */ 120 | return tvr->Reconstruct(vKeys1,vKeys2,vMatches12,R21,t21,vP3D,vbTriangulated); 121 | } 122 | 123 | /** 124 | * 内参K 125 | */ 126 | cv::Mat Pinhole::toK() { 127 | cv::Mat K = (cv::Mat_(3, 3) 128 | << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f); 129 | return K; 130 | } 131 | 132 | bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) { 133 | //Compute Fundamental Matrix 134 | cv::Mat t12x = SkewSymmetricMatrix(t12); 135 | cv::Mat K1 = this->toK(); 136 | cv::Mat K2 = pCamera2->toK(); 137 | cv::Mat F12 = K1.t().inv()*t12x*R12*K2.inv(); 138 | 139 | // Epipolar line in second image l = x1'F12 = [a b c] 140 | const float a = kp1.pt.x*F12.at(0,0)+kp1.pt.y*F12.at(1,0)+F12.at(2,0); 141 | const float b = kp1.pt.x*F12.at(0,1)+kp1.pt.y*F12.at(1,1)+F12.at(2,1); 142 | const float c = kp1.pt.x*F12.at(0,2)+kp1.pt.y*F12.at(1,2)+F12.at(2,2); 143 | 144 | const float num = a*kp2.pt.x+b*kp2.pt.y+c; 145 | 146 | const float den = a*a+b*b; 147 | 148 | if(den==0) 149 | return false; 150 | 151 | const float dsqr = num*num/den; 152 | 153 | return dsqr<3.84*unc; 154 | } 155 | 156 | std::ostream & operator<<(std::ostream &os, const Pinhole &ph) { 157 | os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3]; 158 | return os; 159 | } 160 | 161 | std::istream & operator>>(std::istream &is, Pinhole &ph) { 162 | float nextParam; 163 | for(size_t i = 0; i < 4; i++){ 164 | assert(is.good()); //Make sure the input stream is good 165 | is >> nextParam; 166 | ph.mvParameters[i] = nextParam; 167 | 168 | } 169 | return is; 170 | } 171 | 172 | cv::Mat Pinhole::SkewSymmetricMatrix(const cv::Mat &v) 173 | { 174 | return (cv::Mat_(3,3) << 0, -v.at(2), v.at(1), 175 | v.at(2), 0,-v.at(0), 176 | -v.at(1), v.at(0), 0); 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /include/PnPsolver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | /** 20 | * Copyright (c) 2009, V. Lepetit, EPFL 21 | * All rights reserved. 22 | * 23 | * Redistribution and use in source and binary forms, with or without 24 | * modification, are permitted provided that the following conditions are met: 25 | * 26 | * 1. Redistributions of source code must retain the above copyright notice, this 27 | * list of conditions and the following disclaimer. 28 | * 2. Redistributions in binary form must reproduce the above copyright notice, 29 | * this list of conditions and the following disclaimer in the documentation 30 | * and/or other materials provided with the distribution. 31 | * 32 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 33 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 34 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 35 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 36 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 37 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 38 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 39 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 40 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 41 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 42 | * 43 | * The views and conclusions contained in the software and documentation are those 44 | * of the authors and should not be interpreted as representing official policies, 45 | * either expressed or implied, of the FreeBSD Project 46 | */ 47 | 48 | #ifndef PNPSOLVER_H 49 | #define PNPSOLVER_H 50 | 51 | #include 52 | #include "MapPoint.h" 53 | #include "Frame.h" 54 | 55 | namespace ORB_SLAM3 56 | { 57 | 58 | class PnPsolver { 59 | public: 60 | PnPsolver(const Frame &F, const vector &vpMapPointMatches); 61 | 62 | ~PnPsolver(); 63 | 64 | void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4, 65 | float th2 = 5.991); 66 | 67 | cv::Mat find(vector &vbInliers, int &nInliers); 68 | 69 | cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); 70 | 71 | private: 72 | 73 | void CheckInliers(); 74 | bool Refine(); 75 | 76 | // Functions from the original EPnP code 77 | void set_maximum_number_of_correspondences(const int n); 78 | void reset_correspondences(void); 79 | void add_correspondence(const double X, const double Y, const double Z, 80 | const double u, const double v); 81 | 82 | double compute_pose(double R[3][3], double T[3]); 83 | 84 | void relative_error(double & rot_err, double & transl_err, 85 | const double Rtrue[3][3], const double ttrue[3], 86 | const double Rest[3][3], const double test[3]); 87 | 88 | void print_pose(const double R[3][3], const double t[3]); 89 | double reprojection_error(const double R[3][3], const double t[3]); 90 | 91 | void choose_control_points(void); 92 | void compute_barycentric_coordinates(void); 93 | void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); 94 | void compute_ccs(const double * betas, const double * ut); 95 | void compute_pcs(void); 96 | 97 | void solve_for_sign(void); 98 | 99 | void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); 100 | void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); 101 | void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); 102 | void qr_solve(CvMat * A, CvMat * b, CvMat * X); 103 | 104 | double dot(const double * v1, const double * v2); 105 | double dist2(const double * p1, const double * p2); 106 | 107 | void compute_rho(double * rho); 108 | void compute_L_6x10(const double * ut, double * l_6x10); 109 | 110 | void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); 111 | void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, 112 | double cb[4], CvMat * A, CvMat * b); 113 | 114 | double compute_R_and_t(const double * ut, const double * betas, 115 | double R[3][3], double t[3]); 116 | 117 | void estimate_R_and_t(double R[3][3], double t[3]); 118 | 119 | void copy_R_and_t(const double R_dst[3][3], const double t_dst[3], 120 | double R_src[3][3], double t_src[3]); 121 | 122 | void mat_to_quat(const double R[3][3], double q[4]); 123 | 124 | 125 | double uc, vc, fu, fv; 126 | 127 | double * pws, * us, * alphas, * pcs; 128 | int maximum_number_of_correspondences; 129 | int number_of_correspondences; 130 | 131 | double cws[4][3], ccs[4][3]; 132 | double cws_determinant; 133 | 134 | vector mvpMapPointMatches; 135 | 136 | // 2D Points 137 | vector mvP2D; 138 | vector mvSigma2; 139 | 140 | // 3D Points 141 | vector mvP3Dw; 142 | 143 | // Index in Frame 144 | vector mvKeyPointIndices; 145 | 146 | // Current Estimation 147 | double mRi[3][3]; 148 | double mti[3]; 149 | cv::Mat mTcwi; 150 | vector mvbInliersi; 151 | int mnInliersi; 152 | 153 | // Current Ransac State 154 | int mnIterations; 155 | vector mvbBestInliers; 156 | int mnBestInliers; 157 | cv::Mat mBestTcw; 158 | 159 | // Refined 160 | cv::Mat mRefinedTcw; 161 | vector mvbRefinedInliers; 162 | int mnRefinedInliers; 163 | 164 | // Number of Correspondences 165 | int N; 166 | 167 | // Indices for random selection [0 .. N-1] 168 | vector mvAllIndices; 169 | 170 | // RANSAC probability 171 | double mRansacProb; 172 | 173 | // RANSAC min inliers 174 | int mRansacMinInliers; 175 | 176 | // RANSAC max iterations 177 | int mRansacMaxIts; 178 | 179 | // RANSAC expected inliers/total ratio 180 | float mRansacEpsilon; 181 | 182 | // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 183 | float mRansacTh; 184 | 185 | // RANSAC Minimun Set used at each iteration 186 | int mRansacMinSet; 187 | 188 | // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) 189 | vector mvMaxError; 190 | 191 | }; 192 | 193 | } //namespace ORB_SLAM 194 | 195 | #endif //PNPSOLVER_H 196 | -------------------------------------------------------------------------------- /include/Optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef OPTIMIZER_H 21 | #define OPTIMIZER_H 22 | 23 | #include "Map.h" 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include "LoopClosing.h" 27 | #include "Frame.h" 28 | 29 | #include 30 | 31 | #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" 32 | #include "Thirdparty/g2o/g2o/core/sparse_block_matrix.h" 33 | #include "Thirdparty/g2o/g2o/core/block_solver.h" 34 | #include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" 35 | #include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h" 36 | #include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" 37 | #include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" 38 | #include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h" 39 | #include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h" 40 | 41 | namespace ORB_SLAM3 42 | { 43 | 44 | class LoopClosing; 45 | 46 | class Optimizer 47 | { 48 | public: 49 | 50 | void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, 51 | int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0, 52 | const bool bRobust = true); 53 | void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL, 54 | const unsigned long nLoopKF=0, const bool bRobust = true); 55 | void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKF=0, bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL); 56 | 57 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, vector &vpNonEnoughOptKFs); 58 | void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF); 59 | 60 | void static MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vector vpWeldingKFs, vector vpFixedKFs, bool *pbStopFlag); 61 | 62 | int static PoseOptimization(Frame* pFrame); 63 | 64 | int static PoseInertialOptimizationLastKeyFrame(Frame* pFrame, bool bRecInit = false); 65 | int static PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit = false); 66 | 67 | // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono) 68 | void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 69 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 70 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 71 | const map > &LoopConnections, 72 | const bool &bFixScale); 73 | void static OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, 74 | vector &vpNonFixedKFs, vector &vpNonCorrectedMPs, double scale); 75 | void static OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, 76 | vector &vpNonFixedKFs, vector &vpNonCorrectedMPs); 77 | void static OptimizeEssentialGraph(KeyFrame* pCurKF, 78 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 79 | const LoopClosing::KeyFrameAndPose &CorrectedSim3); 80 | // For inetial loopclosing 81 | void static OptimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, 82 | const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, 83 | const LoopClosing::KeyFrameAndPose &CorrectedSim3, 84 | const map > &LoopConnections); 85 | 86 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) (OLD) 87 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 88 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale); 89 | // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono) (NEW) 90 | static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, 91 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale, 92 | Eigen::Matrix &mAcumHessian, const bool bAllPoints=false); 93 | static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, vector &vpMatches1KF, 94 | g2o::Sim3 &g2oS12, const float th2, const bool bFixScale, Eigen::Matrix &mAcumHessian, 95 | const bool bAllPoints = false); 96 | 97 | // For inertial systems 98 | 99 | void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, bool bLarge = false, bool bRecInit = false); 100 | 101 | void static MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses); 102 | 103 | // Local BA in welding area when two maps are merged 104 | void static LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag); 105 | 106 | // Marginalize block element (start:end,start:end). Perform Schur complement. 107 | // Marginalized elements are filled with zeros. 108 | static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end); 109 | // Condition block element (start:end,start:end). Fill with zeros. 110 | static Eigen::MatrixXd Condition(const Eigen::MatrixXd &H, const int &start, const int &end); 111 | // Remove link between element 1 and 2. Given elements 1,2 and 3 must define the whole matrix. 112 | static Eigen::MatrixXd Sparsify(const Eigen::MatrixXd &H, const int &start1, const int &end1, const int &start2, const int &end2); 113 | 114 | // Inertial pose-graph 115 | void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel=false, bool bGauss=false, float priorG = 1e2, float priorA = 1e6); 116 | void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6); 117 | void static InertialOptimization(vector vpKFs, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6); 118 | void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale); 119 | }; 120 | 121 | } //namespace ORB_SLAM3 122 | 123 | #endif // OPTIMIZER_H 124 | -------------------------------------------------------------------------------- /include/OptimizableTypes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef ORB_SLAM3_OPTIMIZABLETYPES_H 20 | #define ORB_SLAM3_OPTIMIZABLETYPES_H 21 | 22 | #include "Thirdparty/g2o/g2o/core/base_unary_edge.h" 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | 30 | namespace ORB_SLAM3 { 31 | class EdgeSE3ProjectXYZOnlyPose: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap>{ 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | EdgeSE3ProjectXYZOnlyPose(){} 36 | 37 | bool read(std::istream& is); 38 | 39 | bool write(std::ostream& os) const; 40 | 41 | void computeError() { 42 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[0]); 43 | Eigen::Vector2d obs(_measurement); 44 | _error = obs-pCamera->project(v1->estimate().map(Xw)); 45 | } 46 | 47 | bool isDepthPositive() { 48 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[0]); 49 | return (v1->estimate().map(Xw))(2)>0.0; 50 | } 51 | 52 | 53 | virtual void linearizeOplus(); 54 | 55 | Eigen::Vector3d Xw; 56 | GeometricCamera* pCamera; 57 | }; 58 | 59 | class EdgeSE3ProjectXYZOnlyPoseToBody: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap>{ 60 | public: 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | 63 | EdgeSE3ProjectXYZOnlyPoseToBody(){} 64 | 65 | bool read(std::istream& is); 66 | 67 | bool write(std::ostream& os) const; 68 | 69 | void computeError() { 70 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[0]); 71 | Eigen::Vector2d obs(_measurement); 72 | _error = obs-pCamera->project((mTrl * v1->estimate()).map(Xw)); 73 | } 74 | 75 | bool isDepthPositive() { 76 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[0]); 77 | return ((mTrl * v1->estimate()).map(Xw))(2)>0.0; 78 | } 79 | 80 | 81 | virtual void linearizeOplus(); 82 | 83 | Eigen::Vector3d Xw; 84 | GeometricCamera* pCamera; 85 | 86 | g2o::SE3Quat mTrl; 87 | }; 88 | 89 | class EdgeSE3ProjectXYZ: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>{ 90 | public: 91 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 92 | 93 | EdgeSE3ProjectXYZ(); 94 | 95 | bool read(std::istream& is); 96 | 97 | bool write(std::ostream& os) const; 98 | 99 | void computeError() { 100 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[1]); 101 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 102 | Eigen::Vector2d obs(_measurement); 103 | _error = obs-pCamera->project(v1->estimate().map(v2->estimate())); 104 | } 105 | 106 | bool isDepthPositive() { 107 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[1]); 108 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 109 | return ((v1->estimate().map(v2->estimate()))(2)>0.0); 110 | } 111 | 112 | virtual void linearizeOplus(); 113 | 114 | GeometricCamera* pCamera; 115 | }; 116 | 117 | class EdgeSE3ProjectXYZToBody: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>{ 118 | public: 119 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 120 | 121 | EdgeSE3ProjectXYZToBody(); 122 | 123 | bool read(std::istream& is); 124 | 125 | bool write(std::ostream& os) const; 126 | 127 | void computeError() { 128 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[1]); 129 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 130 | Eigen::Vector2d obs(_measurement); 131 | _error = obs-pCamera->project((mTrl * v1->estimate()).map(v2->estimate())); 132 | } 133 | 134 | bool isDepthPositive() { 135 | const g2o::VertexSE3Expmap* v1 = static_cast(_vertices[1]); 136 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 137 | return ((mTrl * v1->estimate()).map(v2->estimate()))(2)>0.0; 138 | } 139 | 140 | virtual void linearizeOplus(); 141 | 142 | GeometricCamera* pCamera; 143 | g2o::SE3Quat mTrl; 144 | }; 145 | 146 | class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> 147 | { 148 | public: 149 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 150 | VertexSim3Expmap(); 151 | virtual bool read(std::istream& is); 152 | virtual bool write(std::ostream& os) const; 153 | 154 | virtual void setToOriginImpl() { 155 | _estimate = g2o::Sim3(); 156 | } 157 | 158 | virtual void oplusImpl(const double* update_) 159 | { 160 | Eigen::Map update(const_cast(update_)); 161 | 162 | if (_fix_scale) 163 | update[6] = 0; 164 | 165 | g2o::Sim3 s(update); 166 | setEstimate(s*estimate()); 167 | } 168 | 169 | GeometricCamera* pCamera1, *pCamera2; 170 | 171 | bool _fix_scale; 172 | }; 173 | 174 | 175 | class EdgeSim3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, ORB_SLAM3::VertexSim3Expmap> 176 | { 177 | public: 178 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 179 | EdgeSim3ProjectXYZ(); 180 | virtual bool read(std::istream& is); 181 | virtual bool write(std::ostream& os) const; 182 | 183 | void computeError() 184 | { 185 | const ORB_SLAM3::VertexSim3Expmap* v1 = static_cast(_vertices[1]); 186 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 187 | 188 | Eigen::Vector2d obs(_measurement); 189 | _error = obs-v1->pCamera1->project(v1->estimate().map(v2->estimate())); 190 | } 191 | 192 | // virtual void linearizeOplus(); 193 | 194 | }; 195 | 196 | class EdgeInverseSim3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap> 197 | { 198 | public: 199 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 200 | EdgeInverseSim3ProjectXYZ(); 201 | virtual bool read(std::istream& is); 202 | virtual bool write(std::ostream& os) const; 203 | 204 | void computeError() 205 | { 206 | const ORB_SLAM3::VertexSim3Expmap* v1 = static_cast(_vertices[1]); 207 | const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); 208 | 209 | Eigen::Vector2d obs(_measurement); 210 | _error = obs-v1->pCamera2->project((v1->estimate().inverse().map(v2->estimate()))); 211 | } 212 | 213 | // virtual void linearizeOplus(); 214 | 215 | }; 216 | 217 | } 218 | 219 | #endif //ORB_SLAM3_OPTIMIZABLETYPES_H 220 | -------------------------------------------------------------------------------- /include/System.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef SYSTEM_H 21 | #define SYSTEM_H 22 | 23 | //#define SAVE_TIMES 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "Tracking.h" 33 | #include "FrameDrawer.h" 34 | #include "MapDrawer.h" 35 | #include "Atlas.h" 36 | #include "LocalMapping.h" 37 | #include "LoopClosing.h" 38 | #include "KeyFrameDatabase.h" 39 | #include "ORBVocabulary.h" 40 | #include "Viewer.h" 41 | #include "ImuTypes.h" 42 | 43 | 44 | namespace ORB_SLAM3 45 | { 46 | 47 | class Verbose 48 | { 49 | public: 50 | enum eLevel 51 | { 52 | VERBOSITY_QUIET=0, 53 | VERBOSITY_NORMAL=1, 54 | VERBOSITY_VERBOSE=2, 55 | VERBOSITY_VERY_VERBOSE=3, 56 | VERBOSITY_DEBUG=4 57 | }; 58 | 59 | static eLevel th; 60 | 61 | public: 62 | static void PrintMess(std::string str, eLevel lev) 63 | { 64 | if(lev <= th) 65 | cout << str << endl; 66 | } 67 | 68 | static void SetTh(eLevel _th) 69 | { 70 | th = _th; 71 | } 72 | }; 73 | 74 | class Viewer; 75 | class FrameDrawer; 76 | class Atlas; 77 | class Tracking; 78 | class LocalMapping; 79 | class LoopClosing; 80 | 81 | class System 82 | { 83 | public: 84 | // Input sensor 85 | enum eSensor{ 86 | MONOCULAR=0, 87 | STEREO=1, 88 | RGBD=2, 89 | IMU_MONOCULAR=3, 90 | IMU_STEREO=4 91 | }; 92 | 93 | // File type 94 | enum eFileType{ 95 | TEXT_FILE=0, 96 | BINARY_FILE=1, 97 | }; 98 | 99 | public: 100 | 101 | // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. 102 | System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string(), const string &strLoadingFile = std::string()); 103 | 104 | // Proccess the given stereo frame. Images must be synchronized and rectified. 105 | // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 106 | // Returns the camera pose (empty if tracking fails). 107 | cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); 108 | 109 | // Process the given rgbd frame. Depthmap must be registered to the RGB frame. 110 | // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 111 | // Input depthmap: Float (CV_32F). 112 | // Returns the camera pose (empty if tracking fails). 113 | cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, string filename=""); 114 | 115 | // Proccess the given monocular frame and optionally imu data 116 | // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. 117 | // Returns the camera pose (empty if tracking fails). 118 | cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp, const vector& vImuMeas = vector(), string filename=""); 119 | 120 | 121 | // This stops local mapping thread (map building) and performs only camera tracking. 122 | void ActivateLocalizationMode(); 123 | // This resumes local mapping thread and performs SLAM again. 124 | void DeactivateLocalizationMode(); 125 | 126 | // Returns true if there have been a big map change (loop closure, global BA) 127 | // since last call to this function 128 | bool MapChanged(); 129 | 130 | // Reset the system (clear Atlas or the active map) 131 | void Reset(); 132 | void ResetActiveMap(); 133 | 134 | // All threads will be requested to finish. 135 | // It waits until all threads have finished. 136 | // This function must be called before saving the trajectory. 137 | void Shutdown(); 138 | 139 | // Save camera trajectory in the TUM RGB-D dataset format. 140 | // Only for stereo and RGB-D. This method does not work for monocular. 141 | // Call first Shutdown() 142 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 143 | void SaveTrajectoryTUM(const string &filename); 144 | 145 | // Save keyframe poses in the TUM RGB-D dataset format. 146 | // This method works for all sensor input. 147 | // Call first Shutdown() 148 | // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset 149 | void SaveKeyFrameTrajectoryTUM(const string &filename); 150 | 151 | void SaveTrajectoryEuRoC(const string &filename); 152 | void SaveKeyFrameTrajectoryEuRoC(const string &filename); 153 | 154 | // Save data used for initialization debug 155 | void SaveDebugData(const int &iniIdx); 156 | 157 | // Save camera trajectory in the KITTI dataset format. 158 | // Only for stereo and RGB-D. This method does not work for monocular. 159 | // Call first Shutdown() 160 | // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php 161 | void SaveTrajectoryKITTI(const string &filename); 162 | 163 | // TODO: Save/Load functions 164 | // SaveMap(const string &filename); 165 | // LoadMap(const string &filename); 166 | 167 | // Information from most recent processed frame 168 | // You can call this right after TrackMonocular (or stereo or RGBD) 169 | int GetTrackingState(); 170 | std::vector GetTrackedMapPoints(); 171 | std::vector GetTrackedKeyPointsUn(); 172 | 173 | // For debugging 174 | double GetTimeFromIMUInit(); 175 | bool isLost(); 176 | bool isFinished(); 177 | 178 | void ChangeDataset(); 179 | 180 | //void SaveAtlas(int type); 181 | 182 | private: 183 | 184 | //bool LoadAtlas(string filename, int type); 185 | 186 | //string CalculateCheckSum(string filename, int type); 187 | 188 | // Input sensor 189 | eSensor mSensor; 190 | 191 | // ORB vocabulary used for place recognition and feature matching. 192 | ORBVocabulary* mpVocabulary; 193 | 194 | // KeyFrame database for place recognition (relocalization and loop detection). 195 | KeyFrameDatabase* mpKeyFrameDatabase; 196 | 197 | // Map structure that stores the pointers to all KeyFrames and MapPoints. 198 | //Map* mpMap; 199 | Atlas* mpAtlas; 200 | 201 | // Tracker. It receives a frame and computes the associated camera pose. 202 | // It also decides when to insert a new keyframe, create some new MapPoints and 203 | // performs relocalization if tracking fails. 204 | Tracking* mpTracker; 205 | 206 | // Local Mapper. It manages the local map and performs local bundle adjustment. 207 | LocalMapping* mpLocalMapper; 208 | 209 | // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs 210 | // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. 211 | LoopClosing* mpLoopCloser; 212 | 213 | // The viewer draws the map and the current camera pose. It uses Pangolin. 214 | Viewer* mpViewer; 215 | 216 | FrameDrawer* mpFrameDrawer; 217 | MapDrawer* mpMapDrawer; 218 | 219 | // System threads: Local Mapping, Loop Closing, Viewer. 220 | // The Tracking thread "lives" in the main execution thread that creates the System object. 221 | std::thread* mptLocalMapping; 222 | std::thread* mptLoopClosing; 223 | std::thread* mptViewer; 224 | 225 | // Reset flag 226 | std::mutex mMutexReset; 227 | bool mbReset; 228 | bool mbResetActiveMap; 229 | 230 | // Change mode flags 231 | std::mutex mMutexMode; 232 | bool mbActivateLocalizationMode; 233 | bool mbDeactivateLocalizationMode; 234 | 235 | // Tracking state 236 | int mTrackingState; 237 | std::vector mTrackedMapPoints; 238 | std::vector mTrackedKeyPointsUn; 239 | std::mutex mMutexState; 240 | }; 241 | 242 | }// namespace ORB_SLAM 243 | 244 | #endif // SYSTEM_H 245 | -------------------------------------------------------------------------------- /include/MapPoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef MAPPOINT_H 21 | #define MAPPOINT_H 22 | 23 | #include"KeyFrame.h" 24 | #include"Frame.h" 25 | #include"Map.h" 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace ORB_SLAM3 35 | { 36 | 37 | class KeyFrame; 38 | class Map; 39 | class Frame; 40 | 41 | class MapPoint 42 | { 43 | template 44 | void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version) 45 | { 46 | int cols, rows, type; 47 | bool continuous; 48 | 49 | if (Archive::is_saving::value) { 50 | cols = mat.cols; rows = mat.rows; type = mat.type(); 51 | continuous = mat.isContinuous(); 52 | } 53 | 54 | ar & cols & rows & type & continuous; 55 | if (Archive::is_loading::value) 56 | mat.create(rows, cols, type); 57 | 58 | if (continuous) { 59 | const unsigned int data_size = rows * cols * mat.elemSize(); 60 | ar & boost::serialization::make_array(mat.ptr(), data_size); 61 | } else { 62 | const unsigned int row_size = cols*mat.elemSize(); 63 | for (int i = 0; i < rows; i++) { 64 | ar & boost::serialization::make_array(mat.ptr(i), row_size); 65 | } 66 | } 67 | } 68 | 69 | friend class boost::serialization::access; 70 | template 71 | void serialize(Archive & ar, const unsigned int version) 72 | { 73 | ar & mnId; 74 | ar & mnFirstKFid; 75 | ar & mnFirstFrame; 76 | ar & nObs; 77 | // Variables used by the tracking 78 | ar & mTrackProjX; 79 | ar & mTrackProjY; 80 | ar & mTrackDepth; 81 | ar & mTrackDepthR; 82 | ar & mTrackProjXR; 83 | ar & mTrackProjYR; 84 | ar & mbTrackInView; 85 | ar & mbTrackInViewR; 86 | ar & mnTrackScaleLevel; 87 | ar & mnTrackScaleLevelR; 88 | ar & mTrackViewCos; 89 | ar & mTrackViewCosR; 90 | ar & mnTrackReferenceForFrame; 91 | ar & mnLastFrameSeen; 92 | 93 | // Variables used by local mapping 94 | ar & mnBALocalForKF; 95 | ar & mnFuseCandidateForKF; 96 | 97 | // Variables used by loop closing and merging 98 | ar & mnLoopPointForKF; 99 | ar & mnCorrectedByKF; 100 | ar & mnCorrectedReference; 101 | serializeMatrix(ar,mPosGBA,version); 102 | ar & mnBAGlobalForKF; 103 | ar & mnBALocalForMerge; 104 | serializeMatrix(ar,mPosMerge,version); 105 | serializeMatrix(ar,mNormalVectorMerge,version); 106 | 107 | // Protected variables 108 | serializeMatrix(ar,mWorldPos,version); 109 | //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId); 110 | ar & mBackupObservationsId1; 111 | ar & mBackupObservationsId2; 112 | serializeMatrix(ar,mNormalVector,version); 113 | serializeMatrix(ar,mDescriptor,version); 114 | ar & mBackupRefKFId; 115 | ar & mnVisible; 116 | ar & mnFound; 117 | 118 | ar & mbBad; 119 | ar & mBackupReplacedId; 120 | 121 | ar & mfMinDistance; 122 | ar & mfMaxDistance; 123 | 124 | } 125 | 126 | 127 | public: 128 | MapPoint(); 129 | 130 | MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); 131 | MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap); 132 | MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF); 133 | 134 | void SetWorldPos(const cv::Mat &Pos); 135 | 136 | cv::Mat GetWorldPos(); 137 | 138 | cv::Mat GetNormal(); 139 | KeyFrame* GetReferenceKeyFrame(); 140 | 141 | std::map> GetObservations(); 142 | int Observations(); 143 | 144 | void AddObservation(KeyFrame* pKF,int idx); 145 | void EraseObservation(KeyFrame* pKF); 146 | 147 | std::tuple GetIndexInKeyFrame(KeyFrame* pKF); 148 | bool IsInKeyFrame(KeyFrame* pKF); 149 | 150 | void SetBadFlag(); 151 | bool isBad(); 152 | 153 | void Replace(MapPoint* pMP); 154 | MapPoint* GetReplaced(); 155 | 156 | void IncreaseVisible(int n=1); 157 | void IncreaseFound(int n=1); 158 | float GetFoundRatio(); 159 | inline int GetFound(){ 160 | return mnFound; 161 | } 162 | 163 | void ComputeDistinctiveDescriptors(); 164 | 165 | cv::Mat GetDescriptor(); 166 | 167 | void UpdateNormalAndDepth(); 168 | void SetNormalVector(cv::Mat& normal); 169 | 170 | float GetMinDistanceInvariance(); 171 | float GetMaxDistanceInvariance(); 172 | int PredictScale(const float ¤tDist, KeyFrame*pKF); 173 | int PredictScale(const float ¤tDist, Frame* pF); 174 | 175 | Map* GetMap(); 176 | void UpdateMap(Map* pMap); 177 | 178 | void PrintObservations(); 179 | 180 | void PreSave(set& spKF,set& spMP); 181 | void PostLoad(map& mpKFid, map& mpMPid); 182 | 183 | public: 184 | long unsigned int mnId; 185 | static long unsigned int nNextId; 186 | long int mnFirstKFid; 187 | long int mnFirstFrame; 188 | // 观测数量 189 | int nObs; 190 | 191 | // Variables used by the tracking 192 | float mTrackProjX; 193 | float mTrackProjY; 194 | float mTrackDepth; 195 | float mTrackDepthR; 196 | float mTrackProjXR; 197 | float mTrackProjYR; 198 | bool mbTrackInView, mbTrackInViewR; 199 | int mnTrackScaleLevel, mnTrackScaleLevelR; 200 | float mTrackViewCos, mTrackViewCosR; 201 | long unsigned int mnTrackReferenceForFrame; 202 | long unsigned int mnLastFrameSeen; 203 | 204 | // Variables used by local mapping 205 | long unsigned int mnBALocalForKF; 206 | long unsigned int mnFuseCandidateForKF; 207 | 208 | // Variables used by loop closing 209 | long unsigned int mnLoopPointForKF; 210 | long unsigned int mnCorrectedByKF; 211 | long unsigned int mnCorrectedReference; 212 | cv::Mat mPosGBA; 213 | long unsigned int mnBAGlobalForKF; 214 | long unsigned int mnBALocalForMerge; 215 | 216 | // Variable used by merging 217 | cv::Mat mPosMerge; 218 | cv::Mat mNormalVectorMerge; 219 | 220 | 221 | // Fopr inverse depth optimization 222 | double mInvDepth; 223 | double mInitU; 224 | double mInitV; 225 | KeyFrame* mpHostKF; 226 | 227 | static std::mutex mGlobalMutex; 228 | 229 | unsigned int mnOriginMapId; 230 | 231 | protected: 232 | 233 | // Position in absolute coordinates 234 | cv::Mat mWorldPos; 235 | 236 | // Keyframes observing the point and associated index in keyframe 237 | std::map > mObservations; 238 | // For save relation without pointer, this is necessary for save/load function 239 | std::map mBackupObservationsId1; 240 | std::map mBackupObservationsId2; 241 | 242 | // Mean viewing direction 243 | cv::Mat mNormalVector; 244 | 245 | // Best descriptor to fast matching 246 | cv::Mat mDescriptor; 247 | 248 | // Reference KeyFrame 249 | KeyFrame* mpRefKF; 250 | long unsigned int mBackupRefKFId; 251 | 252 | // Tracking counters 253 | int mnVisible; 254 | int mnFound; 255 | 256 | // Bad flag (we do not currently erase MapPoint from memory) 257 | bool mbBad; 258 | MapPoint* mpReplaced; 259 | // For save relation without pointer, this is necessary for save/load function 260 | long long int mBackupReplacedId; 261 | 262 | // Scale invariance distances 263 | float mfMinDistance; 264 | float mfMaxDistance; 265 | 266 | Map* mpMap; 267 | 268 | std::mutex mMutexPos; 269 | std::mutex mMutexFeatures; 270 | std::mutex mMutexMap; 271 | }; 272 | 273 | } //namespace ORB_SLAM 274 | 275 | #endif // MAPPOINT_H 276 | -------------------------------------------------------------------------------- /include/ImuTypes.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef IMUTYPES_H 21 | #define IMUTYPES_H 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace ORB_SLAM3 35 | { 36 | 37 | namespace IMU 38 | { 39 | 40 | const float GRAVITY_VALUE=9.81; 41 | 42 | //IMU measurement (gyro, accelerometer and timestamp) 43 | class Point 44 | { 45 | public: 46 | Point(const float &acc_x, const float &acc_y, const float &acc_z, 47 | const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z, 48 | const double ×tamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){} 49 | Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double ×tamp): 50 | a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){} 51 | public: 52 | cv::Point3f a; 53 | cv::Point3f w; 54 | double t; 55 | }; 56 | 57 | //IMU biases (gyro and accelerometer) 58 | class Bias 59 | { 60 | friend class boost::serialization::access; 61 | template 62 | void serialize(Archive & ar, const unsigned int version) 63 | { 64 | ar & bax; 65 | ar & bay; 66 | ar & baz; 67 | 68 | ar & bwx; 69 | ar & bwy; 70 | ar & bwz; 71 | } 72 | 73 | public: 74 | Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){} 75 | Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z, 76 | const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z): 77 | bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){} 78 | void CopyFrom(Bias &b); 79 | friend std::ostream& operator<< (std::ostream &out, const Bias &b); 80 | 81 | public: 82 | float bax, bay, baz; 83 | float bwx, bwy, bwz; 84 | }; 85 | 86 | //IMU calibration (Tbc, Tcb, noise) 87 | class Calib 88 | { 89 | template 90 | void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version) 91 | { 92 | int cols, rows, type; 93 | bool continuous; 94 | 95 | if (Archive::is_saving::value) { 96 | cols = mat.cols; rows = mat.rows; type = mat.type(); 97 | continuous = mat.isContinuous(); 98 | } 99 | 100 | ar & cols & rows & type & continuous; 101 | if (Archive::is_loading::value) 102 | mat.create(rows, cols, type); 103 | 104 | if (continuous) { 105 | const unsigned int data_size = rows * cols * mat.elemSize(); 106 | ar & boost::serialization::make_array(mat.ptr(), data_size); 107 | } else { 108 | const unsigned int row_size = cols*mat.elemSize(); 109 | for (int i = 0; i < rows; i++) { 110 | ar & boost::serialization::make_array(mat.ptr(i), row_size); 111 | } 112 | } 113 | } 114 | 115 | friend class boost::serialization::access; 116 | template 117 | void serialize(Archive & ar, const unsigned int version) 118 | { 119 | serializeMatrix(ar,Tcb,version); 120 | serializeMatrix(ar,Tbc,version); 121 | serializeMatrix(ar,Cov,version); 122 | serializeMatrix(ar,CovWalk,version); 123 | } 124 | 125 | public: 126 | Calib(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw) 127 | { 128 | Set(Tbc_,ng,na,ngw,naw); 129 | } 130 | Calib(const Calib &calib); 131 | Calib(){} 132 | 133 | void Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw); 134 | 135 | public: 136 | cv::Mat Tcb; 137 | cv::Mat Tbc; 138 | cv::Mat Cov, CovWalk; 139 | }; 140 | 141 | //Integration of 1 gyro measurement 142 | class IntegratedRotation 143 | { 144 | public: 145 | IntegratedRotation(){} 146 | IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time); 147 | 148 | public: 149 | float deltaT; //integration time 150 | cv::Mat deltaR; //integrated rotation 151 | cv::Mat rightJ; // right jacobian 152 | }; 153 | 154 | //Preintegration of Imu Measurements 155 | class Preintegrated 156 | { 157 | template 158 | void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version) 159 | { 160 | int cols, rows, type; 161 | bool continuous; 162 | 163 | if (Archive::is_saving::value) { 164 | cols = mat.cols; rows = mat.rows; type = mat.type(); 165 | continuous = mat.isContinuous(); 166 | } 167 | 168 | ar & cols & rows & type & continuous; 169 | if (Archive::is_loading::value) 170 | mat.create(rows, cols, type); 171 | 172 | if (continuous) { 173 | const unsigned int data_size = rows * cols * mat.elemSize(); 174 | ar & boost::serialization::make_array(mat.ptr(), data_size); 175 | } else { 176 | const unsigned int row_size = cols*mat.elemSize(); 177 | for (int i = 0; i < rows; i++) { 178 | ar & boost::serialization::make_array(mat.ptr(i), row_size); 179 | } 180 | } 181 | } 182 | 183 | friend class boost::serialization::access; 184 | template 185 | void serialize(Archive & ar, const unsigned int version) 186 | { 187 | ar & dT; 188 | serializeMatrix(ar,C,version); 189 | serializeMatrix(ar,Info,version); 190 | serializeMatrix(ar,Nga,version); 191 | serializeMatrix(ar,NgaWalk,version); 192 | ar & b; 193 | serializeMatrix(ar,dR,version); 194 | serializeMatrix(ar,dV,version); 195 | serializeMatrix(ar,dP,version); 196 | serializeMatrix(ar,JRg,version); 197 | serializeMatrix(ar,JVg,version); 198 | serializeMatrix(ar,JVa,version); 199 | serializeMatrix(ar,JPg,version); 200 | serializeMatrix(ar,JPa,version); 201 | serializeMatrix(ar,avgA,version); 202 | serializeMatrix(ar,avgW,version); 203 | 204 | ar & bu; 205 | serializeMatrix(ar,db,version); 206 | ar & mvMeasurements; 207 | } 208 | 209 | public: 210 | Preintegrated(const Bias &b_, const Calib &calib); 211 | Preintegrated(Preintegrated* pImuPre); 212 | Preintegrated() {} 213 | ~Preintegrated() {} 214 | void CopyFrom(Preintegrated* pImuPre); 215 | void Initialize(const Bias &b_); 216 | void IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt); 217 | void Reintegrate(); 218 | void MergePrevious(Preintegrated* pPrev); 219 | void SetNewBias(const Bias &bu_); 220 | IMU::Bias GetDeltaBias(const Bias &b_); 221 | cv::Mat GetDeltaRotation(const Bias &b_); 222 | cv::Mat GetDeltaVelocity(const Bias &b_); 223 | cv::Mat GetDeltaPosition(const Bias &b_); 224 | cv::Mat GetUpdatedDeltaRotation(); 225 | cv::Mat GetUpdatedDeltaVelocity(); 226 | cv::Mat GetUpdatedDeltaPosition(); 227 | cv::Mat GetOriginalDeltaRotation(); 228 | cv::Mat GetOriginalDeltaVelocity(); 229 | cv::Mat GetOriginalDeltaPosition(); 230 | Eigen::Matrix GetInformationMatrix(); 231 | cv::Mat GetDeltaBias(); 232 | Bias GetOriginalBias(); 233 | Bias GetUpdatedBias(); 234 | 235 | public: 236 | float dT; 237 | cv::Mat C; 238 | cv::Mat Info; 239 | // 测量噪声协方差矩阵，随机游走噪声协方差矩阵 240 | cv::Mat Nga, NgaWalk; 241 | 242 | // Values for the original bias (when integration was computed) 243 | Bias b; 244 | cv::Mat dR, dV, dP; 245 | cv::Mat JRg, JVg, JVa, JPg, JPa; 246 | cv::Mat avgA; 247 | cv::Mat avgW; 248 | 249 | 250 | private: 251 | // Updated bias 252 | Bias bu; 253 | // Dif between original and updated bias 254 | // This is used to compute the updated values of the preintegration 255 | cv::Mat db; 256 | 257 | struct integrable 258 | { 259 | integrable(const cv::Point3f &a_, const cv::Point3f &w_ , const float &t_):a(a_),w(w_),t(t_){} 260 | cv::Point3f a; 261 | cv::Point3f w; 262 | float t; 263 | }; 264 | 265 | std::vector mvMeasurements; 266 | 267 | std::mutex mMutex; 268 | }; 269 | 270 | // Lie Algebra Functions 271 | cv::Mat ExpSO3(const float &x, const float &y, const float &z); 272 | Eigen::Matrix ExpSO3(const double &x, const double &y, const double &z); 273 | cv::Mat ExpSO3(const cv::Mat &v); 274 | cv::Mat LogSO3(const cv::Mat &R); 275 | cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z); 276 | cv::Mat RightJacobianSO3(const cv::Mat &v); 277 | cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z); 278 | cv::Mat InverseRightJacobianSO3(const cv::Mat &v); 279 | cv::Mat Skew(const cv::Mat &v); 280 | cv::Mat NormalizeRotation(const cv::Mat &R); 281 | 282 | } 283 | 284 | } //namespace ORB_SLAM2 285 | 286 | #endif // IMUTYPES_H 287 | -------------------------------------------------------------------------------- /include/MLPnPsolver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | /****************************************************************************** 20 | * Author: Steffen Urban * 21 | * Contact: urbste@gmail.com * 22 | * License: Copyright (c) 2016 Steffen Urban, ANU. All rights reserved. * 23 | * * 24 | * Redistribution and use in source and binary forms, with or without * 25 | * modification, are permitted provided that the following conditions * 26 | * are met: * 27 | * * Redistributions of source code must retain the above copyright * 28 | * notice, this list of conditions and the following disclaimer. * 29 | * * Redistributions in binary form must reproduce the above copyright * 30 | * notice, this list of conditions and the following disclaimer in the * 31 | * documentation and/or other materials provided with the distribution. * 32 | * * Neither the name of ANU nor the names of its contributors may be * 33 | * used to endorse or promote products derived from this software without * 34 | * specific prior written permission. * 35 | * * 36 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"* 37 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 38 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 39 | * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE * 40 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * 41 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 42 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 43 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 44 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 45 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * 46 | * SUCH DAMAGE. * 47 | ******************************************************************************/ 48 | 49 | #ifndef ORB_SLAM3_MLPNPSOLVER_H 50 | #define ORB_SLAM3_MLPNPSOLVER_H 51 | 52 | #include "MapPoint.h" 53 | #include "Frame.h" 54 | 55 | #include 56 | #include 57 | 58 | namespace ORB_SLAM3{ 59 | class MLPnPsolver { 60 | public: 61 | MLPnPsolver(const Frame &F, const vector &vpMapPointMatches); 62 | 63 | ~MLPnPsolver(); 64 | 65 | void SetRansacParameters(double probability = 0.99, int minInliers = 8, int maxIterations = 300, int minSet = 6, float epsilon = 0.4, 66 | float th2 = 5.991); 67 | 68 | //Find metod is necessary? 69 | 70 | cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); 71 | 72 | //Type definitions needed by the original code 73 | 74 | /** A 3-vector of unit length used to describe landmark observations/bearings 75 | * in camera frames (always expressed in camera frames) 76 | */ 77 | typedef Eigen::Vector3d bearingVector_t; 78 | 79 | /** An array of bearing-vectors */ 80 | typedef std::vector > 81 | bearingVectors_t; 82 | 83 | /** A 2-matrix containing the 2D covariance information of a bearing vector 84 | */ 85 | typedef Eigen::Matrix2d cov2_mat_t; 86 | 87 | /** A 3-matrix containing the 3D covariance information of a bearing vector */ 88 | typedef Eigen::Matrix3d cov3_mat_t; 89 | 90 | /** An array of 3D covariance matrices */ 91 | typedef std::vector > 92 | cov3_mats_t; 93 | 94 | /** A 3-vector describing a point in 3D-space */ 95 | typedef Eigen::Vector3d point_t; 96 | 97 | /** An array of 3D-points */ 98 | typedef std::vector > 99 | points_t; 100 | 101 | /** A homogeneous 3-vector describing a point in 3D-space */ 102 | typedef Eigen::Vector4d point4_t; 103 | 104 | /** An array of homogeneous 3D-points */ 105 | typedef std::vector > 106 | points4_t; 107 | 108 | /** A 3-vector containing the rodrigues parameters of a rotation matrix */ 109 | typedef Eigen::Vector3d rodrigues_t; 110 | 111 | /** A rotation matrix */ 112 | typedef Eigen::Matrix3d rotation_t; 113 | 114 | /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and 115 | * translation \f$ \mathbf{t} \f$ as follows: 116 | * \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right) \f$ 117 | */ 118 | typedef Eigen::Matrix transformation_t; 119 | 120 | /** A 3-vector describing a translation/camera position */ 121 | typedef Eigen::Vector3d translation_t; 122 | 123 | 124 | 125 | private: 126 | void CheckInliers(); 127 | bool Refine(); 128 | 129 | //Functions from de original MLPnP code 130 | 131 | /* 132 | * Computes the camera pose given 3D points coordinates (in the camera reference 133 | * system), the camera rays and (optionally) the covariance matrix of those camera rays. 134 | * Result is stored in solution 135 | */ 136 | void computePose( 137 | const bearingVectors_t & f, 138 | const points_t & p, 139 | const cov3_mats_t & covMats, 140 | const std::vector& indices, 141 | transformation_t & result); 142 | 143 | void mlpnp_gn(Eigen::VectorXd& x, 144 | const points_t& pts, 145 | const std::vector& nullspaces, 146 | const Eigen::SparseMatrix Kll, 147 | bool use_cov); 148 | 149 | void mlpnp_residuals_and_jacs( 150 | const Eigen::VectorXd& x, 151 | const points_t& pts, 152 | const std::vector& nullspaces, 153 | Eigen::VectorXd& r, 154 | Eigen::MatrixXd& fjac, 155 | bool getJacs); 156 | 157 | void mlpnpJacs( 158 | const point_t& pt, 159 | const Eigen::Vector3d& nullspace_r, 160 | const Eigen::Vector3d& nullspace_s, 161 | const rodrigues_t& w, 162 | const translation_t& t, 163 | Eigen::MatrixXd& jacs); 164 | 165 | //Auxiliar methods 166 | 167 | /** 168 | * \brief Compute a rotation matrix from Rodrigues axis angle. 169 | * 170 | * \param[in] omega The Rodrigues-parameters of a rotation. 171 | * \return The 3x3 rotation matrix. 172 | */ 173 | Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d & omega); 174 | 175 | /** 176 | * \brief Compute the Rodrigues-parameters of a rotation matrix. 177 | * 178 | * \param[in] R The 3x3 rotation matrix. 179 | * \return The Rodrigues-parameters. 180 | */ 181 | Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d & R); 182 | 183 | //---------------------------------------------------- 184 | //Fields of the solver 185 | //---------------------------------------------------- 186 | vector mvpMapPointMatches; 187 | 188 | // 2D Points 189 | vector mvP2D; 190 | //Substitued by bearing vectors 191 | bearingVectors_t mvBearingVecs; 192 | 193 | vector mvSigma2; 194 | 195 | // 3D Points 196 | //vector mvP3Dw; 197 | points_t mvP3Dw; 198 | 199 | // Index in Frame 200 | vector mvKeyPointIndices; 201 | 202 | // Current Estimation 203 | double mRi[3][3]; 204 | double mti[3]; 205 | cv::Mat mTcwi; 206 | vector mvbInliersi; 207 | int mnInliersi; 208 | 209 | // Current Ransac State 210 | int mnIterations; 211 | vector mvbBestInliers; 212 | int mnBestInliers; 213 | cv::Mat mBestTcw; 214 | 215 | // Refined 216 | cv::Mat mRefinedTcw; 217 | vector mvbRefinedInliers; 218 | int mnRefinedInliers; 219 | 220 | // Number of Correspondences 221 | int N; 222 | 223 | // Indices for random selection [0 .. N-1] 224 | vector mvAllIndices; 225 | 226 | // RANSAC probability 227 | double mRansacProb; 228 | 229 | // RANSAC min inliers 230 | int mRansacMinInliers; 231 | 232 | // RANSAC max iterations 233 | int mRansacMaxIts; 234 | 235 | // RANSAC expected inliers/total ratio 236 | float mRansacEpsilon; 237 | 238 | // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2 239 | float mRansacTh; 240 | 241 | // RANSAC Minimun Set used at each iteration 242 | int mRansacMinSet; 243 | 244 | // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level) 245 | vector mvMaxError; 246 | 247 | GeometricCamera* mpCamera; 248 | 249 | }; 250 | 251 | } 252 | 253 | 254 | 255 | 256 | #endif //ORB_SLAM3_MLPNPSOLVER_H 257 | -------------------------------------------------------------------------------- /include/Tracking.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef TRACKING_H 21 | #define TRACKING_H 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include"Viewer.h" 28 | #include"FrameDrawer.h" 29 | #include"Atlas.h" 30 | #include"LocalMapping.h" 31 | #include"LoopClosing.h" 32 | #include"Frame.h" 33 | #include "ORBVocabulary.h" 34 | #include"KeyFrameDatabase.h" 35 | #include"ORBextractor.h" 36 | #include "Initializer.h" 37 | #include "MapDrawer.h" 38 | #include "System.h" 39 | #include "ImuTypes.h" 40 | 41 | #include "GeometricCamera.h" 42 | 43 | #include 44 | #include 45 | 46 | namespace ORB_SLAM3 47 | { 48 | 49 | class Viewer; 50 | class FrameDrawer; 51 | class Atlas; 52 | class LocalMapping; 53 | class LoopClosing; 54 | class System; 55 | 56 | class Tracking 57 | { 58 | 59 | public: 60 | Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pAtlas, 61 | KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, const string &_nameSeq=std::string()); 62 | 63 | ~Tracking(); 64 | 65 | // Parse the config file 66 | bool ParseCamParamFile(cv::FileStorage &fSettings); 67 | bool ParseORBParamFile(cv::FileStorage &fSettings); 68 | bool ParseIMUParamFile(cv::FileStorage &fSettings); 69 | 70 | // Preprocess the input and call Track(). Extract features and performs stereo matching. 71 | cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp, string filename); 72 | cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp, string filename); 73 | cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename); 74 | // cv::Mat GrabImageImuMonocular(const cv::Mat &im, const double ×tamp); 75 | 76 | void GrabImuData(const IMU::Point &imuMeasurement); 77 | 78 | void SetLocalMapper(LocalMapping* pLocalMapper); 79 | void SetLoopClosing(LoopClosing* pLoopClosing); 80 | void SetViewer(Viewer* pViewer); 81 | void SetStepByStep(bool bSet); 82 | 83 | // Load new settings 84 | // The focal lenght should be similar or scale prediction will fail when projecting points 85 | void ChangeCalibration(const string &strSettingPath); 86 | 87 | // Use this function if you have deactivated local mapping and you only want to localize the camera. 88 | void InformOnlyTracking(const bool &flag); 89 | 90 | void UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame); 91 | KeyFrame* GetLastKeyFrame() 92 | { 93 | return mpLastKeyFrame; 94 | } 95 | 96 | void CreateMapInAtlas(); 97 | std::mutex mMutexTracks; 98 | 99 | //-- 100 | void NewDataset(); 101 | int GetNumberDataset(); 102 | int GetMatchesInliers(); 103 | public: 104 | 105 | // Tracking states 106 | enum eTrackingState{ 107 | SYSTEM_NOT_READY=-1, 108 | NO_IMAGES_YET=0, 109 | NOT_INITIALIZED=1, 110 | OK=2, 111 | RECENTLY_LOST=3, 112 | LOST=4, 113 | OK_KLT=5 114 | }; 115 | 116 | eTrackingState mState; 117 | eTrackingState mLastProcessedState; 118 | 119 | // Input sensor 120 | int mSensor; 121 | 122 | // Current Frame 123 | Frame mCurrentFrame; 124 | Frame mLastFrame; 125 | 126 | cv::Mat mImGray; 127 | 128 | // Initialization Variables (Monocular) 129 | std::vector mvIniLastMatches; 130 | std::vector mvIniMatches; 131 | std::vector mvbPrevMatched; 132 | std::vector mvIniP3D; 133 | Frame mInitialFrame; 134 | 135 | // Lists used to recover the full camera trajectory at the end of the execution. 136 | // Basically we store the reference keyframe for each frame and its relative transformation 137 | list mlRelativeFramePoses; 138 | list mlpReferences; 139 | list mlFrameTimes; 140 | list mlbLost; 141 | 142 | // frames with estimated pose 143 | int mTrackedFr; 144 | bool mbStep; 145 | 146 | // True if local mapping is deactivated and we are performing only localization 147 | bool mbOnlyTracking; 148 | 149 | void Reset(bool bLocMap = false); 150 | void ResetActiveMap(bool bLocMap = false); 151 | 152 | float mMeanTrack; 153 | bool mbInitWith3KFs; 154 | double t0; // time-stamp of first read frame 155 | double t0vis; // time-stamp of first inserted keyframe 156 | double t0IMU; // time-stamp of IMU initialization 157 | 158 | 159 | vector GetLocalMapMPS(); 160 | 161 | 162 | //TEST-- 163 | bool mbNeedRectify; 164 | //cv::Mat M1l, M2l; 165 | //cv::Mat M1r, M2r; 166 | 167 | bool mbWriteStats; 168 | 169 | protected: 170 | 171 | // Main tracking function. It is independent of the input sensor. 172 | void Track(); 173 | 174 | // Map initialization for stereo and RGB-D 175 | void StereoInitialization(); 176 | 177 | // Map initialization for monocular 178 | void MonocularInitialization(); 179 | void CreateNewMapPoints(); 180 | cv::Mat ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2); 181 | void CreateInitialMapMonocular(); 182 | 183 | void CheckReplacedInLastFrame(); 184 | bool TrackReferenceKeyFrame(); 185 | void UpdateLastFrame(); 186 | bool TrackWithMotionModel(); 187 | bool PredictStateIMU(); 188 | 189 | bool Relocalization(); 190 | 191 | void UpdateLocalMap(); 192 | void UpdateLocalPoints(); 193 | void UpdateLocalKeyFrames(); 194 | 195 | bool TrackLocalMap(); 196 | bool TrackLocalMap_old(); 197 | void SearchLocalPoints(); 198 | 199 | bool NeedNewKeyFrame(); 200 | void CreateNewKeyFrame(); 201 | 202 | // Perform preintegration from last frame 203 | void PreintegrateIMU(); 204 | 205 | // Reset IMU biases and compute frame velocity 206 | void ResetFrameIMU(); 207 | void ComputeGyroBias(const vector &vpFs, float &bwx, float &bwy, float &bwz); 208 | void ComputeVelocitiesAccBias(const vector &vpFs, float &bax, float &bay, float &baz); 209 | 210 | 211 | bool mbMapUpdated; 212 | 213 | // Imu preintegration from last frame 214 | IMU::Preintegrated *mpImuPreintegratedFromLastKF; 215 | 216 | // Queue of IMU measurements between frames 217 | std::list mlQueueImuData; 218 | 219 | // Vector of IMU measurements from previous to current frame (to be filled by PreintegrateIMU) 220 | std::vector mvImuFromLastFrame; 221 | std::mutex mMutexImuQueue; 222 | 223 | // Imu calibration parameters 224 | IMU::Calib *mpImuCalib; 225 | 226 | // Last Bias Estimation (at keyframe creation) 227 | IMU::Bias mLastBias; 228 | 229 | // In case of performing only localization, this flag is true when there are no matches to 230 | // points in the map. Still tracking will continue if there are enough matches with temporal points. 231 | // In that case we are doing visual odometry. The system will try to do relocalization to recover 232 | // "zero-drift" localization to the map. 233 | bool mbVO; 234 | 235 | //Other Thread Pointers 236 | LocalMapping* mpLocalMapper; 237 | LoopClosing* mpLoopClosing; 238 | 239 | //ORB 240 | ORBextractor* mpORBextractorLeft, *mpORBextractorRight; 241 | ORBextractor* mpIniORBextractor; 242 | 243 | //BoW 244 | ORBVocabulary* mpORBVocabulary; 245 | KeyFrameDatabase* mpKeyFrameDB; 246 | 247 | // Initalization (only for monocular) 248 | Initializer* mpInitializer; 249 | bool mbSetInit; 250 | 251 | //Local Map 252 | KeyFrame* mpReferenceKF; 253 | std::vector mvpLocalKeyFrames; 254 | std::vector mvpLocalMapPoints; 255 | 256 | // System 257 | System* mpSystem; 258 | 259 | //Drawers 260 | Viewer* mpViewer; 261 | FrameDrawer* mpFrameDrawer; 262 | MapDrawer* mpMapDrawer; 263 | bool bStepByStep; 264 | 265 | //Atlas 266 | Atlas* mpAtlas; 267 | 268 | //Calibration matrix 269 | cv::Mat mK; 270 | cv::Mat mDistCoef; // k1,k2,p1,p2,k3 271 | float mbf; 272 | 273 | //New KeyFrame rules (according to fps) 274 | int mMinFrames; 275 | int mMaxFrames; 276 | 277 | int mnFirstImuFrameId; 278 | int mnFramesToResetIMU; 279 | 280 | // Threshold close/far points 281 | // Points seen as close by the stereo/RGBD sensor are considered reliable 282 | // and inserted from just one frame. Far points requiere a match in two keyframes. 283 | float mThDepth; 284 | 285 | // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled. 286 | float mDepthMapFactor; 287 | 288 | //Current matches in frame 289 | int mnMatchesInliers; 290 | 291 | //Last Frame, KeyFrame and Relocalisation Info 292 | KeyFrame* mpLastKeyFrame; 293 | unsigned int mnLastKeyFrameId; 294 | unsigned int mnLastRelocFrameId; 295 | double mTimeStampLost; 296 | double time_recently_lost; 297 | 298 | unsigned int mnFirstFrameId; 299 | unsigned int mnInitialFrameId; 300 | unsigned int mnLastInitFrameId; 301 | 302 | bool mbCreatedMap; 303 | 304 | 305 | //Motion Model 306 | cv::Mat mVelocity; 307 | 308 | //Color order (true RGB, false BGR, ignored if grayscale) 309 | bool mbRGB; 310 | 311 | list mlpTemporalPoints; 312 | 313 | //int nMapChangeIndex; 314 | 315 | int mnNumDataset; 316 | 317 | ofstream f_track_stats; 318 | 319 | ofstream f_track_times; 320 | double mTime_PreIntIMU; 321 | double mTime_PosePred; 322 | double mTime_LocalMapTrack; 323 | double mTime_NewKF_Dec; 324 | 325 | GeometricCamera* mpCamera, *mpCamera2; 326 | 327 | // 上一帧 328 | int initID, lastID; 329 | 330 | cv::Mat mTlr; 331 | 332 | public: 333 | cv::Mat mImRight; 334 | }; 335 | 336 | } //namespace ORB_SLAM 337 | 338 | #endif // TRACKING_H 339 | -------------------------------------------------------------------------------- /src/Atlas.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #include "Atlas.h" 20 | #include "Viewer.h" 21 | 22 | #include "GeometricCamera.h" 23 | #include "Pinhole.h" 24 | #include "KannalaBrandt8.h" 25 | 26 | namespace ORB_SLAM3 27 | { 28 | 29 | Atlas::Atlas(){ 30 | mpCurrentMap = static_cast(NULL); 31 | } 32 | 33 | Atlas::Atlas(int initKFid): mnLastInitKFidMap(initKFid), mHasViewer(false) 34 | { 35 | mpCurrentMap = static_cast(NULL); 36 | CreateNewMap(); 37 | } 38 | 39 | Atlas::~Atlas() 40 | { 41 | for(std::set::iterator it = mspMaps.begin(), end = mspMaps.end(); it != end;) 42 | { 43 | Map* pMi = *it; 44 | 45 | if(pMi) 46 | { 47 | delete pMi; 48 | pMi = static_cast(NULL); 49 | 50 | it = mspMaps.erase(it); 51 | } 52 | else 53 | ++it; 54 | 55 | } 56 | } 57 | 58 | void Atlas::CreateNewMap() 59 | { 60 | unique_lock lock(mMutexAtlas); 61 | cout << "Creation of new map with id: " << Map::nNextId << endl; 62 | if(mpCurrentMap){ 63 | cout << "Exits current map " << endl; 64 | if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) 65 | mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum 66 | 67 | mpCurrentMap->SetStoredMap(); 68 | cout << "Saved map with ID: " << mpCurrentMap->GetId() << endl; 69 | 70 | //if(mHasViewer) 71 | // mpViewer->AddMapToCreateThumbnail(mpCurrentMap); 72 | } 73 | cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl; 74 | 75 | mpCurrentMap = new Map(mnLastInitKFidMap); 76 | mpCurrentMap->SetCurrentMap(); 77 | mspMaps.insert(mpCurrentMap); 78 | } 79 | 80 | void Atlas::ChangeMap(Map* pMap) 81 | { 82 | unique_lock lock(mMutexAtlas); 83 | cout << "Chage to map with id: " << pMap->GetId() << endl; 84 | if(mpCurrentMap){ 85 | mpCurrentMap->SetStoredMap(); 86 | } 87 | 88 | mpCurrentMap = pMap; 89 | mpCurrentMap->SetCurrentMap(); 90 | } 91 | 92 | unsigned long int Atlas::GetLastInitKFid() 93 | { 94 | unique_lock lock(mMutexAtlas); 95 | return mnLastInitKFidMap; 96 | } 97 | 98 | void Atlas::SetViewer(Viewer* pViewer) 99 | { 100 | mpViewer = pViewer; 101 | mHasViewer = true; 102 | } 103 | 104 | void Atlas::AddKeyFrame(KeyFrame* pKF) 105 | { 106 | Map* pMapKF = pKF->GetMap(); 107 | pMapKF->AddKeyFrame(pKF); 108 | } 109 | 110 | void Atlas::AddMapPoint(MapPoint* pMP) 111 | { 112 | Map* pMapMP = pMP->GetMap(); 113 | pMapMP->AddMapPoint(pMP); 114 | } 115 | 116 | void Atlas::AddCamera(GeometricCamera* pCam) 117 | { 118 | mvpCameras.push_back(pCam); 119 | } 120 | 121 | void Atlas::SetReferenceMapPoints(const std::vector &vpMPs) 122 | { 123 | unique_lock lock(mMutexAtlas); 124 | mpCurrentMap->SetReferenceMapPoints(vpMPs); 125 | } 126 | 127 | void Atlas::InformNewBigChange() 128 | { 129 | unique_lock lock(mMutexAtlas); 130 | mpCurrentMap->InformNewBigChange(); 131 | } 132 | 133 | int Atlas::GetLastBigChangeIdx() 134 | { 135 | unique_lock lock(mMutexAtlas); 136 | return mpCurrentMap->GetLastBigChangeIdx(); 137 | } 138 | 139 | long unsigned int Atlas::MapPointsInMap() 140 | { 141 | unique_lock lock(mMutexAtlas); 142 | return mpCurrentMap->MapPointsInMap(); 143 | } 144 | 145 | long unsigned Atlas::KeyFramesInMap() 146 | { 147 | unique_lock lock(mMutexAtlas); 148 | return mpCurrentMap->KeyFramesInMap(); 149 | } 150 | 151 | std::vector Atlas::GetAllKeyFrames() 152 | { 153 | unique_lock lock(mMutexAtlas); 154 | return mpCurrentMap->GetAllKeyFrames(); 155 | } 156 | 157 | std::vector Atlas::GetAllMapPoints() 158 | { 159 | unique_lock lock(mMutexAtlas); 160 | return mpCurrentMap->GetAllMapPoints(); 161 | } 162 | 163 | std::vector Atlas::GetReferenceMapPoints() 164 | { 165 | unique_lock lock(mMutexAtlas); 166 | return mpCurrentMap->GetReferenceMapPoints(); 167 | } 168 | 169 | vector Atlas::GetAllMaps() 170 | { 171 | unique_lock lock(mMutexAtlas); 172 | struct compFunctor 173 | { 174 | inline bool operator()(Map* elem1 ,Map* elem2) 175 | { 176 | return elem1->GetId() < elem2->GetId(); 177 | } 178 | }; 179 | vector vMaps(mspMaps.begin(),mspMaps.end()); 180 | sort(vMaps.begin(), vMaps.end(), compFunctor()); 181 | return vMaps; 182 | } 183 | 184 | int Atlas::CountMaps() 185 | { 186 | unique_lock lock(mMutexAtlas); 187 | return mspMaps.size(); 188 | } 189 | 190 | void Atlas::clearMap() 191 | { 192 | unique_lock lock(mMutexAtlas); 193 | mpCurrentMap->clear(); 194 | } 195 | 196 | void Atlas::clearAtlas() 197 | { 198 | unique_lock lock(mMutexAtlas); 199 | /*for(std::set::iterator it=mspMaps.begin(), send=mspMaps.end(); it!=send; it++) 200 | { 201 | (*it)->clear(); 202 | delete *it; 203 | }*/ 204 | mspMaps.clear(); 205 | mpCurrentMap = static_cast(NULL); 206 | mnLastInitKFidMap = 0; 207 | } 208 | 209 | Map* Atlas::GetCurrentMap() 210 | { 211 | unique_lock lock(mMutexAtlas); 212 | if(!mpCurrentMap) 213 | CreateNewMap(); 214 | while(mpCurrentMap->IsBad()) 215 | usleep(3000); 216 | 217 | return mpCurrentMap; 218 | } 219 | 220 | void Atlas::SetMapBad(Map* pMap) 221 | { 222 | mspMaps.erase(pMap); 223 | pMap->SetBad(); 224 | 225 | mspBadMaps.insert(pMap); 226 | } 227 | 228 | void Atlas::RemoveBadMaps() 229 | { 230 | /*for(Map* pMap : mspBadMaps) 231 | { 232 | delete pMap; 233 | pMap = static_cast(NULL); 234 | }*/ 235 | mspBadMaps.clear(); 236 | } 237 | 238 | bool Atlas::isInertial() 239 | { 240 | unique_lock lock(mMutexAtlas); 241 | return mpCurrentMap->IsInertial(); 242 | } 243 | 244 | void Atlas::SetInertialSensor() 245 | { 246 | unique_lock lock(mMutexAtlas); 247 | mpCurrentMap->SetInertialSensor(); 248 | } 249 | 250 | void Atlas::SetImuInitialized() 251 | { 252 | unique_lock lock(mMutexAtlas); 253 | mpCurrentMap->SetImuInitialized(); 254 | } 255 | 256 | bool Atlas::isImuInitialized() 257 | { 258 | unique_lock lock(mMutexAtlas); 259 | return mpCurrentMap->isImuInitialized(); 260 | } 261 | 262 | void Atlas::PreSave() 263 | { 264 | if(mpCurrentMap){ 265 | if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) 266 | mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum 267 | } 268 | 269 | struct compFunctor 270 | { 271 | inline bool operator()(Map* elem1 ,Map* elem2) 272 | { 273 | return elem1->GetId() < elem2->GetId(); 274 | } 275 | }; 276 | std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps)); 277 | sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor()); 278 | 279 | std::set spCams(mvpCameras.begin(), mvpCameras.end()); 280 | cout << "There are " << spCams.size() << " cameras in the atlas" << endl; 281 | for(Map* pMi : mvpBackupMaps) 282 | { 283 | cout << "Pre-save of map " << pMi->GetId() << endl; 284 | pMi->PreSave(spCams); 285 | } 286 | cout << "Maps stored" << endl; 287 | for(GeometricCamera* pCam : mvpCameras) 288 | { 289 | cout << "Pre-save of camera " << pCam->GetId() << endl; 290 | if(pCam->GetType() == pCam->CAM_PINHOLE) 291 | { 292 | mvpBackupCamPin.push_back((Pinhole*) pCam); 293 | } 294 | else if(pCam->GetType() == pCam->CAM_FISHEYE) 295 | { 296 | mvpBackupCamKan.push_back((KannalaBrandt8*) pCam); 297 | } 298 | } 299 | 300 | } 301 | 302 | void Atlas::PostLoad() 303 | { 304 | mvpCameras.clear(); 305 | map mpCams; 306 | for(Pinhole* pCam : mvpBackupCamPin) 307 | { 308 | //mvpCameras.push_back((GeometricCamera*)pCam); 309 | mvpCameras.push_back(pCam); 310 | mpCams[pCam->GetId()] = pCam; 311 | } 312 | for(KannalaBrandt8* pCam : mvpBackupCamKan) 313 | { 314 | //mvpCameras.push_back((GeometricCamera*)pCam); 315 | mvpCameras.push_back(pCam); 316 | mpCams[pCam->GetId()] = pCam; 317 | } 318 | 319 | mspMaps.clear(); 320 | unsigned long int numKF = 0, numMP = 0; 321 | map mpAllKeyFrameId; 322 | for(Map* pMi : mvpBackupMaps) 323 | { 324 | cout << "Map id:" << pMi->GetId() << endl; 325 | mspMaps.insert(pMi); 326 | map mpKeyFrameId; 327 | pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpKeyFrameId, mpCams); 328 | mpAllKeyFrameId.insert(mpKeyFrameId.begin(), mpKeyFrameId.end()); 329 | numKF += pMi->GetAllKeyFrames().size(); 330 | numMP += pMi->GetAllMapPoints().size(); 331 | } 332 | 333 | cout << "Number KF:" << numKF << "; number MP:" << numMP << endl; 334 | mvpBackupMaps.clear(); 335 | } 336 | 337 | void Atlas::SetKeyFrameDababase(KeyFrameDatabase* pKFDB) 338 | { 339 | mpKeyFrameDB = pKFDB; 340 | } 341 | 342 | KeyFrameDatabase* Atlas::GetKeyFrameDatabase() 343 | { 344 | return mpKeyFrameDB; 345 | } 346 | 347 | void Atlas::SetORBVocabulary(ORBVocabulary* pORBVoc) 348 | { 349 | mpORBVocabulary = pORBVoc; 350 | } 351 | 352 | ORBVocabulary* Atlas::GetORBVocabulary() 353 | { 354 | return mpORBVocabulary; 355 | } 356 | 357 | long unsigned int Atlas::GetNumLivedKF() 358 | { 359 | unique_lock lock(mMutexAtlas); 360 | long unsigned int num = 0; 361 | for(Map* mMAPi : mspMaps) 362 | { 363 | num += mMAPi->GetAllKeyFrames().size(); 364 | } 365 | 366 | return num; 367 | } 368 | 369 | long unsigned int Atlas::GetNumLivedMP() { 370 | unique_lock lock(mMutexAtlas); 371 | long unsigned int num = 0; 372 | for (Map *mMAPi : mspMaps) { 373 | num += mMAPi->GetAllMapPoints().size(); 374 | } 375 | 376 | return num; 377 | } 378 | 379 | } //namespace ORB_SLAM3 380 | -------------------------------------------------------------------------------- /src/OptimizableTypes.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #include "OptimizableTypes.h" 20 | 21 | namespace ORB_SLAM3 { 22 | bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){ 23 | for (int i=0; i<2; i++){ 24 | is >> _measurement[i]; 25 | } 26 | for (int i=0; i<2; i++) 27 | for (int j=i; j<2; j++) { 28 | is >> information()(i,j); 29 | if (i!=j) 30 | information()(j,i)=information()(i,j); 31 | } 32 | return true; 33 | } 34 | 35 | bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const { 36 | 37 | for (int i=0; i<2; i++){ 38 | os << measurement()[i] << " "; 39 | } 40 | 41 | for (int i=0; i<2; i++) 42 | for (int j=i; j<2; j++){ 43 | os << " " << information()(i,j); 44 | } 45 | return os.good(); 46 | } 47 | 48 | 49 | void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() { 50 | g2o::VertexSE3Expmap * vi = static_cast(_vertices[0]); 51 | Eigen::Vector3d xyz_trans = vi->estimate().map(Xw); 52 | 53 | double x = xyz_trans[0]; 54 | double y = xyz_trans[1]; 55 | double z = xyz_trans[2]; 56 | 57 | Eigen::Matrix SE3deriv; 58 | SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, 59 | -z , 0.f, x, 0.f, 1.f, 0.f, 60 | y , -x , 0.f, 0.f, 0.f, 1.f; 61 | 62 | _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv; 63 | } 64 | 65 | bool EdgeSE3ProjectXYZOnlyPoseToBody::read(std::istream& is){ 66 | for (int i=0; i<2; i++){ 67 | is >> _measurement[i]; 68 | } 69 | for (int i=0; i<2; i++) 70 | for (int j=i; j<2; j++) { 71 | is >> information()(i,j); 72 | if (i!=j) 73 | information()(j,i)=information()(i,j); 74 | } 75 | return true; 76 | } 77 | 78 | bool EdgeSE3ProjectXYZOnlyPoseToBody::write(std::ostream& os) const { 79 | 80 | for (int i=0; i<2; i++){ 81 | os << measurement()[i] << " "; 82 | } 83 | 84 | for (int i=0; i<2; i++) 85 | for (int j=i; j<2; j++){ 86 | os << " " << information()(i,j); 87 | } 88 | return os.good(); 89 | } 90 | 91 | void EdgeSE3ProjectXYZOnlyPoseToBody::linearizeOplus() { 92 | g2o::VertexSE3Expmap * vi = static_cast(_vertices[0]); 93 | g2o::SE3Quat T_lw(vi->estimate()); 94 | Eigen::Vector3d X_l = T_lw.map(Xw); 95 | Eigen::Vector3d X_r = mTrl.map(T_lw.map(Xw)); 96 | 97 | double x_w = X_l[0]; 98 | double y_w = X_l[1]; 99 | double z_w = X_l[2]; 100 | 101 | Eigen::Matrix SE3deriv; 102 | SE3deriv << 0.f, z_w, -y_w, 1.f, 0.f, 0.f, 103 | -z_w , 0.f, x_w, 0.f, 1.f, 0.f, 104 | y_w , -x_w , 0.f, 0.f, 0.f, 1.f; 105 | 106 | _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv; 107 | } 108 | 109 | EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() { 110 | } 111 | 112 | bool EdgeSE3ProjectXYZ::read(std::istream& is){ 113 | for (int i=0; i<2; i++){ 114 | is >> _measurement[i]; 115 | } 116 | for (int i=0; i<2; i++) 117 | for (int j=i; j<2; j++) { 118 | is >> information()(i,j); 119 | if (i!=j) 120 | information()(j,i)=information()(i,j); 121 | } 122 | return true; 123 | } 124 | 125 | bool EdgeSE3ProjectXYZ::write(std::ostream& os) const { 126 | 127 | for (int i=0; i<2; i++){ 128 | os << measurement()[i] << " "; 129 | } 130 | 131 | for (int i=0; i<2; i++) 132 | for (int j=i; j<2; j++){ 133 | os << " " << information()(i,j); 134 | } 135 | return os.good(); 136 | } 137 | 138 | 139 | void EdgeSE3ProjectXYZ::linearizeOplus() { 140 | g2o::VertexSE3Expmap * vj = static_cast(_vertices[1]); 141 | g2o::SE3Quat T(vj->estimate()); 142 | g2o::VertexSBAPointXYZ* vi = static_cast(_vertices[0]); 143 | Eigen::Vector3d xyz = vi->estimate(); 144 | Eigen::Vector3d xyz_trans = T.map(xyz); 145 | 146 | double x = xyz_trans[0]; 147 | double y = xyz_trans[1]; 148 | double z = xyz_trans[2]; 149 | 150 | auto projectJac = -pCamera->projectJac(xyz_trans); 151 | 152 | _jacobianOplusXi = projectJac * T.rotation().toRotationMatrix(); 153 | 154 | Eigen::Matrix SE3deriv; 155 | SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, 156 | -z , 0.f, x, 0.f, 1.f, 0.f, 157 | y , -x , 0.f, 0.f, 0.f, 1.f; 158 | 159 | _jacobianOplusXj = projectJac * SE3deriv; 160 | } 161 | 162 | EdgeSE3ProjectXYZToBody::EdgeSE3ProjectXYZToBody() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>() { 163 | } 164 | 165 | bool EdgeSE3ProjectXYZToBody::read(std::istream& is){ 166 | for (int i=0; i<2; i++){ 167 | is >> _measurement[i]; 168 | } 169 | for (int i=0; i<2; i++) 170 | for (int j=i; j<2; j++) { 171 | is >> information()(i,j); 172 | if (i!=j) 173 | information()(j,i)=information()(i,j); 174 | } 175 | return true; 176 | } 177 | 178 | bool EdgeSE3ProjectXYZToBody::write(std::ostream& os) const { 179 | 180 | for (int i=0; i<2; i++){ 181 | os << measurement()[i] << " "; 182 | } 183 | 184 | for (int i=0; i<2; i++) 185 | for (int j=i; j<2; j++){ 186 | os << " " << information()(i,j); 187 | } 188 | return os.good(); 189 | } 190 | 191 | 192 | void EdgeSE3ProjectXYZToBody::linearizeOplus() { 193 | g2o::VertexSE3Expmap * vj = static_cast(_vertices[1]); 194 | g2o::SE3Quat T_lw(vj->estimate()); 195 | g2o::SE3Quat T_rw = mTrl * T_lw; 196 | g2o::VertexSBAPointXYZ* vi = static_cast(_vertices[0]); 197 | Eigen::Vector3d X_w = vi->estimate(); 198 | Eigen::Vector3d X_l = T_lw.map(X_w); 199 | Eigen::Vector3d X_r = mTrl.map(T_lw.map(X_w)); 200 | 201 | _jacobianOplusXi = -pCamera->projectJac(X_r) * T_rw.rotation().toRotationMatrix(); 202 | 203 | double x = X_l[0]; 204 | double y = X_l[1]; 205 | double z = X_l[2]; 206 | 207 | Eigen::Matrix SE3deriv; 208 | SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f, 209 | -z , 0.f, x, 0.f, 1.f, 0.f, 210 | y , -x , 0.f, 0.f, 0.f, 1.f; 211 | 212 | _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv; 213 | } 214 | 215 | 216 | VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, g2o::Sim3>() 217 | { 218 | _marginalized=false; 219 | _fix_scale = false; 220 | } 221 | 222 | bool VertexSim3Expmap::read(std::istream& is) 223 | { 224 | g2o::Vector7d cam2world; 225 | for (int i=0; i<6; i++){ 226 | is >> cam2world[i]; 227 | } 228 | is >> cam2world[6]; 229 | 230 | float nextParam; 231 | for(size_t i = 0; i < pCamera1->size(); i++){ 232 | is >> nextParam; 233 | pCamera1->setParameter(nextParam,i); 234 | } 235 | 236 | for(size_t i = 0; i < pCamera2->size(); i++){ 237 | is >> nextParam; 238 | pCamera2->setParameter(nextParam,i); 239 | } 240 | 241 | setEstimate(g2o::Sim3(cam2world).inverse()); 242 | return true; 243 | } 244 | 245 | bool VertexSim3Expmap::write(std::ostream& os) const 246 | { 247 | g2o::Sim3 cam2world(estimate().inverse()); 248 | g2o::Vector7d lv=cam2world.log(); 249 | for (int i=0; i<7; i++){ 250 | os << lv[i] << " "; 251 | } 252 | 253 | for(size_t i = 0; i < pCamera1->size(); i++){ 254 | os << pCamera1->getParameter(i) << " "; 255 | } 256 | 257 | for(size_t i = 0; i < pCamera2->size(); i++){ 258 | os << pCamera2->getParameter(i) << " "; 259 | } 260 | 261 | return os.good(); 262 | } 263 | 264 | EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() : 265 | g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>() 266 | { 267 | } 268 | 269 | bool EdgeSim3ProjectXYZ::read(std::istream& is) 270 | { 271 | for (int i=0; i<2; i++) 272 | { 273 | is >> _measurement[i]; 274 | } 275 | 276 | for (int i=0; i<2; i++) 277 | for (int j=i; j<2; j++) { 278 | is >> information()(i,j); 279 | if (i!=j) 280 | information()(j,i)=information()(i,j); 281 | } 282 | return true; 283 | } 284 | 285 | bool EdgeSim3ProjectXYZ::write(std::ostream& os) const 286 | { 287 | for (int i=0; i<2; i++){ 288 | os << _measurement[i] << " "; 289 | } 290 | 291 | for (int i=0; i<2; i++) 292 | for (int j=i; j<2; j++){ 293 | os << " " << information()(i,j); 294 | } 295 | return os.good(); 296 | } 297 | 298 | EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() : 299 | g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>() 300 | { 301 | } 302 | 303 | bool EdgeInverseSim3ProjectXYZ::read(std::istream& is) 304 | { 305 | for (int i=0; i<2; i++) 306 | { 307 | is >> _measurement[i]; 308 | } 309 | 310 | for (int i=0; i<2; i++) 311 | for (int j=i; j<2; j++) { 312 | is >> information()(i,j); 313 | if (i!=j) 314 | information()(j,i)=information()(i,j); 315 | } 316 | return true; 317 | } 318 | 319 | bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const 320 | { 321 | for (int i=0; i<2; i++){ 322 | os << _measurement[i] << " "; 323 | } 324 | 325 | for (int i=0; i<2; i++) 326 | for (int j=i; j<2; j++){ 327 | os << " " << information()(i,j); 328 | } 329 | return os.good(); 330 | } 331 | 332 | } 333 | -------------------------------------------------------------------------------- /include/Frame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #ifndef FRAME_H 21 | #define FRAME_H 22 | 23 | //#define SAVE_TIMES 24 | 25 | #include 26 | 27 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 28 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 29 | 30 | #include "ImuTypes.h" 31 | #include "ORBVocabulary.h" 32 | 33 | #include 34 | #include 35 | 36 | namespace ORB_SLAM3 37 | { 38 | #define FRAME_GRID_ROWS 48 39 | #define FRAME_GRID_COLS 64 40 | 41 | class MapPoint; 42 | class KeyFrame; 43 | class ConstraintPoseImu; 44 | class GeometricCamera; 45 | class ORBextractor; 46 | 47 | class Frame 48 | { 49 | public: 50 | Frame(); 51 | 52 | // Copy constructor. 53 | Frame(const Frame &frame); 54 | 55 | // Constructor for stereo cameras. 56 | 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(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); 57 | 58 | // Constructor for RGB-D cameras. 59 | 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(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); 60 | 61 | // Constructor for Monocular cameras. 62 | 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(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); 63 | 64 | // Destructor 65 | // ~Frame(); 66 | 67 | // Extract ORB on the image. 0 for left image and 1 for right image. 68 | void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1); 69 | 70 | // Compute Bag of Words representation. 71 | void ComputeBoW(); 72 | 73 | // Set the camera pose. (Imu pose is not modified!) 74 | void SetPose(cv::Mat Tcw); 75 | void GetPose(cv::Mat &Tcw); 76 | 77 | // Set IMU velocity 78 | void SetVelocity(const cv::Mat &Vwb); 79 | 80 | // Set IMU pose and velocity (implicitly changes camera pose) 81 | void SetImuPoseVelocity(const cv::Mat &Rwb, const cv::Mat &twb, const cv::Mat &Vwb); 82 | 83 | 84 | // Computes rotation, translation and camera center matrices from the camera pose. 85 | void UpdatePoseMatrices(); 86 | 87 | // Returns the camera center. 88 | inline cv::Mat GetCameraCenter(){ 89 | return mOw.clone(); 90 | } 91 | 92 | // Returns inverse of rotation 93 | inline cv::Mat GetRotationInverse(){ 94 | return mRwc.clone(); 95 | } 96 | 97 | cv::Mat GetImuPosition(); 98 | cv::Mat GetImuRotation(); 99 | cv::Mat GetImuPose(); 100 | 101 | void SetNewBias(const IMU::Bias &b); 102 | 103 | // Check if a MapPoint is in the frustum of the camera 104 | // and fill variables of the MapPoint to be used by the tracking 105 | bool isInFrustum(MapPoint* pMP, float viewingCosLimit); 106 | 107 | bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); 108 | 109 | cv::Mat inRefCoordinates(cv::Mat pCw); 110 | 111 | // Compute the cell of a keypoint (return false if outside the grid) 112 | bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY); 113 | 114 | vector GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1, const bool bRight = false) const; 115 | 116 | // Search a match for each keypoint in the left image to a keypoint in the right image. 117 | // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored. 118 | void ComputeStereoMatches(); 119 | 120 | // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap. 121 | void ComputeStereoFromRGBD(const cv::Mat &imDepth); 122 | 123 | // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates. 124 | cv::Mat UnprojectStereo(const int &i); 125 | 126 | ConstraintPoseImu* mpcpi; 127 | 128 | bool imuIsPreintegrated(); 129 | void setIntegrated(); 130 | 131 | cv::Mat mRwc; 132 | cv::Mat mOw; 133 | public: 134 | // Vocabulary used for relocalization. 135 | ORBVocabulary* mpORBvocabulary; 136 | 137 | // Feature extractor. The right is used only in the stereo case. 138 | ORBextractor* mpORBextractorLeft, *mpORBextractorRight; 139 | 140 | // Frame timestamp. 141 | double mTimeStamp; 142 | 143 | // Calibration matrix and OpenCV distortion parameters. 144 | cv::Mat mK; 145 | static float fx; 146 | static float fy; 147 | static float cx; 148 | static float cy; 149 | static float invfx; 150 | static float invfy; 151 | cv::Mat mDistCoef; 152 | 153 | // Stereo baseline multiplied by fx. 154 | float mbf; 155 | 156 | // Stereo baseline in meters. 157 | float mb; 158 | 159 | // Threshold close/far points. Close points are inserted from 1 view. 160 | // Far points are inserted as in the monocular case from 2 views. 161 | float mThDepth; 162 | 163 | // Number of KeyPoints. 164 | int N; 165 | 166 | // Vector of keypoints (original for visualization) and undistorted (actually used by the system). 167 | // In the stereo case, mvKeysUn is redundant as images must be rectified. 168 | // In the RGB-D case, RGB images can be distorted. 169 | std::vector mvKeys, mvKeysRight; 170 | std::vector mvKeysUn; 171 | 172 | // Corresponding stereo coordinate and depth for each keypoint. 173 | std::vector mvpMapPoints; 174 | // "Monocular" keypoints have a negative value. 175 | // 左目特征点对应的右目匹配点，左目多，右目少（有些左目点匹配不到） 176 | std::vector mvuRight; 177 | std::vector mvDepth; 178 | 179 | // Bag of Words Vector structures. 180 | DBoW2::BowVector mBowVec; 181 | DBoW2::FeatureVector mFeatVec; 182 | 183 | // ORB descriptor, each row associated to a keypoint. 184 | // 描述子，每行表示一个特征点 185 | cv::Mat mDescriptors, mDescriptorsRight; 186 | 187 | // MapPoints associated to keypoints, NULL pointer if no association. 188 | // Flag to identify outlier associations. 189 | std::vector mvbOutlier; 190 | int mnCloseMPs; 191 | 192 | // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints. 193 | static float mfGridElementWidthInv; 194 | static float mfGridElementHeightInv; 195 | // 每个grid存若干特征点的索引，对应mvKeysUn 196 | std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 197 | 198 | 199 | // Camera pose. 200 | cv::Mat mTcw; 201 | 202 | // IMU linear velocity 203 | cv::Mat mVw; 204 | 205 | cv::Mat mPredRwb, mPredtwb, mPredVwb; 206 | IMU::Bias mPredBias; 207 | 208 | // IMU bias 209 | IMU::Bias mImuBias; 210 | 211 | // Imu calibration 212 | IMU::Calib mImuCalib; 213 | 214 | // Imu preintegration from last keyframe 215 | IMU::Preintegrated* mpImuPreintegrated; 216 | KeyFrame* mpLastKeyFrame; 217 | 218 | // Pointer to previous frame 219 | Frame* mpPrevFrame; 220 | IMU::Preintegrated* mpImuPreintegratedFrame; 221 | 222 | // Current and Next Frame id. 223 | static long unsigned int nNextId; 224 | long unsigned int mnId; 225 | 226 | // Reference Keyframe. 227 | KeyFrame* mpReferenceKF; 228 | 229 | // Scale pyramid info. 230 | int mnScaleLevels; 231 | float mfScaleFactor; 232 | float mfLogScaleFactor; 233 | // 1，1.2，1.44，... 234 | vector mvScaleFactors; 235 | vector mvInvScaleFactors; 236 | vector mvLevelSigma2; 237 | vector mvInvLevelSigma2; 238 | 239 | // Undistorted Image Bounds (computed once). 240 | static float mnMinX; 241 | static float mnMaxX; 242 | static float mnMinY; 243 | static float mnMaxY; 244 | 245 | static bool mbInitialComputations; 246 | 247 | map mmProjectPoints; 248 | map mmMatchedInImage; 249 | 250 | string mNameFile; 251 | 252 | int mnDataset; 253 | 254 | double mTimeStereoMatch; 255 | double mTimeORB_Ext; 256 | 257 | 258 | private: 259 | 260 | // Undistort keypoints given OpenCV distortion parameters. 261 | // Only for the RGB-D case. Stereo must be already rectified! 262 | // (called in the constructor). 263 | void UndistortKeyPoints(); 264 | 265 | // Computes image bounds for the undistorted image (called in the constructor). 266 | void ComputeImageBounds(const cv::Mat &imLeft); 267 | 268 | // Assign keypoints to the grid for speed up feature matching (called in the constructor). 269 | void AssignFeaturesToGrid(); 270 | 271 | // Rotation, translation and camera center 272 | cv::Mat mRcw; 273 | cv::Mat mtcw; 274 | //==mtwc 275 | 276 | bool mbImuPreintegrated; 277 | 278 | std::mutex *mpMutexImu; 279 | 280 | public: 281 | GeometricCamera* mpCamera, *mpCamera2; 282 | 283 | //Number of KeyPoints extracted in the left and right images 284 | // 285 | int Nleft, Nright; 286 | //Number of Non Lapping Keypoints 287 | int monoLeft, monoRight; 288 | 289 | //For stereo matching 290 | std::vector mvLeftToRightMatch, mvRightToLeftMatch; 291 | 292 | //For stereo fisheye matching 293 | static cv::BFMatcher BFmatcher; 294 | 295 | //Triangulated stereo observations using as reference the left camera. These are 296 | //computed during ComputeStereoFishEyeMatches 297 | std::vector mvStereo3Dpoints; 298 | 299 | //Grid for the right image 300 | std::vector mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 301 | 302 | cv::Mat mTlr, mRlr, mtlr, mTrl; 303 | 304 | 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, cv::Mat& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); 305 | 306 | //Stereo fisheye 307 | void ComputeStereoFishEyeMatches(); 308 | 309 | bool isInFrustumChecks(MapPoint* pMP, float viewingCosLimit, bool bRight = false); 310 | 311 | cv::Mat UnprojectStereoFishEye(const int &i); 312 | 313 | cv::Mat imgLeft, imgRight; 314 | 315 | void PrintPointDistribution(){ 316 | int left = 0, right = 0; 317 | int Nlim = (Nleft != -1) ? Nleft : N; 318 | for(int i = 0; i < N; i++){ 319 | if(mvpMapPoints[i] && !mvbOutlier[i]){ 320 | if(i < Nlim) left++; 321 | else right++; 322 | } 323 | } 324 | cout << "Point distribution in Frame: left-> " << left << " --- right-> " << right << endl; 325 | } 326 | }; 327 | 328 | }// namespace ORB_SLAM 329 | 330 | #endif // FRAME_H 331 | -------------------------------------------------------------------------------- /src/Viewer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #include "Viewer.h" 21 | #include 22 | 23 | #include 24 | 25 | namespace ORB_SLAM3 26 | { 27 | 28 | Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath): 29 | both(false), mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking), 30 | mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false) 31 | { 32 | cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); 33 | 34 | bool is_correct = ParseViewerParamFile(fSettings); 35 | 36 | if(!is_correct) 37 | { 38 | std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; 39 | try 40 | { 41 | throw -1; 42 | } 43 | catch(exception &e) 44 | { 45 | 46 | } 47 | } 48 | 49 | mbStopTrack = false; 50 | } 51 | 52 | bool Viewer::ParseViewerParamFile(cv::FileStorage &fSettings) 53 | { 54 | bool b_miss_params = false; 55 | 56 | float fps = fSettings["Camera.fps"]; 57 | if(fps<1) 58 | fps=30; 59 | mT = 1e3/fps; 60 | 61 | cv::FileNode node = fSettings["Camera.width"]; 62 | if(!node.empty()) 63 | { 64 | mImageWidth = node.real(); 65 | } 66 | else 67 | { 68 | std::cerr << "*Camera.width parameter doesn't exist or is not a real number*" << std::endl; 69 | b_miss_params = true; 70 | } 71 | 72 | node = fSettings["Camera.height"]; 73 | if(!node.empty()) 74 | { 75 | mImageHeight = node.real(); 76 | } 77 | else 78 | { 79 | std::cerr << "*Camera.height parameter doesn't exist or is not a real number*" << std::endl; 80 | b_miss_params = true; 81 | } 82 | 83 | node = fSettings["Viewer.ViewpointX"]; 84 | if(!node.empty()) 85 | { 86 | mViewpointX = node.real(); 87 | } 88 | else 89 | { 90 | std::cerr << "*Viewer.ViewpointX parameter doesn't exist or is not a real number*" << std::endl; 91 | b_miss_params = true; 92 | } 93 | 94 | node = fSettings["Viewer.ViewpointY"]; 95 | if(!node.empty()) 96 | { 97 | mViewpointY = node.real(); 98 | } 99 | else 100 | { 101 | std::cerr << "*Viewer.ViewpointY parameter doesn't exist or is not a real number*" << std::endl; 102 | b_miss_params = true; 103 | } 104 | 105 | node = fSettings["Viewer.ViewpointZ"]; 106 | if(!node.empty()) 107 | { 108 | mViewpointZ = node.real(); 109 | } 110 | else 111 | { 112 | std::cerr << "*Viewer.ViewpointZ parameter doesn't exist or is not a real number*" << std::endl; 113 | b_miss_params = true; 114 | } 115 | 116 | node = fSettings["Viewer.ViewpointF"]; 117 | if(!node.empty()) 118 | { 119 | mViewpointF = node.real(); 120 | } 121 | else 122 | { 123 | std::cerr << "*Viewer.ViewpointF parameter doesn't exist or is not a real number*" << std::endl; 124 | b_miss_params = true; 125 | } 126 | 127 | return !b_miss_params; 128 | } 129 | 130 | void Viewer::Run() 131 | { 132 | mbFinished = false; 133 | mbStopped = false; 134 | 135 | pangolin::CreateWindowAndBind("ORB-SLAM3: Map Viewer",1024,768); 136 | 137 | // 3D Mouse handler requires depth testing to be enabled 138 | glEnable(GL_DEPTH_TEST); 139 | 140 | // Issue specific OpenGl we might need 141 | glEnable (GL_BLEND); 142 | glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 143 | 144 | pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175)); 145 | pangolin::Var menuFollowCamera("menu.Follow Camera",true,true); 146 | pangolin::Var menuCamView("menu.Camera View",false,false); 147 | pangolin::Var menuTopView("menu.Top View",false,false); 148 | // pangolin::Var menuSideView("menu.Side View",false,false); 149 | pangolin::Var menuShowPoints("menu.Show Points",true,true); 150 | pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); 151 | pangolin::Var menuShowGraph("menu.Show Graph",false,true); 152 | pangolin::Var menuShowInertialGraph("menu.Show Inertial Graph",true,true); 153 | pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); 154 | pangolin::Var menuReset("menu.Reset",false,false); 155 | pangolin::Var menuStepByStep("menu.Step By Step",false,true); // false, true 156 | pangolin::Var menuStep("menu.Step",false,false); 157 | 158 | // Define Camera Render Object (for view / scene browsing) 159 | pangolin::OpenGlRenderState s_cam( 160 | pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000), 161 | pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0) 162 | ); 163 | 164 | // Add named OpenGL viewport to window and provide 3D Handler 165 | pangolin::View& d_cam = pangolin::CreateDisplay() 166 | .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f) 167 | .SetHandler(new pangolin::Handler3D(s_cam)); 168 | 169 | pangolin::OpenGlMatrix Twc, Twr; 170 | Twc.SetIdentity(); 171 | pangolin::OpenGlMatrix Ow; // Oriented with g in the z axis 172 | Ow.SetIdentity(); 173 | pangolin::OpenGlMatrix Twwp; // Oriented with g in the z axis, but y and x from camera 174 | Twwp.SetIdentity(); 175 | cv::namedWindow("ORB-SLAM3: Current Frame"); 176 | 177 | bool bFollow = true; 178 | bool bLocalizationMode = false; 179 | bool bStepByStep = false; 180 | bool bCameraView = true; 181 | 182 | if(mpTracker->mSensor == mpSystem->MONOCULAR || mpTracker->mSensor == mpSystem->STEREO || mpTracker->mSensor == mpSystem->RGBD) 183 | { 184 | menuShowGraph = true; 185 | } 186 | 187 | while(1) 188 | { 189 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 190 | 191 | mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc,Ow,Twwp); 192 | 193 | if(mbStopTrack) 194 | { 195 | menuStepByStep = true; 196 | mbStopTrack = false; 197 | } 198 | 199 | if(menuFollowCamera && bFollow) 200 | { 201 | if(bCameraView) 202 | s_cam.Follow(Twc); 203 | else 204 | s_cam.Follow(Ow); 205 | } 206 | else if(menuFollowCamera && !bFollow) 207 | { 208 | if(bCameraView) 209 | { 210 | s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000)); 211 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); 212 | s_cam.Follow(Twc); 213 | } 214 | else 215 | { 216 | s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); 217 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0)); 218 | s_cam.Follow(Ow); 219 | } 220 | bFollow = true; 221 | } 222 | else if(!menuFollowCamera && bFollow) 223 | { 224 | bFollow = false; 225 | } 226 | 227 | if(menuCamView) 228 | { 229 | menuCamView = false; 230 | bCameraView = true; 231 | s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,10000)); 232 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)); 233 | s_cam.Follow(Twc); 234 | } 235 | 236 | if(menuTopView && mpMapDrawer->mpAtlas->isImuInitialized()) 237 | { 238 | menuTopView = false; 239 | bCameraView = false; 240 | /*s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); 241 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));*/ 242 | s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); 243 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0)); 244 | s_cam.Follow(Ow); 245 | } 246 | 247 | /*if(menuSideView && mpMapDrawer->mpAtlas->isImuInitialized()) 248 | { 249 | s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); 250 | s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0.0,0.1,30.0,0,0,0,0.0,0.0,1.0)); 251 | s_cam.Follow(Twwp); 252 | }*/ 253 | 254 | 255 | if(menuLocalizationMode && !bLocalizationMode) 256 | { 257 | mpSystem->ActivateLocalizationMode(); 258 | bLocalizationMode = true; 259 | } 260 | else if(!menuLocalizationMode && bLocalizationMode) 261 | { 262 | mpSystem->DeactivateLocalizationMode(); 263 | bLocalizationMode = false; 264 | } 265 | 266 | if(menuStepByStep && !bStepByStep) 267 | { 268 | mpTracker->SetStepByStep(true); 269 | bStepByStep = true; 270 | } 271 | else if(!menuStepByStep && bStepByStep) 272 | { 273 | mpTracker->SetStepByStep(false); 274 | bStepByStep = false; 275 | } 276 | 277 | if(menuStep) 278 | { 279 | mpTracker->mbStep = true; 280 | menuStep = false; 281 | } 282 | 283 | 284 | d_cam.Activate(s_cam); 285 | glClearColor(1.0f,1.0f,1.0f,1.0f); 286 | mpMapDrawer->DrawCurrentCamera(Twc); 287 | if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph) 288 | mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph); 289 | if(menuShowPoints) 290 | mpMapDrawer->DrawMapPoints(); 291 | 292 | pangolin::FinishFrame(); 293 | 294 | cv::Mat toShow; 295 | cv::Mat im = mpFrameDrawer->DrawFrame(true); 296 | 297 | if(both){ 298 | cv::Mat imRight = mpFrameDrawer->DrawRightFrame(); 299 | cv::hconcat(im,imRight,toShow); 300 | } 301 | else{ 302 | toShow = im; 303 | } 304 | 305 | cv::imshow("ORB-SLAM3: Current Frame",toShow); 306 | cv::waitKey(mT); 307 | 308 | if(menuReset) 309 | { 310 | menuShowGraph = true; 311 | menuShowInertialGraph = true; 312 | menuShowKeyFrames = true; 313 | menuShowPoints = true; 314 | menuLocalizationMode = false; 315 | if(bLocalizationMode) 316 | mpSystem->DeactivateLocalizationMode(); 317 | bLocalizationMode = false; 318 | bFollow = true; 319 | menuFollowCamera = true; 320 | //mpSystem->Reset(); 321 | mpSystem->ResetActiveMap(); 322 | menuReset = false; 323 | } 324 | 325 | if(Stop()) 326 | { 327 | while(isStopped()) 328 | { 329 | usleep(3000); 330 | } 331 | } 332 | 333 | if(CheckFinish()) 334 | break; 335 | } 336 | 337 | SetFinish(); 338 | } 339 | 340 | void Viewer::RequestFinish() 341 | { 342 | unique_lock lock(mMutexFinish); 343 | mbFinishRequested = true; 344 | } 345 | 346 | bool Viewer::CheckFinish() 347 | { 348 | unique_lock lock(mMutexFinish); 349 | return mbFinishRequested; 350 | } 351 | 352 | void Viewer::SetFinish() 353 | { 354 | unique_lock lock(mMutexFinish); 355 | mbFinished = true; 356 | } 357 | 358 | bool Viewer::isFinished() 359 | { 360 | unique_lock lock(mMutexFinish); 361 | return mbFinished; 362 | } 363 | 364 | void Viewer::RequestStop() 365 | { 366 | unique_lock lock(mMutexStop); 367 | if(!mbStopped) 368 | mbStopRequested = true; 369 | } 370 | 371 | bool Viewer::isStopped() 372 | { 373 | unique_lock lock(mMutexStop); 374 | return mbStopped; 375 | } 376 | 377 | bool Viewer::Stop() 378 | { 379 | unique_lock lock(mMutexStop); 380 | unique_lock lock2(mMutexFinish); 381 | 382 | if(mbFinishRequested) 383 | return false; 384 | else if(mbStopRequested) 385 | { 386 | mbStopped = true; 387 | mbStopRequested = false; 388 | return true; 389 | } 390 | 391 | return false; 392 | 393 | } 394 | 395 | void Viewer::Release() 396 | { 397 | unique_lock lock(mMutexStop); 398 | mbStopped = false; 399 | } 400 | 401 | void Viewer::SetTrackingPause() 402 | { 403 | mbStopTrack = true; 404 | } 405 | 406 | } 407 | -------------------------------------------------------------------------------- /src/Sim3Solver.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | 20 | #include "Sim3Solver.h" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "KeyFrame.h" 27 | #include "ORBmatcher.h" 28 | 29 | #include "Thirdparty/DBoW2/DUtils/Random.h" 30 | 31 | namespace ORB_SLAM3 32 | { 33 | 34 | /** 35 | * 三对3d-3d点，计算sim3变换 36 | */ 37 | 38 | Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12, const bool bFixScale, 39 | vector vpKeyFrameMatchedMP): 40 | mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), 41 | pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) 42 | { 43 | bool bDifferentKFs = false; 44 | if(vpKeyFrameMatchedMP.empty()) 45 | { 46 | bDifferentKFs = true; 47 | vpKeyFrameMatchedMP = vector(vpMatched12.size(), pKF2); 48 | } 49 | 50 | mpKF1 = pKF1; 51 | mpKF2 = pKF2; 52 | 53 | vector vpKeyFrameMP1 = pKF1->GetMapPointMatches(); 54 | 55 | mN1 = vpMatched12.size(); 56 | 57 | mvpMapPoints1.reserve(mN1); 58 | mvpMapPoints2.reserve(mN1); 59 | mvpMatches12 = vpMatched12; 60 | mvnIndices1.reserve(mN1); 61 | mvX3Dc1.reserve(mN1); 62 | mvX3Dc2.reserve(mN1); 63 | 64 | cv::Mat Rcw1 = pKF1->GetRotation(); 65 | cv::Mat tcw1 = pKF1->GetTranslation(); 66 | cv::Mat Rcw2 = pKF2->GetRotation(); 67 | cv::Mat tcw2 = pKF2->GetTranslation(); 68 | 69 | mvAllIndices.reserve(mN1); 70 | 71 | size_t idx=0; 72 | 73 | KeyFrame* pKFm = pKF2; //Default variable 74 | for(int i1=0; i1isBad() || pMP2->isBad()) 85 | continue; 86 | 87 | if(bDifferentKFs) 88 | pKFm = vpKeyFrameMatchedMP[i1]; 89 | 90 | int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1)); 91 | int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm)); 92 | 93 | if(indexKF1<0 || indexKF2<0) 94 | continue; 95 | 96 | const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1]; 97 | const cv::KeyPoint &kp2 = pKFm->mvKeysUn[indexKF2]; 98 | 99 | const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave]; 100 | const float sigmaSquare2 = pKFm->mvLevelSigma2[kp2.octave]; 101 | 102 | mvnMaxError1.push_back(9.210*sigmaSquare1); 103 | mvnMaxError2.push_back(9.210*sigmaSquare2); 104 | 105 | mvpMapPoints1.push_back(pMP1); 106 | mvpMapPoints2.push_back(pMP2); 107 | mvnIndices1.push_back(i1); 108 | 109 | cv::Mat X3D1w = pMP1->GetWorldPos(); 110 | mvX3Dc1.push_back(Rcw1*X3D1w+tcw1); 111 | 112 | cv::Mat X3D2w = pMP2->GetWorldPos(); 113 | mvX3Dc2.push_back(Rcw2*X3D2w+tcw2); 114 | 115 | mvAllIndices.push_back(idx); 116 | idx++; 117 | } 118 | } 119 | 120 | mK1 = pKF1->mK; 121 | mK2 = pKF2->mK; 122 | 123 | FromCameraToImage(mvX3Dc1,mvP1im1,pCamera1); 124 | FromCameraToImage(mvX3Dc2,mvP2im2,pCamera2); 125 | 126 | SetRansacParameters(); 127 | } 128 | 129 | void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations) 130 | { 131 | mRansacProb = probability; 132 | mRansacMinInliers = minInliers; 133 | mRansacMaxIts = maxIterations; 134 | 135 | N = mvpMapPoints1.size(); // number of correspondences 136 | 137 | mvbInliersi.resize(N); 138 | 139 | // Adjust Parameters according to number of correspondences 140 | float epsilon = (float)mRansacMinInliers/N; 141 | 142 | // Set RANSAC iterations according to probability, epsilon, and max iterations 143 | int nIterations; 144 | 145 | if(mRansacMinInliers==N) 146 | nIterations=1; 147 | else 148 | nIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3))); 149 | 150 | mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts)); 151 | 152 | mnIterations = 0; 153 | } 154 | 155 | cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers) 156 | { 157 | bNoMore = false; 158 | vbInliers = vector(mN1,false); 159 | nInliers=0; 160 | 161 | if(N vAvailableIndices; 168 | 169 | cv::Mat P3Dc1i(3,3,CV_32F); 170 | cv::Mat P3Dc2i(3,3,CV_32F); 171 | 172 | int nCurrentIterations = 0; 173 | while(mnIterations=mnBestInliers) 199 | { 200 | mvbBestInliers = mvbInliersi; 201 | mnBestInliers = mnInliersi; 202 | mBestT12 = mT12i.clone(); 203 | mBestRotation = mR12i.clone(); 204 | mBestTranslation = mt12i.clone(); 205 | mBestScale = ms12i; 206 | 207 | if(mnInliersi>mRansacMinInliers) 208 | { 209 | nInliers = mnInliersi; 210 | for(int i=0; i=mRansacMaxIts) 219 | bNoMore=true; 220 | 221 | return cv::Mat(); 222 | } 223 | 224 | cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, bool &bConverge) 225 | { 226 | bNoMore = false; 227 | bConverge = false; 228 | vbInliers = vector(mN1,false); 229 | nInliers=0; 230 | 231 | if(N vAvailableIndices; 238 | 239 | cv::Mat P3Dc1i(3,3,CV_32F); 240 | cv::Mat P3Dc2i(3,3,CV_32F); 241 | 242 | int nCurrentIterations = 0; 243 | 244 | cv::Mat bestSim3; 245 | 246 | while(mnIterations=mnBestInliers) 272 | { 273 | mvbBestInliers = mvbInliersi; 274 | mnBestInliers = mnInliersi; 275 | mBestT12 = mT12i.clone(); 276 | mBestRotation = mR12i.clone(); 277 | mBestTranslation = mt12i.clone(); 278 | mBestScale = ms12i; 279 | 280 | if(mnInliersi>mRansacMinInliers) 281 | { 282 | nInliers = mnInliersi; 283 | for(int i=0; i=mRansacMaxIts) 297 | bNoMore=true; 298 | 299 | return bestSim3; 300 | } 301 | 302 | cv::Mat Sim3Solver::find(vector &vbInliers12, int &nInliers) 303 | { 304 | bool bFlag; 305 | return iterate(mRansacMaxIts,bFlag,vbInliers12,nInliers); 306 | } 307 | 308 | /** 309 | * P是3个点，求质心C，去中心得到Pr 310 | */ 311 | void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C) 312 | { 313 | // 每一行求和，存C 314 | cv::reduce(P,C,1,CV_REDUCE_SUM); 315 | // 求均值 316 | C = C/P.cols; 317 | 318 | for(int i=0; i(0,0)+M.at(1,1)+M.at(2,2); 353 | N12 = M.at(1,2)-M.at(2,1); 354 | N13 = M.at(2,0)-M.at(0,2); 355 | N14 = M.at(0,1)-M.at(1,0); 356 | N22 = M.at(0,0)-M.at(1,1)-M.at(2,2); 357 | N23 = M.at(0,1)+M.at(1,0); 358 | N24 = M.at(2,0)+M.at(0,2); 359 | N33 = -M.at(0,0)+M.at(1,1)-M.at(2,2); 360 | N34 = M.at(1,2)+M.at(2,1); 361 | N44 = -M.at(0,0)-M.at(1,1)+M.at(2,2); 362 | 363 | N = (cv::Mat_(4,4) << N11, N12, N13, N14, 364 | N12, N22, N23, N24, 365 | N13, N23, N33, N34, 366 | N14, N24, N34, N44); 367 | 368 | 369 | // Step 4: Eigenvector of the highest eigenvalue 370 | 371 | cv::Mat eval, evec; 372 | 373 | cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotation 374 | 375 | cv::Mat vec(1,3,evec.type()); 376 | (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis) 377 | 378 | // Rotation angle. sin is the norm of the imaginary part, cos is the real part 379 | double ang=atan2(norm(vec),evec.at(0,0)); 380 | 381 | vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half 382 | 383 | mR12i.create(3,3,P1.type()); 384 | 385 | cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis 386 | 387 | // Step 5: Rotate set 2 388 | 389 | cv::Mat P3 = mR12i*Pr2; 390 | 391 | // Step 6: Scale 392 | 393 | if(!mbFixScale) 394 | { 395 | double nom = Pr1.dot(P3); 396 | cv::Mat aux_P3(P3.size(),P3.type()); 397 | aux_P3=P3; 398 | cv::pow(P3,2,aux_P3); 399 | double den = 0; 400 | 401 | for(int i=0; i(i,j); 406 | } 407 | } 408 | 409 | ms12i = nom/den; 410 | } 411 | else 412 | ms12i = 1.0f; 413 | 414 | // Step 7: Translation 415 | 416 | mt12i.create(1,3,P1.type()); 417 | mt12i = O1 - ms12i*mR12i*O2; 418 | 419 | // Step 8: Transformation 420 | 421 | // Step 8.1 T12 422 | mT12i = cv::Mat::eye(4,4,P1.type()); 423 | 424 | cv::Mat sR = ms12i*mR12i; 425 | 426 | sR.copyTo(mT12i.rowRange(0,3).colRange(0,3)); 427 | mt12i.copyTo(mT12i.rowRange(0,3).col(3)); 428 | 429 | // Step 8.2 T21 430 | 431 | mT21i = cv::Mat::eye(4,4,P1.type()); 432 | 433 | cv::Mat sRinv = (1.0/ms12i)*mR12i.t(); 434 | 435 | sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3)); 436 | cv::Mat tinv = -sRinv*mt12i; 437 | tinv.copyTo(mT21i.rowRange(0,3).col(3)); 438 | } 439 | 440 | 441 | void Sim3Solver::CheckInliers() 442 | { 443 | vector vP1im2, vP2im1; 444 | Project(mvX3Dc2,vP2im1,mT12i,pCamera1); 445 | Project(mvX3Dc1,vP1im2,mT21i,pCamera2); 446 | 447 | mnInliersi=0; 448 | 449 | for(size_t i=0; i &vP3Dw, vector &vP2D, cv::Mat Tcw, GeometricCamera* pCamera) 484 | { 485 | cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3); 486 | cv::Mat tcw = Tcw.rowRange(0,3).col(3); 487 | 488 | vP2D.clear(); 489 | vP2D.reserve(vP3Dw.size()); 490 | 491 | for(size_t i=0, iend=vP3Dw.size(); i(2)); 495 | const float x = P3Dc.at(0); 496 | const float y = P3Dc.at(1); 497 | const float z = P3Dc.at(2); 498 | 499 | vP2D.push_back(pCamera->projectMat(cv::Point3f(x,y,z))); 500 | } 501 | } 502 | 503 | void Sim3Solver::FromCameraToImage(const vector &vP3Dc, vector &vP2D, GeometricCamera* pCamera) 504 | { 505 | vP2D.clear(); 506 | vP2D.reserve(vP3Dc.size()); 507 | 508 | for(size_t i=0, iend=vP3Dc.size(); i(2)); 511 | const float x = vP3Dc[i].at(0); 512 | const float y = vP3Dc[i].at(1); 513 | const float z = vP3Dc[i].at(2); 514 | 515 | vP2D.push_back(pCamera->projectMat(cv::Point3f(x,y,z))); 516 | } 517 | } 518 | 519 | } //namespace ORB_SLAM 520 | --------------------------------------------------------------------------------