看到网上介绍ORB-SLAM2基本上介绍一个系统的框架或者按照线程Tracking,Local Mapping, Loop Closing介绍大致流程但是细节讲的不是很多只能说懂个大概原理。LZ还是自己慢慢阅读吧虽然这样的代码注释有点啰嗦请高手勿喷_在ORB-SLAM2中头文件中作者都写的很清楚各个函数的定义这个LZ就不在赘述了。#ifndef FRAME_H #define FRAME_H #includevector #include MapPoint.h #include Thirdparty/DBoW2/DBoW2/BowVector.h #include Thirdparty/DBoW2/DBoW2/FeatureVector.h #include ORBVocabulary.h #include KeyFrame.h #include ORBextractor.h #include opencv2/opencv.hpp namespace ORB_SLAM2 { #define FRAME_GRID_ROWS 48 #define FRAME_GRID_COLS 64 class MapPoint; class KeyFrame; class Frame { public: Frame(); // Copy constructor. Frame(const Frame frame); // Constructor for stereo cameras. 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); // Constructor for RGB-D cameras. 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); // Constructor for Monocular cameras. Frame(const cv::Mat imGray, const double timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat K, cv::Mat distCoef, const float bf, const float thDepth); // Extract ORB on the image. 0 for left image and 1 for right image. void ExtractORB(int flag, const cv::Mat im); // Compute Bag of Words representation. void ComputeBoW(); // Set the camera pose. void SetPose(cv::Mat Tcw); // Computes rotation, translation and camera center matrices from the camera pose. void UpdatePoseMatrices(); // Returns the camera center. inline cv::Mat GetCameraCenter(){ return mOw.clone(); } // Returns inverse of rotation inline cv::Mat GetRotationInverse(){ return mRwc.clone(); } // Check if a MapPoint is in the frustum of the camera // and fill variables of the MapPoint to be used by the tracking bool isInFrustum(MapPoint* pMP, float viewingCosLimit); // Compute the cell of a keypoint (return false if outside the grid) bool PosInGrid(const cv::KeyPoint kp, int posX, int posY); vectorsize_t GetFeaturesInArea(const float x, const float y, const float r, const int minLevel-1, const int maxLevel-1) const; // Search a match for each keypoint in the left image to a keypoint in the right image. // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored. void ComputeStereoMatches(); // Associate a right coordinate to a keypoint if there is valid depth in the depthmap. void ComputeStereoFromRGBD(const cv::Mat imDepth); // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates. cv::Mat UnprojectStereo(const int i); public: // Vocabulary used for relocalization. ORBVocabulary* mpORBvocabulary; // Feature extractor. The right is used only in the stereo case. ORBextractor* mpORBextractorLeft, *mpORBextractorRight; // Frame timestamp. double mTimeStamp; // Calibration matrix and OpenCV distortion parameters. cv::Mat mK; static float fx; static float fy; static float cx; static float cy; static float invfx; static float invfy; cv::Mat mDistCoef; // Stereo baseline multiplied by fx. float mbf; // Stereo baseline in meters. float mb; // Threshold close/far points. Close points are inserted from 1 view. // Far points are inserted as in the monocular case from 2 views. float mThDepth; // Number of KeyPoints. int N; // Vector of keypoints (original for visualization) and undistorted (actually used by the system). // In the stereo case, mvKeysUn is redundant as images must be rectified. // In the RGB-D case, RGB images can be distorted. std::vectorcv::KeyPoint mvKeys, mvKeysRight; std::vectorcv::KeyPoint mvKeysUn; // Corresponding stereo coordinate and depth for each keypoint. // Monocular keypoints have a negative value. std::vectorfloat mvuRight; std::vectorfloat mvDepth; // Bag of Words Vector structures. DBoW2::BowVector mBowVec; DBoW2::FeatureVector mFeatVec; // ORB descriptor, each row associated to a keypoint. cv::Mat mDescriptors, mDescriptorsRight; // MapPoints associated to keypoints, NULL pointer if no association. std::vectorMapPoint* mvpMapPoints; // Flag to identify outlier associations. std::vectorbool mvbOutlier; // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints. static float mfGridElementWidthInv; static float mfGridElementHeightInv; std::vectorstd::size_t mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; // Camera pose. cv::Mat mTcw; // Current and Next Frame id. static long unsigned int nNextId; long unsigned int mnId; // Reference Keyframe. KeyFrame* mpReferenceKF; // Scale pyramid info. int mnScaleLevels; float mfScaleFactor; float mfLogScaleFactor; vectorfloat mvScaleFactors; vectorfloat mvInvScaleFactors; vectorfloat mvLevelSigma2; vectorfloat mvInvLevelSigma2; // Undistorted Image Bounds (computed once). static float mnMinX; static float mnMaxX; static float mnMinY; static float mnMaxY; static bool mbInitialComputations; private: // Undistort keypoints given OpenCV distortion parameters. // Only for the RGB-D case. Stereo must be already rectified! // (called in the constructor). void UndistortKeyPoints(); // Computes image bounds for the undistorted image (called in the constructor). void ComputeImageBounds(const cv::Mat imLeft); // Assign keypoints to the grid for speed up feature matching (called in the constructor). void AssignFeaturesToGrid(); // Rotation, translation and camera center cv::Mat mRcw; cv::Mat mtcw; cv::Mat mRwc; cv::Mat mOw; //mtwc }; }// namespace ORB_SLAM #endif // FRAME_H#include Frame.h #include Converter.h #include ORBmatcher.h #include thread namespace ORB_SLAM2 { long unsigned int Frame::nNextId0; bool Frame::mbInitialComputationstrue; float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy; float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; Frame::Frame() {} //Copy Constructor Frame::Frame(const Frame frame) :mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight), mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()), mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys), mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn), mvuRight(frame.mvuRight), mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec), mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()), mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mnId(frame.mnId), mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels), mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor), mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2) { for(int i0;iFRAME_GRID_COLS;i) for(int j0; jFRAME_GRID_ROWS; j) mGrid[i][j]frame.mGrid[i][j]; if(!frame.mTcw.empty()) SetPose(frame.mTcw); } //双目构建的Frame对象 Frame::Frame(const cv::Mat imLeft, const cv::Mat imRight, const double timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat K, cv::Mat distCoef, const float bf, const float thDepth) :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_castKeyFrame*(NULL)) { // Frame ID mnIdnNextId; // Scale Level Info mnScaleLevels mpORBextractorLeft-GetLevels(); mfScaleFactor mpORBextractorLeft-GetScaleFactor(); mfLogScaleFactor log(mfScaleFactor); mvScaleFactors mpORBextractorLeft-GetScaleFactors(); mvInvScaleFactors mpORBextractorLeft-GetInverseScaleFactors(); mvLevelSigma2 mpORBextractorLeft-GetScaleSigmaSquares(); mvInvLevelSigma2 mpORBextractorLeft-GetInverseScaleSigmaSquares(); // ORB extraction //提取特征加入双线程同步提取0,1代表左目和右目 //两张提取的特征点会放在不同的vector中 //对单目和RGBD来说右目不用以左为准 thread threadLeft(Frame::ExtractORB,this,0,imLeft); thread threadRight(Frame::ExtractORB,this,1,imRight); //该函数在线程执行完成是返回 //在调用这个函数之后线程对象变为不可连接的并且可以被安全地销毁 threadLeft.join(); threadRight.join(); // N为特征点的数量 N mvKeys.size(); //如果提取的特征点数目为0则直接返回 if(mvKeys.empty()) return; UndistortKeyPoints(); ComputeStereoMatches(); //初始化地图点及其外点 mvpMapPoints vectorMapPoint*(N,static_castMapPoint*(NULL)); mvbOutlier vectorbool(N,false); // This is done only for the first Frame (or after a change in the calibration) if(mbInitialComputations) { ComputeImageBounds(imLeft); mfGridElementWidthInvstatic_castfloat(FRAME_GRID_COLS)/(mnMaxX-mnMinX); mfGridElementHeightInvstatic_castfloat(FRAME_GRID_ROWS)/(mnMaxY-mnMinY); fx K.atfloat(0,0); fy K.atfloat(1,1); cx K.atfloat(0,2); cy K.atfloat(1,2); invfx 1.0f/fx; invfy 1.0f/fy; mbInitialComputationsfalse; } mb mbf/fx; AssignFeaturesToGrid(); } //RGBD构建的Frame对象基本上和双目类似,只需要恢复出右图深度为正的深度即可 Frame::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) :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_castORBextractor*(NULL)), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) { // Frame ID mnIdnNextId; // Scale Level Info mnScaleLevels mpORBextractorLeft-GetLevels(); mfScaleFactor mpORBextractorLeft-GetScaleFactor(); mfLogScaleFactor log(mfScaleFactor); mvScaleFactors mpORBextractorLeft-GetScaleFactors(); mvInvScaleFactors mpORBextractorLeft-GetInverseScaleFactors(); mvLevelSigma2 mpORBextractorLeft-GetScaleSigmaSquares(); mvInvLevelSigma2 mpORBextractorLeft-GetInverseScaleSigmaSquares(); // ORB extraction ExtractORB(0,imGray); N mvKeys.size(); if(mvKeys.empty()) return; UndistortKeyPoints(); ComputeStereoFromRGBD(imDepth); mvpMapPoints vectorMapPoint*(N,static_castMapPoint*(NULL)); mvbOutlier vectorbool(N,false); // This is done only for the first Frame (or after a change in the calibration) if(mbInitialComputations) { ComputeImageBounds(imGray); mfGridElementWidthInvstatic_castfloat(FRAME_GRID_COLS)/static_castfloat(mnMaxX-mnMinX); mfGridElementHeightInvstatic_castfloat(FRAME_GRID_ROWS)/static_castfloat(mnMaxY-mnMinY); fx K.atfloat(0,0); fy K.atfloat(1,1); cx K.atfloat(0,2); cy K.atfloat(1,2); invfx 1.0f/fx; invfy 1.0f/fy; mbInitialComputationsfalse; } mb mbf/fx; AssignFeaturesToGrid(); } //单目构建的Frame类和双目类似但是不包含匹配信息 Frame::Frame(const cv::Mat imGray, const double timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat K, cv::Mat distCoef, const float bf, const float thDepth) :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_castORBextractor*(NULL)), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) { // Frame ID mnIdnNextId; // Scale Level Info mnScaleLevels mpORBextractorLeft-GetLevels(); mfScaleFactor mpORBextractorLeft-GetScaleFactor(); mfLogScaleFactor log(mfScaleFactor); mvScaleFactors mpORBextractorLeft-GetScaleFactors(); mvInvScaleFactors mpORBextractorLeft-GetInverseScaleFactors(); mvLevelSigma2 mpORBextractorLeft-GetScaleSigmaSquares(); mvInvLevelSigma2 mpORBextractorLeft-GetInverseScaleSigmaSquares(); // ORB extraction ExtractORB(0,imGray); N mvKeys.size(); if(mvKeys.empty()) return; UndistortKeyPoints(); // Set no stereo information mvuRight vectorfloat(N,-1); mvDepth vectorfloat(N,-1); mvpMapPoints vectorMapPoint*(N,static_castMapPoint*(NULL)); mvbOutlier vectorbool(N,false); // This is done only for the first Frame (or after a change in the calibration) if(mbInitialComputations) { ComputeImageBounds(imGray); mfGridElementWidthInvstatic_castfloat(FRAME_GRID_COLS)/static_castfloat(mnMaxX-mnMinX); mfGridElementHeightInvstatic_castfloat(FRAME_GRID_ROWS)/static_castfloat(mnMaxY-mnMinY); fx K.atfloat(0,0); fy K.atfloat(1,1); cx K.atfloat(0,2); cy K.atfloat(1,2); invfx 1.0f/fx; invfy 1.0f/fy; mbInitialComputationsfalse; } mb mbf/fx; AssignFeaturesToGrid(); } //将关键点分布到64*48分割而成的网格中为了加速匹配和均匀化关键点分布 void Frame::AssignFeaturesToGrid() { //这里为什么要乘以0.5f int nReserve 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS); for(unsigned int i0; iFRAME_GRID_COLS;i) for (unsigned int j0; jFRAME_GRID_ROWS;j) mGrid[i][j].reserve(nReserve); for(int i0;iN;i) { const cv::KeyPoint kp mvKeysUn[i]; int nGridPosX, nGridPosY; if(PosInGrid(kp,nGridPosX,nGridPosY)) mGrid[nGridPosX][nGridPosY].push_back(i); } } //对输入的图像提取0RB的特征 void Frame::ExtractORB(int flag, const cv::Mat im) { if(flag0) (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors); else (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight); } void Frame::SetPose(cv::Mat Tcw) { mTcw Tcw.clone(); UpdatePoseMatrices(); } void Frame::UpdatePoseMatrices() { mRcw mTcw.rowRange(0,3).colRange(0,3); mRwc mRcw.t(); mtcw mTcw.rowRange(0,3).col(3); mOw -mRcw.t()*mtcw; } //设置当前帧的姿态并更新当前帧相机在世界坐标系下的位姿中心点位置 // 判断一个MapPoint是否在视角范围内 bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) { //注意这里的MapPoint是从SearchLocalPoint传递进来的具备一定信息量 pMP-mbTrackInView false; // 3D in absolute coordinates cv::Mat P pMP-GetWorldPos(); // 3D in camera coordinates const cv::Mat Pc mRcw*Pmtcw; //这里的Rt是经过初步优化后 const float PcX Pc.atfloat(0); const float PcY Pc.atfloat(1); const float PcZ Pc.atfloat(2); // Check positive depth if(PcZ0.0f) return false; // Project in image and check it is not outside const float invz 1.0f/PcZ; const float ufx*PcX*invzcx; const float vfy*PcY*invzcy; if(umnMinX || umnMaxX) return false; if(vmnMinY || vmnMaxY) return false; // Check distance is in the scale invariance region of the MapPoint // 每一个地图点都是对应于若干尺度的金字塔提取出来的具有一定的有效深度 // 如果相对当前帧的深度超过此范围返回false const float maxDistance pMP-GetMaxDistanceInvariance(); const float minDistance pMP-GetMinDistanceInvariance(); // 世界坐标系下相机到3D点P的向量向量方向由相机只想3D点P const cv::Mat PO P-mOw; const float dist cv::norm(PO); if(distminDistance || distmaxDistance) return false; // Check viewing angle // 每一个地图都有其平均视角是从能够观测到地图点的帧位姿中计算出 // 如果当前帧的视角和其平均视角相差太大返回False cv::Mat Pn pMP-GetNormal();//|Pn| 1 const float viewCos PO.dot(Pn)/dist; // P0.dot(Pn)/(|P0|*|Pn|);|P0| dist if(viewCosviewingCosLimit) return false; // Predict scale in the image //根据深度预测尺度对应特征点在一层 const int nPredictedLevel pMP-PredictScale(dist,this); //|pn| 1 // Data used by the tracking //标记该点要被投影 pMP-mbTrackInView true; pMP-mTrackProjX u; pMP-mTrackProjXR u - mbf*invz;//该3D点投影到双目右侧相机的横坐标 pMP-mTrackProjY v; pMP-mnTrackScaleLevel nPredictedLevel; pMP-mTrackViewCos viewCos; return true; } //在某块区域内获取特征点 //minLevel和maxLevel考察特征点是从图像金字塔的哪一层提取出来的 vectorsize_t Frame::GetFeaturesInArea(const float x, const float y, const float r, const int minLevel, const int maxLevel) const { vectorsize_t vIndices; vIndices.reserve(N); const int nMinCellX max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); if(nMinCellXFRAME_GRID_COLS) return vIndices; const int nMaxCellX min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinXr)*mfGridElementWidthInv)); if(nMaxCellX0) return vIndices; const int nMinCellY max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv)); if(nMinCellYFRAME_GRID_ROWS) return vIndices; const int nMaxCellY min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinYr)*mfGridElementHeightInv)); if(nMaxCellY0) return vIndices; const bool bCheckLevels (minLevel0) || (maxLevel0); for(int ix nMinCellX; ixnMaxCellX; ix) { for(int iy nMinCellY; iynMaxCellY; iy) { const vectorsize_t vCell mGrid[ix][iy]; if(vCell.empty()) continue; for(size_t j0, jendvCell.size(); jjend; j) { const cv::KeyPoint kpUn mvKeysUn[vCell[j]]; if(bCheckLevels) { if(kpUn.octaveminLevel) continue; if(maxLevel0) if(kpUn.octavemaxLevel) continue; } const float distx kpUn.pt.x-x; const float disty kpUn.pt.y-y; if(fabs(distx)r fabs(disty)r) vIndices.push_back(vCell[j]); } } } return vIndices; } //判断特征点是否在区域内 bool Frame::PosInGrid(const cv::KeyPoint kp, int posX, int posY) { posX round((kp.pt.x-mnMinX)*mfGridElementWidthInv); posY round((kp.pt.y-mnMinY)*mfGridElementHeightInv); //Keypoints coordinates are undistorted, which could cause to go out of the image if(posX0 || posXFRAME_GRID_COLS || posY0 || posYFRAME_GRID_ROWS) return false; return true; } //将当前帧的描述子矩阵可以转换成向量转换成词袋模型向量 void Frame::ComputeBoW() { if(mBowVec.empty()) { vectorcv::Mat vCurrentDesc Converter::toDescriptorVector(mDescriptors); mpORBvocabulary-transform(vCurrentDesc,mBowVec,mFeatVec,4); } } //通过OpenCV给定的畸变参数给定未畸变的关键点 //只针对RGBD的例子双目的例子一定是要进行校正过的 //在构造函数中我们可以看到该函数被调用 void Frame::UndistortKeyPoints() { //判断是否给定校正参数 if(mDistCoef.atfloat(0)0.0) { mvKeysUnmvKeys; return; } // Fill matrix with points cv::Mat mat(N,2,CV_32F); for(int i0; iN; i) { mat.atfloat(i,0)mvKeys[i].pt.x; mat.atfloat(i,1)mvKeys[i].pt.y; } // Undistort points //使用OpenCV进行校正 matmat.reshape(2); cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); matmat.reshape(1); // Fill undistorted keypoint vector // 重新存储已经去畸变后的特征点坐标 mvKeysUn.resize(N); for(int i0; iN; i) { cv::KeyPoint kp mvKeys[i]; kp.pt.xmat.atfloat(i,0); kp.pt.ymat.atfloat(i,1); mvKeysUn[i]kp; } } //计算图像的边界 void Frame::ComputeImageBounds(const cv::Mat imLeft) { if(mDistCoef.atfloat(0)!0.0) { cv::Mat mat(4,2,CV_32F); mat.atfloat(0,0)0.0; mat.atfloat(0,1)0.0; mat.atfloat(1,0)imLeft.cols; mat.atfloat(1,1)0.0; mat.atfloat(2,0)0.0; mat.atfloat(2,1)imLeft.rows; mat.atfloat(3,0)imLeft.cols; mat.atfloat(3,1)imLeft.rows; // Undistort corners matmat.reshape(2); cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); matmat.reshape(1); mnMinX min(mat.atfloat(0,0),mat.atfloat(2,0)); mnMaxX max(mat.atfloat(1,0),mat.atfloat(3,0)); mnMinY min(mat.atfloat(0,1),mat.atfloat(1,1)); mnMaxY max(mat.atfloat(2,1),mat.atfloat(3,1)); } else { mnMinX 0.0f; mnMaxX imLeft.cols; mnMinY 0.0f; mnMaxY imLeft.rows; } } //进行立体匹配 //找到关键点在左右两幅图之间的匹配 //如果存在匹配计算出深度并存储于左图关键点相关的右图中关键点的坐标 void Frame::ComputeStereoMatches() { //先对要存储的信息进行初始化 mvuRight vectorfloat(N,-1.0f); mvDepth vectorfloat(N,-1.0f); //这里在matcher中设定的阈值TH_HIGH 100 TH_LOW 50这个阈值怎么来的具体还不是很清楚 const int thOrbDist (ORBmatcher::TH_HIGHORBmatcher::TH_LOW)/2; //获得原始图像的行数 const int nRows mpORBextractorLeft-mvImagePyramid[0].rows; //Assign keypoints to row table //构造n个vectorsizt_t类型的vector vectorvectorsize_t vRowIndices(nRows,vectorsize_t()); //每行最多装200个特征点 for(int i0; inRows; i) vRowIndices[i].reserve(200); // 右图特征点的数量 const int Nr mvKeysRight.size(); //https://www.cnblogs.com/shang-slam/p/6393419.html参考这个网址的解释 //在匹配左右帧的特征点时虽然已经过了极线矫正但是不能仅仅搜索极线对应的同一行像素点 //而应该根据右目提取特征点时的尺度金字塔层数确定一个极线附近的扫描范围r //这个带状范围内均包含这个特征信息这个就是r计算的算法 for(int iR0; iRNr; iR) { const cv::KeyPoint kp mvKeysRight[iR]; const float kpY kp.pt.y; const float r 2.0f*mvScaleFactors[mvKeysRight[iR].octave]; const int maxr ceil(kpYr); const int minr floor(kpY-r); for(int yiminr;yimaxr;yi) vRowIndices[yi].push_back(iR); } // Set limits for search //双目视觉中基线的长度单位是m const float minZ mb; const float minD 0; const float maxD mbf/minZ; // For each left keypoint search a match in the right image vectorpairint, int vDistIdx; vDistIdx.reserve(N); for(int iL0; iLN; iL) { const cv::KeyPoint kpL mvKeys[iL]; const int levelL kpL.octave; const float vL kpL.pt.y; const float uL kpL.pt.x; const vectorsize_t vCandidates vRowIndices[vL]; if(vCandidates.empty()) continue; const float minU uL-maxD; const float maxU uL-minD; if(maxU0) continue; int bestDist ORBmatcher::TH_HIGH; size_t bestIdxR 0; const cv::Mat dL mDescriptors.row(iL); // Compare descriptor to right keypoints //通过描述子进行特征点匹配得到每个特征点最佳匹配点scaleduR for(size_t iC0; iCvCandidates.size(); iC) { const size_t iR vCandidates[iC]; const cv::KeyPoint kpR mvKeysRight[iR]; if(kpR.octavelevelL-1 || kpR.octavelevelL1) continue; const float uR kpR.pt.x; if(uRminU uRmaxU) { const cv::Mat dR mDescriptorsRight.row(iR); const int dist ORBmatcher::DescriptorDistance(dL,dR); if(distbestDist) { bestDist dist; bestIdxR iR; } } } // Subpixel match by correlation //通过SAD滑窗得到匹配修正量bestincR if(bestDistthOrbDist) { // coordinates in image pyramid at keypoint scale const float uR0 mvKeysRight[bestIdxR].pt.x; const float scaleFactor mvInvScaleFactors[kpL.octave]; const float scaleduL round(kpL.pt.x*scaleFactor); const float scaledvL round(kpL.pt.y*scaleFactor); const float scaleduR0 round(uR0*scaleFactor); // sliding window search const int w 5; cv::Mat IL mpORBextractorLeft-mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvLw1).colRange(scaleduL-w,scaleduLw1); IL.convertTo(IL,CV_32F); IL IL - IL.atfloat(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F); int bestDist INT_MAX; int bestincR 0; const int L 5; vectorfloat vDists; vDists.resize(2*L1); const float iniu scaleduR0L-w; const float endu scaleduR0Lw1; if(iniu0 || endu mpORBextractorRight-mvImagePyramid[kpL.octave].cols) continue; for(int incR-L; incRL; incR) { cv::Mat IR mpORBextractorRight-mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvLw1).colRange(scaleduR0incR-w,scaleduR0incRw1); IR.convertTo(IR,CV_32F); IR IR - IR.atfloat(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F); float dist cv::norm(IL,IR,cv::NORM_L1); if(distbestDist) { bestDist dist; bestincR incR; } vDists[LincR] dist; } if(bestincR-L || bestincRL) continue; //(bestincR,dist) (bestincR-1,dist) (bestincR1,dist)三个点拟合出抛 //物线得到亚像素修正量deltaR // Sub-pixel match (Parabola fitting) const float dist1 vDists[LbestincR-1]; const float dist2 vDists[LbestincR]; const float dist3 vDists[LbestincR1]; const float deltaR (dist1-dist3)/(2.0f*(dist1dist3-2.0f*dist2)); if(deltaR-1 || deltaR1) continue; // Re-scaled coordinate //最终匹配点位置为scaleduR0 bestincR deltaR float bestuR mvScaleFactors[kpL.octave]*((float)scaleduR0(float)bestincRdeltaR); float disparity (uL-bestuR); if(disparityminD disparitymaxD) { if(disparity0) { disparity0.01; bestuR uL-0.01; } mvDepth[iL]mbf/disparity; mvuRight[iL] bestuR; vDistIdx.push_back(pairint,int(bestDist,iL)); } } } sort(vDistIdx.begin(),vDistIdx.end()); const float median vDistIdx[vDistIdx.size()/2].first; const float thDist 1.5f*1.4f*median; for(int ivDistIdx.size()-1;i0;i--) { if(vDistIdx[i].firstthDist) break; else { mvuRight[vDistIdx[i].second]-1; mvDepth[vDistIdx[i].second]-1; } } } //RGBD计算立体匹配只要判断下深度 void Frame::ComputeStereoFromRGBD(const cv::Mat imDepth) { mvuRight vectorfloat(N,-1); mvDepth vectorfloat(N,-1); for(int i0; iN; i) { const cv::KeyPoint kp mvKeys[i]; const cv::KeyPoint kpU mvKeysUn[i]; const float v kp.pt.y; const float u kp.pt.x; const float d imDepth.atfloat(v,u); if(d0) { mvDepth[i] d; mvuRight[i] kpU.pt.x-mbf/d; } } } //将特征点坐标给id即可反投影到3D地图点世界坐标 //单纯的二维像素点是没办法投影到3D空间中去的因为缺少对应的尺度 //但是在已知深度的情况下,则可确定对应的尺度最后获得3D中点坐标 cv::Mat Frame::UnprojectStereo(const int i) { const float z mvDepth[i]; if(z0) { const float u mvKeysUn[i].pt.x; const float v mvKeysUn[i].pt.y; const float x (u-cx)*z*invfx; const float y (v-cy)*z*invfy; cv::Mat x3Dc (cv::Mat_float(3,1) x, y, z); return mRwc*x3DcmOw; } else return cv::Mat(); } } //namespace ORB_SLAM里面有些涉及到其他类就没有具体解释还有就是LZ也不明白。。。反正代码还能怎么着多敲多瞧呗O(∩_∩)O哈哈~
