orb_slam3_details/src/Frame.cc

1569 lines
64 KiB
C++
Raw Normal View History

2020-12-01 11:58:17 +08:00
/**
* This file is part of ORB-SLAM3
*
* 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.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/
#include "Frame.h"
#include "G2oTypes.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include "ORBextractor.h"
#include "Converter.h"
#include "ORBmatcher.h"
#include "GeometricCamera.h"
#include <thread>
#include <include/CameraModels/Pinhole.h>
#include <include/CameraModels/KannalaBrandt8.h>
namespace ORB_SLAM3
{
//下一个生成的帧的ID,这里是初始化类的静态成员变量
long unsigned int Frame::nNextId=0;
//是否要进行初始化操作的标志
//这里给这个标志置位的操作是在最初系统开始加载到内存的时候进行的,下一帧就是整个系统的第一帧,所以这个标志要置位
bool Frame::mbInitialComputations=true;
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;
//For stereo fisheye matching
cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING);
Frame::Frame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false)
{
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
mTimeStereoMatch = 0;
mTimeORB_Ext = 0;
#endif
2020-12-01 11:58:17 +08:00
}
//Copy Constructor
Frame::Frame(const Frame &frame)
:mpcpi(frame.mpcpi),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), mImuCalib(frame.mImuCalib), mnCloseMPs(frame.mnCloseMPs),
mpImuPreintegrated(frame.mpImuPreintegrated), mpImuPreintegratedFrame(frame.mpImuPreintegratedFrame), mImuBias(frame.mImuBias),
mnId(frame.mnId), mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels),
mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor),
mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), mNameFile(frame.mNameFile), mnDataset(frame.mnDataset),
mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2), mpPrevFrame(frame.mpPrevFrame), mpLastKeyFrame(frame.mpLastKeyFrame), mbImuPreintegrated(frame.mbImuPreintegrated), mpMutexImu(frame.mpMutexImu),
mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), Nright(frame.Nright),
monoLeft(frame.monoLeft), monoRight(frame.monoRight), mvLeftToRightMatch(frame.mvLeftToRightMatch),
mvRightToLeftMatch(frame.mvRightToLeftMatch), mvStereo3Dpoints(frame.mvStereo3Dpoints),
2021-08-09 19:34:51 +08:00
mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()),
mTrlx(frame.mTrlx), mTlrx(frame.mTlrx), mOwx(frame.mOwx), mRcwx(frame.mRcwx), mtcwx(frame.mtcwx)
2020-12-01 11:58:17 +08:00
{
for(int i=0;i<FRAME_GRID_COLS;i++)
for(int j=0; j<FRAME_GRID_ROWS; j++){
mGrid[i][j]=frame.mGrid[i][j];
if(frame.Nleft > 0){
mGridRight[i][j] = frame.mGridRight[i][j];
}
}
if(!frame.mTcw.empty())
SetPose(frame.mTcw);
if(!frame.mVw.empty())
mVw = frame.mVw.clone();
mmProjectPoints = frame.mmProjectPoints;
mmMatchedInImage = frame.mmMatchedInImage;
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
mTimeStereoMatch = frame.mTimeStereoMatch;
mTimeORB_Ext = frame.mTimeORB_Ext;
#endif
2020-12-01 11:58:17 +08:00
}
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, Frame* pPrevF, const IMU::Calib &ImuCalib)
:mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false),
2021-08-09 19:34:51 +08:00
mpCamera(pCamera) ,mpCamera2(nullptr)
2020-12-01 11:58:17 +08:00
{
// Step 1 帧的ID 自增
mnId=nNextId++;
// Step 2 计算图像金字塔的参数
//获取图像金字塔的层数
mnScaleLevels = mpORBextractorLeft->GetLevels();
//获得层与层之间的缩放比
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
//计算上面缩放比的对数
mfLogScaleFactor = log(mfScaleFactor);
//获取每层图像的缩放因子
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
//同样获取每层图像缩放因子的倒数
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
//高斯模糊的时候,使用的方差
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
//获取sigma^2的倒数
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
2021-08-09 19:34:51 +08:00
// Step 3 对左目右目图像提取ORB特征点, 第一个参数0-左图, 1-右图。为加速计算,同时开了两个线程计算
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
#endif
2020-12-01 11:58:17 +08:00
thread threadLeft(&Frame::ExtractORB,this,0,imLeft,0,0);
// 对右目图像提取orb特征
thread threadRight(&Frame::ExtractORB,this,1,imRight,0,0);
//等待两张图像特征点提取过程完成
threadLeft.join();
threadRight.join();
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
#endif
2020-12-01 11:58:17 +08:00
//mvKeys中保存的是左图像中的特征点这里是获取左侧图像中特征点的个数
N = mvKeys.size();
//如果左图像中没有成功提取到特征点那么就返回,也意味这这一帧的图像无法使用
if(mvKeys.empty())
return;
// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
UndistortKeyPoints();
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now();
#endif
// Step 5 计算双目间特征点的匹配,只有匹配成功的特征点会计算其深度,深度存放在 mvDepth
2020-12-01 11:58:17 +08:00
// mvuRight中存储的应该是左图像中的点所匹配的在右图像中的点的横坐标纵坐标相同
ComputeStereoMatches();
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now();
2020-12-01 11:58:17 +08:00
2021-08-09 19:34:51 +08:00
mTimeStereoMatch = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndStereoMatches - time_StartStereoMatches).count();
#endif
// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
2020-12-01 11:58:17 +08:00
mvbOutlier = vector<bool>(N,false);
mmProjectPoints.clear();// = map<long unsigned int, cv::Point2f>(N, static_cast<cv::Point2f>(NULL));
mmMatchedInImage.clear();
// This is done only for the first Frame (or after a change in the calibration)
// Step 5 计算去畸变后图像边界,将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行
if(mbInitialComputations)
{
//计算去畸变后图像的边界
ComputeImageBounds(imLeft);
// 表示一个图像像素相当于多少个图像网格列(宽)
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行(高)
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
// 猜测是因为这种除法计算需要的时间略长,所以这里直接存储了这个中间计算结果
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//特殊的初始化过程完成,标志复位
mbInitialComputations=false;
}
// 双目相机基线长度
mb = mbf/fx;
if(pPrevF)
{
if(!pPrevF->mVw.empty())
mVw = pPrevF->mVw.clone();
}
else
{
mVw = cv::Mat::zeros(3,1,CV_32F);
}
// Step 6 将特征点分配到图像网格中
AssignFeaturesToGrid();
mpMutexImu = new std::mutex();
//Set no stereo fisheye information
Nleft = -1;
Nright = -1;
mvLeftToRightMatch = vector<int>(0);
mvRightToLeftMatch = vector<int>(0);
mTlr = cv::Mat(3,4,CV_32F);
mTrl = cv::Mat(3,4,CV_32F);
mvStereo3Dpoints = vector<cv::Mat>(0);
monoLeft = -1;
monoRight = -1;
}
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, GeometricCamera* pCamera,Frame* pPrevF, const IMU::Calib &ImuCalib)
:mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false),
2021-08-09 19:34:51 +08:00
mpCamera(pCamera),mpCamera2(nullptr)
2020-12-01 11:58:17 +08:00
{
// Step 1 帧的ID 自增
mnId=nNextId++;
// Step 2 计算图像金字塔的参数
//获取图像金字塔的层数
mnScaleLevels = mpORBextractorLeft->GetLevels();
//获取每层的缩放因子
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
//计算每层缩放因子的自然对数
mfLogScaleFactor = log(mfScaleFactor);
//获取各层图像的缩放因子
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
//获取各层图像的缩放因子的倒数
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
//计算上面获取的sigma^2的倒数
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
2020-12-01 11:58:17 +08:00
std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
#endif
ExtractORB(0,imGray,0,0);
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
2020-12-01 11:58:17 +08:00
std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
#endif
//获取特征点的个数
N = mvKeys.size();
//如果这一帧没有能够提取出特征点,那么就直接返回了
if(mvKeys.empty())
return;
// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
UndistortKeyPoints();
// Step 5 获取图像的深度,并且根据这个深度推算其右图中匹配的特征点的视差
ComputeStereoFromRGBD(imDepth);
// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
mmProjectPoints.clear();// = map<long unsigned int, cv::Point2f>(N, static_cast<cv::Point2f>(NULL));
mmMatchedInImage.clear();
// 记录地图点是否为外点初始化均为外点false
mvbOutlier = vector<bool>(N,false);
// This is done only for the first Frame (or after a change in the calibration)
// Step 5 计算去畸变后图像边界,将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行
if(mbInitialComputations)
{
//计算去畸变后图像的边界
ComputeImageBounds(imGray);
// 表示一个图像像素相当于多少个图像网格列(宽)
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行(高)
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
// 猜测是因为这种除法计算需要的时间略长,所以这里直接存储了这个中间计算结果
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//特殊的初始化过程完成,标志复位
mbInitialComputations=false;
}
// 计算假想的基线长度 baseline= mbf/fx
// 后面要对从RGBD相机输入的特征点,结合相机基线长度,焦距,以及点的深度等信息来计算其在假想的"右侧图像"上的匹配点
mb = mbf/fx;
mpMutexImu = new std::mutex();
//Set no stereo fisheye information
Nleft = -1;
Nright = -1;
mvLeftToRightMatch = vector<int>(0);
mvRightToLeftMatch = vector<int>(0);
mTlr = cv::Mat(3,4,CV_32F);
mTrl = cv::Mat(3,4,CV_32F);
mvStereo3Dpoints = vector<cv::Mat>(0);
monoLeft = -1;
monoRight = -1;
// 将特征点分配到图像网格中
AssignFeaturesToGrid();
}
Frame::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, const IMU::Calib &ImuCalib)
:mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
mTimeStamp(timeStamp), mK(static_cast<Pinhole*>(pCamera)->toK()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false), mpCamera(pCamera),
2021-08-09 19:34:51 +08:00
mpCamera2(nullptr)
2020-12-01 11:58:17 +08:00
{
// Frame ID
// Step 1 帧的ID 自增
mnId=nNextId++;
// Step 2 计算图像金字塔的参数
// Scale Level Info
//获取图像金字塔的层数
mnScaleLevels = mpORBextractorLeft->GetLevels();
//获取每层的缩放因子
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
//计算每层缩放因子的自然对数
mfLogScaleFactor = log(mfScaleFactor);
//获取各层图像的缩放因子
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
//获取各层图像的缩放因子的倒数
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
//获取sigma^2
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
//获取sigma^2的倒数
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
2020-12-01 11:58:17 +08:00
std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
#endif
// Step 3 对这个单目图像进行提取特征点, 第一个参数0-左图, 1-右图
ExtractORB(0,imGray,0,1000);
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
2020-12-01 11:58:17 +08:00
std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
#endif
2021-09-03 14:24:01 +08:00
//提取特征点的个数
2020-12-01 11:58:17 +08:00
N = mvKeys.size();
//如果没有能够成功提取出特征点,那么就直接返回了
if(mvKeys.empty())
return;
// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
UndistortKeyPoints();
// Set no stereo information
// 由于单目相机无法直接获得立体信息,所以这里要给右图像对应点和深度赋值-1表示没有相关信息
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);
mnCloseMPs = 0;
// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
mmProjectPoints.clear();// = map<long unsigned int, cv::Point2f>(N, static_cast<cv::Point2f>(NULL));
mmMatchedInImage.clear();
// 记录地图点是否为外点初始化均为外点false
mvbOutlier = vector<bool>(N,false);
// This is done only for the first Frame (or after a change in the calibration)
// Step 5 计算去畸变后图像边界,将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行
if(mbInitialComputations)
{
// 计算去畸变后图像的边界
ComputeImageBounds(imGray);
// 表示一个图像像素相当于多少个图像网格列(宽)
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
// 表示一个图像像素相当于多少个图像网格行(高)
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
fx = static_cast<Pinhole*>(mpCamera)->toK().at<float>(0,0);
fy = static_cast<Pinhole*>(mpCamera)->toK().at<float>(1,1);
cx = static_cast<Pinhole*>(mpCamera)->toK().at<float>(0,2);
cy = static_cast<Pinhole*>(mpCamera)->toK().at<float>(1,2);
// 猜测是因为这种除法计算需要的时间略长,所以这里直接存储了这个中间计算结果
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//特殊的初始化过程完成,标志复位
mbInitialComputations=false;
}
//计算 basline
mb = mbf/fx;
//Set no stereo fisheye information
Nleft = -1;
Nright = -1;
mvLeftToRightMatch = vector<int>(0);
mvRightToLeftMatch = vector<int>(0);
mTlr = cv::Mat(3,4,CV_32F);
mTrl = cv::Mat(3,4,CV_32F);
mvStereo3Dpoints = vector<cv::Mat>(0);
monoLeft = -1;
monoRight = -1;
// 将特征点分配到图像网格中
AssignFeaturesToGrid();
// mVw = cv::Mat::zeros(3,1,CV_32F);
if(pPrevF)
{
if(!pPrevF->mVw.empty())
mVw = pPrevF->mVw.clone();
}
else
{
mVw = cv::Mat::zeros(3,1,CV_32F);
}
mpMutexImu = new std::mutex();
}
void Frame::AssignFeaturesToGrid()
{
// Fill matrix with points
// Step 1 给存储特征点的网格数组 Frame::mGrid 预分配空间
const int nCells = FRAME_GRID_COLS*FRAME_GRID_ROWS;
int nReserve = 0.5f*N/(nCells);
//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
for (unsigned int j=0; j<FRAME_GRID_ROWS;j++){
mGrid[i][j].reserve(nReserve);
if(Nleft != -1){
mGridRight[i][j].reserve(nReserve);
}
}
// Step 2 遍历每个特征点将每个特征点在mvKeysUn中的索引值放到对应的网格mGrid中
for(int i=0;i<N;i++)
{
const cv::KeyPoint &kp = (Nleft == -1) ? mvKeysUn[i]
: (i < Nleft) ? mvKeys[i]
: mvKeysRight[i - Nleft];
//存储某个特征点所在网格的网格坐标nGridPosX范围[0,FRAME_GRID_COLS], nGridPosY范围[0,FRAME_GRID_ROWS]
int nGridPosX, nGridPosY;
// 计算某个特征点所在网格的网格坐标如果找到特征点所在的网格坐标记录在nGridPosX,nGridPosY里返回true没找到返回false
if(PosInGrid(kp,nGridPosX,nGridPosY)){
if(Nleft == -1 || i < Nleft)
//如果找到特征点所在网格坐标将这个特征点的索引添加到对应网格的数组mGrid中
mGrid[nGridPosX][nGridPosY].push_back(i);
else
mGridRight[nGridPosX][nGridPosY].push_back(i - Nleft);
}
}
}
void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1)
{
vector<int> vLapping = {x0,x1};
// 判断是左图还是右图
if(flag==0)
// 左图的话就套使用左图指定的特征点提取器,并将提取结果保存到对应的变量中
monoLeft = (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors,vLapping);
else
// 右图的话就需要使用右图指定的特征点提取器,并将提取结果保存到对应的变量中
monoRight = (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight,vLapping);
}
void Frame::SetPose(cv::Mat Tcw)
{
mTcw = Tcw.clone();
UpdatePoseMatrices();
}
void Frame::GetPose(cv::Mat &Tcw)
{
Tcw = mTcw.clone();
}
void Frame::SetNewBias(const IMU::Bias &b)
{
mImuBias = b;
if(mpImuPreintegrated)
mpImuPreintegrated->SetNewBias(b);
}
void Frame::SetVelocity(const cv::Mat &Vwb)
{
mVw = Vwb.clone();
}
/**
* @brief IMU姿imu姿imu姿姿
* 姿
*
* @param[in] Rwb
* @param[in] twb
* @param[in] Vwb
*/
2020-12-01 11:58:17 +08:00
void Frame::SetImuPoseVelocity(const cv::Mat &Rwb, const cv::Mat &twb, const cv::Mat &Vwb)
{
//Twb -> Tbw 求逆的过程根据旋转矩阵的逆求得参见十四讲第一版P44公式3.13
2020-12-01 11:58:17 +08:00
mVw = Vwb.clone();
cv::Mat Rbw = Rwb.t();
cv::Mat tbw = -Rbw*twb;
cv::Mat Tbw = cv::Mat::eye(4,4,CV_32F);
Rbw.copyTo(Tbw.rowRange(0,3).colRange(0,3));
tbw.copyTo(Tbw.rowRange(0,3).col(3));
// 外参值mImuCalib.Tcb与Tbw求mTcw
2020-12-01 11:58:17 +08:00
mTcw = mImuCalib.Tcb*Tbw;
UpdatePoseMatrices();
}
//根据Tcw计算mRcw、mtcw和mRwc、mOw
void Frame::UpdatePoseMatrices()
{
// mOw 当前相机光心在世界坐标系下坐标
// mTcw 世界坐标系到相机坐标系的变换矩阵
// mRcw 世界坐标系到相机坐标系的旋转矩阵
// mtcw 世界坐标系到相机坐标系的平移向量
// mRwc 相机坐标系到世界坐标系的旋转矩阵
//从变换矩阵中提取出旋转矩阵
//注意rowRange这个只取到范围的左边界而不取右边界
mRcw = mTcw.rowRange(0,3).colRange(0,3);
// mRcw求逆即可
mRwc = mRcw.t();
// 从变换矩阵中提取出旋转矩阵
mtcw = mTcw.rowRange(0,3).col(3);
// mTcw 求逆后是当前相机坐标系变换到世界坐标系下,对应的光心变换到世界坐标系下就是 mTcw的逆 中对应的平移向量
mOw = -mRcw.t()*mtcw;
2021-08-09 19:34:51 +08:00
// Static matrix
mOwx = cv::Matx31f(mOw.at<float>(0), mOw.at<float>(1), mOw.at<float>(2));
mRcwx = cv::Matx33f(mRcw.at<float>(0,0), mRcw.at<float>(0,1), mRcw.at<float>(0,2),
mRcw.at<float>(1,0), mRcw.at<float>(1,1), mRcw.at<float>(1,2),
mRcw.at<float>(2,0), mRcw.at<float>(2,1), mRcw.at<float>(2,2));
mtcwx = cv::Matx31f(mtcw.at<float>(0), mtcw.at<float>(1), mtcw.at<float>(2));
2020-12-01 11:58:17 +08:00
}
cv::Mat Frame::GetImuPosition()
{
return mRwc*mImuCalib.Tcb.rowRange(0,3).col(3)+mOw;
}
cv::Mat Frame::GetImuRotation()
{
return mRwc*mImuCalib.Tcb.rowRange(0,3).colRange(0,3);
}
cv::Mat Frame::GetImuPose()
{
cv::Mat Twb = cv::Mat::eye(4,4,CV_32F);
Twb.rowRange(0,3).colRange(0,3) = mRwc*mImuCalib.Tcb.rowRange(0,3).colRange(0,3);
Twb.rowRange(0,3).col(3) = mRwc*mImuCalib.Tcb.rowRange(0,3).col(3)+mOw;
return Twb.clone();
}
/**
* @brief
*
* Step 1
* Step 2 .false
* Step 3 MapPoint(u,v),
* Step 4 MapPoint,
* Step 5 , false
* Step 6 仿
* Step 7
* @param[in] pMP
* @param[in] viewingCosLimit 线线
* @return true
* @return false
*/
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
if(Nleft == -1){
// cout << "\na";
// mbTrackInView是决定一个地图点是否进行重投影的标志
// 这个标志的确定要经过多个函数的确定isInFrustum()只是其中的一个验证关卡。这里默认设置为否
pMP->mbTrackInView = false;
pMP->mTrackProjX = -1;
pMP->mTrackProjY = -1;
// 3D in absolute coordinates
// Step 1 获得这个地图点的世界坐标
2021-08-09 19:34:51 +08:00
cv::Matx31f Px = pMP->GetWorldPos2();
2020-12-01 11:58:17 +08:00
// cout << "b";
// 3D in camera coordinates
// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
2021-08-09 19:34:51 +08:00
const cv::Matx31f Pc = mRcwx * Px + mtcwx;
2020-12-01 11:58:17 +08:00
const float Pc_dist = cv::norm(Pc);
// Check positive depth
2021-08-09 19:34:51 +08:00
const float &PcZ = Pc(2);
2020-12-01 11:58:17 +08:00
const float invz = 1.0f/PcZ;
// Step 2 关卡一:检查这个地图点在当前帧的相机坐标系下,是否有正的深度.如果是负的表示出错直接返回false
if(PcZ<0.0f)
return false;
const cv::Point2f uv = mpCamera->project(Pc);
// Step 3 关卡二将MapPoint投影到当前帧的像素坐标(u,v), 并判断是否在图像有效范围内
// cout << "c";
// 判断是否在图像边界中,只要不在那么就说明无法在当前帧下进行重投影
if(uv.x<mnMinX || uv.x>mnMaxX)
return false;
if(uv.y<mnMinY || uv.y>mnMaxY)
return false;
// cout << "d";
pMP->mTrackProjX = uv.x;
pMP->mTrackProjY = uv.y;
// Check distance is in the scale invariance region of the MapPoint
// Step 4 关卡三计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
// 得到认为的可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance]
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
// 得到当前地图点距离当前帧相机光心的距离,注意PmOw都是在同一坐标系下才可以
// mOw当前相机光心在世界坐标系下坐标
2021-08-09 19:34:51 +08:00
const cv::Matx31f PO = Px-mOwx;
2020-12-01 11:58:17 +08:00
//取模就得到了距离
const float dist = cv::norm(PO);
//如果不在允许的尺度变化范围内,认为重投影不可靠
if(dist<minDistance || dist>maxDistance)
return false;
// cout << "e";
// Check viewing angle
// Step 5 关卡四:计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, 若小于cos(viewingCosLimit), 即夹角大于viewingCosLimit弧度则返回
2021-08-09 19:34:51 +08:00
cv::Matx31f Pnx = pMP->GetNormal2();
2020-12-01 11:58:17 +08:00
// cout << "f";
// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值,注意平均观测方向为单位向量
2021-08-09 19:34:51 +08:00
const float viewCos = PO.dot(Pnx)/dist;
2020-12-01 11:58:17 +08:00
//如果大于给定的阈值 cos(60°)=0.5认为这个点方向太偏了重投影不可靠返回false
if(viewCos<viewingCosLimit)
return false;
// Predict scale in the image
// Step 6 根据地图点到光心的距离来预测一个尺度(仿照特征点金字塔层级)
const int nPredictedLevel = pMP->PredictScale(dist,this);
// cout << "g";
// Step 7 记录计算得到的一些参数
// Data used by the tracking
// 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影
pMP->mbTrackInView = true;
// 该地图点投影在当前图像(一般是左图)的像素横坐标
pMP->mTrackProjX = uv.x;
// bf/z其实是视差相减得到右图如有中对应点的横坐标
pMP->mTrackProjXR = uv.x - mbf*invz;
pMP->mTrackDepth = Pc_dist;
// cout << "h";
// 该地图点投影在当前图像(一般是左图)的像素纵坐标
pMP->mTrackProjY = uv.y;
// 根据地图点到光心距离,预测的该地图点的尺度层级
pMP->mnTrackScaleLevel= nPredictedLevel;
// 保存当前视角和法线夹角的余弦值
pMP->mTrackViewCos = viewCos;
// cout << "i";
//执行到这里说明这个地图点在相机的视野中并且进行重投影是可靠的返回true
return true;
}
else{
pMP->mbTrackInView = false;
pMP->mbTrackInViewR = false;
pMP -> mnTrackScaleLevel = -1;
pMP -> mnTrackScaleLevelR = -1;
pMP->mbTrackInView = isInFrustumChecks(pMP,viewingCosLimit);
pMP->mbTrackInViewR = isInFrustumChecks(pMP,viewingCosLimit,true);
return pMP->mbTrackInView || pMP->mbTrackInViewR;
}
}
bool Frame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v)
{
// 3D in absolute coordinates
cv::Mat P = pMP->GetWorldPos();
// 3D in camera coordinates
const cv::Mat Pc = mRcw*P+mtcw;
const float &PcX = Pc.at<float>(0);
const float &PcY= Pc.at<float>(1);
const float &PcZ = Pc.at<float>(2);
// Check positive depth
if(PcZ<0.0f)
{
cout << "Negative depth: " << PcZ << endl;
return false;
}
// Project in image and check it is not outside
const float invz = 1.0f/PcZ;
u=fx*PcX*invz+cx;
v=fy*PcY*invz+cy;
// cout << "c";
if(u<mnMinX || u>mnMaxX)
return false;
if(v<mnMinY || v>mnMaxY)
return false;
float u_distort, v_distort;
float x = (u - cx) * invfx;
float y = (v - cy) * invfy;
float r2 = x * x + y * y;
float k1 = mDistCoef.at<float>(0);
float k2 = mDistCoef.at<float>(1);
float p1 = mDistCoef.at<float>(2);
float p2 = mDistCoef.at<float>(3);
float k3 = 0;
if(mDistCoef.total() == 5)
{
k3 = mDistCoef.at<float>(4);
}
// Radial distorsion
float x_distort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
float y_distort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
// Tangential distorsion
x_distort = x_distort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x));
y_distort = y_distort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y);
u_distort = x_distort * fx + cx;
v_distort = y_distort * fy + cy;
u = u_distort;
v = v_distort;
kp = cv::Point2f(u, v);
return true;
}
cv::Mat Frame::inRefCoordinates(cv::Mat pCw)
{
return mRcw*pCw+mtcw;
}
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel, const bool bRight) const
{
// 存储搜索结果的vector
vector<size_t> vIndices;
vIndices.reserve(N);
float factorX = r;
float factorY = r;
// Step 1 计算半径为r圆左右上下边界所在的网格列和行的id
// 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕慢慢理解下
// (mnMaxX-mnMinX)/FRAME_GRID_COLS表示列方向每个网格可以平均分得几个像素肯定大于1
// mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数表示每个像素可以均分几个网格列肯定小于1
// (x-mnMinX-r)可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
// 两者相乘就是求出那个半径为r的圆的左侧边界在哪个网格列中
// 保证nMinCellX 结果大于等于0
const int nMinCellX = max(0,(int)floor((x-mnMinX-factorX)*mfGridElementWidthInv));
// 如果最终求得的圆的左边界所在的网格列超过了设定了上限那么就说明计算出错找不到符合要求的特征点返回空vector
if(nMinCellX>=FRAME_GRID_COLS)
{
return vIndices;
}
// 计算圆所在的右边界网格列索引
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+factorX)*mfGridElementWidthInv));
// 如果计算出的圆右边界所在的网格不合法说明该特征点不好直接返回空vector
if(nMaxCellX<0)
{
return vIndices;
}
//后面的操作也都是类似的计算出这个圆上下边界所在的网格行的id
const int nMinCellY = max(0,(int)floor((y-mnMinY-factorY)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
{
return vIndices;
}
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+factorY)*mfGridElementHeightInv));
if(nMaxCellY<0)
{
return vIndices;
}
// 检查需要搜索的图像金字塔层数范围是否符合要求
//? 疑似bug。(minLevel>0) 后面条件 (maxLevel>=0)肯定成立
//? 改为 const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0);
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
// Step 2 遍历圆形区域内的所有网格寻找满足条件的候选特征点并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy];
// 如果这个网格中没有特征点,那么跳过这个网格继续下一个
if(vCell.empty())
continue;
// 如果这个网格中有特征点,那么遍历这个图像网格中所有的特征点
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
// 根据索引先读取这个特征点
const cv::KeyPoint &kpUn = (Nleft == -1) ? mvKeysUn[vCell[j]]
: (!bRight) ? mvKeys[vCell[j]]
: mvKeysRight[vCell[j]];
if(bCheckLevels)
{
// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据
// 保证特征点是在金字塔层级minLevel和maxLevel之间不是的话跳过
if(kpUn.octave<minLevel)
continue;
if(maxLevel>=0)
if(kpUn.octave>maxLevel)
continue;
}
// 通过检查,计算候选特征点到圆中心的距离,查看是否是在这个圆形区域之内
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 如果x方向和y方向的距离都在指定的半径之内存储其index为候选特征点
if(fabs(distx)<factorX && fabs(disty)<factorY)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
/**
* @brief nGridPosX,nGridPosYtruefalse
*
* @param[in] kp
* @param[in & out] posX
* @param[in & out] posY
* @return true true
* @return false false
*/
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
// 计算特征点x,y坐标落在哪个网格内网格坐标为posXposY
// mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
// mfGridElementHeightInv=(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);
//Keypoint's coordinates are undistorted, which could cause to go out of the image
// 因为特征点进行了去畸变而且前面计算是round取整所以有可能得到的点落在图像网格坐标外面
// 如果网格坐标posXposY超出了[0,FRAME_GRID_COLS] 和[0,FRAME_GRID_ROWS]表示该特征点没有对应网格坐标返回false
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
return false;
// 计算成功返回true
return true;
}
/**
* @brief BowmBowVec mFeatVec
*
*/
void Frame::ComputeBoW()
{
// 判断是否以前已经计算过了,计算过了就跳过
if(mBowVec.empty())
{
// 将描述子mDescriptors转换为DBOW要求的输入格式
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
mpORBvocabulary->transform(vCurrentDesc, //当前的描述子vector
mBowVec, //输出词袋向量记录的是单词的id及其对应权重TF-IDF值
mFeatVec, //输出记录node id及其对应的图像 feature对应的索引
4); //4表示从叶节点向前数的层数
}
}
/**
* @brief mvKeysUn
*
*/
void Frame::UndistortKeyPoints()
{
// Step 1 如果第一个畸变参数为0不需要矫正。第一个畸变参数k1是最重要的一般不为0为0的话说明畸变参数都是0
//变量mDistCoef中存储了opencv指定格式的去畸变参数格式为(k1,k2,p1,p2,k3)
if(mDistCoef.at<float>(0)==0.0)
{
mvKeysUn=mvKeys;
return;
}
// Step 2 如果畸变参数不为0用OpenCV函数进行畸变矫正
// Fill matrix with points
// N为提取的特征点数量为满足OpenCV函数输入要求将N个特征点保存在N*2的矩阵中
cv::Mat mat(N,2,CV_32F);
//遍历每个特征点,并将它们的坐标保存到矩阵中
for(int i=0; i<N; i++)
{
//然后将这个特征点的横纵坐标分别保存
mat.at<float>(i,0)=mvKeys[i].pt.x;
mat.at<float>(i,1)=mvKeys[i].pt.y;
}
// Undistort points
// 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数rows=0表示这个行将保持原来的参数不变
//为了能够直接调用opencv的函数来去畸变需要先将矩阵调整为2通道对应坐标x,y
mat=mat.reshape(2);
cv::undistortPoints(mat,mat, static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
//调整回只有一个通道,回归我们正常的处理方式
mat=mat.reshape(1);
// Fill undistorted keypoint vector
// Step 3 存储校正后的特征点
mvKeysUn.resize(N);
//遍历每一个特征点
for(int i=0; i<N; i++)
{
//根据索引获取这个特征点
//注意之所以这样做而不是直接重新声明一个特征点对象的目的是,能够得到源特征点对象的其他属性
cv::KeyPoint kp = mvKeys[i];
//读取校正后的坐标并覆盖老坐标
kp.pt.x=mat.at<float>(i,0);
kp.pt.y=mat.at<float>(i,1);
mvKeysUn[i]=kp;
}
}
/**
* @brief
*
* @param[in] imLeft
*/
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
// 如果畸变参数不为0用OpenCV函数进行畸变矫正
if(mDistCoef.at<float>(0)!=0.0)
{
// 保存矫正前的图像四个边界点坐标: (0,0) (cols,0) (0,rows) (cols,rows)
cv::Mat mat(4,2,CV_32F);
mat.at<float>(0,0)=0.0; //左上
mat.at<float>(0,1)=0.0;
mat.at<float>(1,0)=imLeft.cols; //右上
mat.at<float>(1,1)=0.0;
mat.at<float>(2,0)=0.0; //左下
mat.at<float>(2,1)=imLeft.rows;
mat.at<float>(3,0)=imLeft.cols; //右下
mat.at<float>(3,1)=imLeft.rows;
// Undistort corners
// 和前面校正特征点一样的操作,将这几个边界点作为输入进行校正
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
// Undistort corners
//校正后的四个边界点已经不能够围成一个严格的矩形,因此在这个四边形的外侧加边框作为坐标的边界
mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));//左上和左下横坐标最小的
mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));//右上和右下横坐标最大的
mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));//左上和右上纵坐标最小的
mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));//左下和右下纵坐标最小的
}
else
{
// 如果畸变参数为0就直接获得图像边界
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}
/*
*
*
* \n
* 线(), SAD \n
* SAD[https://blog.csdn.net/u012507022/article/details/51446891]
* SAD, SAD线 \n
* 使
* mvuRight(ur) mvDepth(Z)
*/
void Frame::ComputeStereoMatches()
{
/*两帧图像稀疏立体匹配ORB特征点匹配非逐像素的密集匹配但依然满足行对齐
* img_left img_right orb
*
1. . img_rightORB便使(/线, .
2. . 1img_leftiorbpiimg_rightiorborb, qi
3. . qirSAD
4. . 3uchar/int线)float
5. /. WTA
6. (outliers). sad
* /mvDepth mvuRight
*/
// 为匹配结果预先分配内存数据类型为float型
// mvuRight存储右图匹配点索引
// mvDepth存储特征点的深度信息
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
// orb特征相似度阈值 -> mean = (max + min) / 2
const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
// 金字塔顶层0层图像高 nRows
const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
// 二维vector存储每一行的orb特征点的列坐标为什么是vector因为每一行的特征点有可能不一样例如
// vRowIndices[0] = [1258, 11] 第1行有5个特征点,他们的列号即x坐标分别是1,2,5,8,11
// vRowIndices[1] = [2679, 13, 17, 20] 第2行有7个特征点.etc
vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
for(int i=0; i<nRows; i++)
vRowIndices[i].reserve(200);
// 右图特征点数量N表示数量 r表示右图且不能被修改
const int Nr = mvKeysRight.size();
// Step 1. 行特征点统计. 考虑到尺度金字塔特征,一个特征点可能存在于多行,而非唯一的一行
for(int iR=0; iR<Nr; iR++)
{
// 获取特征点ir的y坐标即行号
const cv::KeyPoint &kp = mvKeysRight[iR];
const float &kpY = kp.pt.y;
// 计算特征点ir在行方向上可能的偏移范围r即可能的行号为[kpY + r, kpY -r]
// 2 表示在全尺寸(scale = 1)的情况下假设有2个像素的偏移随着尺度变化r也跟着变化
const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];
const int maxr = ceil(kpY+r);
const int minr = floor(kpY-r);
// 将特征点ir保证在可能的行号中
for(int yi=minr;yi<=maxr;yi++)
vRowIndices[yi].push_back(iR);
}
// Step 2 -> 3. 粗匹配 + 精匹配
// 对于立体矫正后的两张图,在列方向(x)存在最大视差maxd和最小视差mind
// 也即是左图中任何一点p在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素
// maxd = baseline * length_focal / minZ
// mind = baseline * length_focal / maxZ
// Set limits for search
const float minZ = mb;
const float minD = 0;
const float maxD = mbf/minZ;
// For each left keypoint search a match in the right image
// 保存sad块匹配相似度和左图特征点索引
vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);
// 为左图每一个特征点il在右图搜索最相似的特征点ir
for(int iL=0; iL<N; iL++)
{
const cv::KeyPoint &kpL = mvKeys[iL];
const int &levelL = kpL.octave;
const float &vL = kpL.pt.y;
const float &uL = kpL.pt.x;
// 获取左图特征点il所在行以及在右图对应行中可能的匹配点
const vector<size_t> &vCandidates = vRowIndices[vL];
if(vCandidates.empty())
continue;
// 计算理论上的最佳搜索范围
const float minU = uL-maxD;
const float maxU = uL-minD;
// 最大搜索范围小于0说明无匹配点
if(maxU<0)
continue;
// 初始化最佳相似度,用最大相似度,以及最佳匹配点索引
int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;
const cv::Mat &dL = mDescriptors.row(iL);
// Step2. 粗配准. 左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的相似度和索引
for(size_t iC=0; iC<vCandidates.size(); iC++)
{
const size_t iR = vCandidates[iC];
const cv::KeyPoint &kpR = mvKeysRight[iR];
// 左图特征点il与带匹配点ic的空间尺度差超过2放弃
if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
continue;
// 使用列坐标(x)进行匹配和stereomatch一样
const float &uR = kpR.pt.x;
// 超出理论搜索范围[minU, maxU],可能是误匹配,放弃
if(uR>=minU && uR<=maxU)
{
// 计算匹配点il和待匹配点ic的相似度dist
const cv::Mat &dR = mDescriptorsRight.row(iR);
const int dist = ORBmatcher::DescriptorDistance(dL,dR);
//统计最小相似度及其对应的列坐标(x)
if(dist<bestDist)
{
bestDist = dist;
bestIdxR = iR;
}
}
}
// Subpixel match by correlation
// 如果刚才匹配过程中的最佳描述子距离小于给定的阈值
// Step 3. 精确匹配.
if(bestDist<thOrbDist)
{
// coordinates in image pyramid at keypoint scale
// 计算右图特征点x坐标和对应的金字塔尺度
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);
// 滑动窗口搜索, 类似模版卷积或滤波
// w表示sad相似度的窗口半径
const int w = 5;
// 提取左图中,以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch
cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
2021-08-09 19:34:51 +08:00
2020-12-01 11:58:17 +08:00
//初始化最佳相似度
int bestDist = INT_MAX;
// 通过滑动窗口搜索优化,得到的列坐标偏移量
int bestincR = 0;
//滑动窗口的滑动范围为(-L, L
const int L = 5;
// 初始化存储图像块相似度
vector<float> vDists;
vDists.resize(2*L+1);
// 计算滑动窗口滑动范围的边界,因为是块匹配,还要算上图像块的尺寸
// 列方向起点 iniu = r0 + 最大窗口滑动范围 - 图像块尺寸
// 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1
// 此次 + 1 和下面的提取图像块是列坐标+1是一样的保证提取的图像块的宽是2 * w + 1
const float iniu = scaleduR0+L-w;
const float endu = scaleduR0+L+w+1;
// 判断搜索是否越界
if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)
continue;
// 在搜索范围内从左到右滑动,并计算图像块相似度
for(int incR=-L; incR<=+L; incR++)
{
// 提取左图中,以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch
cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
2021-08-09 19:34:51 +08:00
2020-12-01 11:58:17 +08:00
// sad 计算
float dist = cv::norm(IL,IR,cv::NORM_L1);
// 统计最小sad和偏移量
if(dist<bestDist)
{
bestDist = dist;
bestincR = incR;
}
//L+incR 为refine后的匹配点列坐标(x)
vDists[L+incR] = dist;
}
// 搜索窗口越界判断ß
if(bestincR==-L || bestincR==L)
continue;
// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线
// 使用3点拟合抛物线的方式用极小值代替之前计算的最优是差值
// \ / <- 由视差为141516的相似度拟合的抛物线
// . .(16)
// .14 .(15) <- int/uchar最佳视差值
// .
// 14.5<- 真实的视差值
// deltaR = 15.5 - 16 = -0.5
// 公式参考opencv sgbm源码中的亚像素插值公式
// 或论文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7
// Sub-pixel match (Parabola fitting)
const float dist1 = vDists[L+bestincR-1];
const float dist2 = vDists[L+bestincR];
const float dist3 = vDists[L+bestincR+1];
const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));
// 亚像素精度的修正量应该是在[-1,1]之间,否则就是误匹配
if(deltaR<-1 || deltaR>1)
continue;
// 根据亚像素精度偏移量delta调整最佳匹配索引
float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
float disparity = (uL-bestuR);
if(disparity>=minD && disparity<maxD)
{
// 如果存在负视差则约束为0.01
if(disparity<=0)
{
disparity=0.01;
bestuR = uL-0.01;
}
// 根据视差值计算深度信息
// 保存最相似点的列坐标(x)信息
// 保存归一化sad最小相似度
// Step 5. 最优视差值/深度选择.
mvDepth[iL]=mbf/disparity;
mvuRight[iL] = bestuR;
vDistIdx.push_back(pair<int,int>(bestDist,iL));
}
}
}
// Step 6. 删除离缺点(outliers)
// 块匹配相似度阈值判断归一化sad最小并不代表就一定是匹配的比如光照变化、弱纹理、无纹理等同样会造成误匹配
// 误匹配判断条件 norm_sad > 1.5 * 1.4 * median
sort(vDistIdx.begin(),vDistIdx.end());
const float median = vDistIdx[vDistIdx.size()/2].first;
const float thDist = 1.5f*1.4f*median;
for(int i=vDistIdx.size()-1;i>=0;i--)
{
if(vDistIdx[i].first<thDist)
break;
else
{
// 误匹配点置为-1和初始化时保持一直作为error code
mvuRight[vDistIdx[i].second]=-1;
mvDepth[vDistIdx[i].second]=-1;
}
}
}
//计算RGBD图像的立体深度信息
void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)
{
/** 主要步骤如下:.对于彩色图像中的每一个特征点:<ul> */
// mvDepth直接由depth图像读取`
//这里是初始化这两个存储“右图”匹配特征点横坐标和存储特征点深度值的vector
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);
//开始遍历彩色图像中的所有特征点
for(int i=0; i<N; i++)
{
/** <li> 从<b>未矫正的特征点</b>提供的坐标来读取深度图像拿到这个点的深度数据 </li> */
//获取校正前和校正后的特征点
const cv::KeyPoint &kp = mvKeys[i];
const cv::KeyPoint &kpU = mvKeysUn[i];
//获取其横纵坐标,注意 NOTICE 是校正前的特征点的
const float &v = kp.pt.y;
const float &u = kp.pt.x;
//从深度图像中获取这个特征点对应的深度点
//NOTE 从这里看对深度图像进行去畸变处理是没有必要的,我们依旧可以直接通过未矫正的特征点的坐标来直接拿到深度数据
const float d = imDepth.at<float>(v,u);
// 如果获取到的深度点合法(d>0), 那么就保存这个特征点的深度,并且计算出等效的\在假想的右图中该特征点所匹配的特征点的横坐标
if(d>0)
{
mvDepth[i] = d;
mvuRight[i] = kpU.pt.x-mbf/d;
}
}
}
//当某个特征点的深度信息或者双目信息有效时,将它反投影到三维世界坐标系中
cv::Mat Frame::UnprojectStereo(const int &i)
{
const float z = mvDepth[i];
if(z>0)
{
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*x3Dc+mOw;
}
else
return cv::Mat();
}
bool Frame::imuIsPreintegrated()
{
unique_lock<std::mutex> lock(*mpMutexImu);
return mbImuPreintegrated;
}
void Frame::setIntegrated()
{
unique_lock<std::mutex> lock(*mpMutexImu);
mbImuPreintegrated = true;
}
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, cv::Mat& Tlr,Frame* pPrevF, const IMU::Calib &ImuCalib)
:mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), mTlr(Tlr)
{
imgLeft = imLeft.clone();
imgRight = imRight.clone();
// Frame ID
mnId=nNextId++;
// 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
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
#endif
2020-12-01 11:58:17 +08:00
thread threadLeft(&Frame::ExtractORB,this,0,imLeft,static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[0],static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[1]);
thread threadRight(&Frame::ExtractORB,this,1,imRight,static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[0],static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[1]);
threadLeft.join();
threadRight.join();
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
#endif
2021-09-03 14:24:01 +08:00
// 左图中提取的特征点数目
2020-12-01 11:58:17 +08:00
Nleft = mvKeys.size();
2021-09-03 14:24:01 +08:00
// 右图中提取的特征点数目
2020-12-01 11:58:17 +08:00
Nright = mvKeysRight.size();
2021-09-03 14:24:01 +08:00
// 特征点总数
2020-12-01 11:58:17 +08:00
N = Nleft + Nright;
if(N == 0)
return;
// This is done only for the first Frame (or after a change in the calibration)
if(mbInitialComputations)
{
ComputeImageBounds(imLeft);
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
invfx = 1.0f/fx;
invfy = 1.0f/fy;
mbInitialComputations=false;
}
mb = mbf / fx;
mRlr = mTlr.rowRange(0,3).colRange(0,3);
mtlr = mTlr.col(3);
cv::Mat Rrl = mTlr.rowRange(0,3).colRange(0,3).t();
cv::Mat trl = Rrl * (-1 * mTlr.col(3));
cv::hconcat(Rrl,trl,mTrl);
2021-08-09 19:34:51 +08:00
mTrlx = cv::Matx34f(Rrl.at<float>(0,0), Rrl.at<float>(0,1), Rrl.at<float>(0,2), trl.at<float>(0),
Rrl.at<float>(1,0), Rrl.at<float>(1,1), Rrl.at<float>(1,2), trl.at<float>(1),
Rrl.at<float>(2,0), Rrl.at<float>(2,1), Rrl.at<float>(2,2), trl.at<float>(2));
mTlrx = cv::Matx34f(mRlr.at<float>(0,0), mRlr.at<float>(0,1), mRlr.at<float>(0,2), mtlr.at<float>(0),
mRlr.at<float>(1,0), mRlr.at<float>(1,1), mRlr.at<float>(1,2), mtlr.at<float>(1),
mRlr.at<float>(2,0), mRlr.at<float>(2,1), mRlr.at<float>(2,2), mtlr.at<float>(2));
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now();
#endif
2020-12-01 11:58:17 +08:00
ComputeStereoFishEyeMatches();
2021-08-09 19:34:51 +08:00
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now();
mTimeStereoMatch = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndStereoMatches - time_StartStereoMatches).count();
#endif
2020-12-01 11:58:17 +08:00
//Put all descriptors in the same matrix
cv::vconcat(mDescriptors,mDescriptorsRight,mDescriptors);
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(nullptr));
mvbOutlier = vector<bool>(N,false);
AssignFeaturesToGrid();
mpMutexImu = new std::mutex();
UndistortKeyPoints();
}
void Frame::ComputeStereoFishEyeMatches() {
//Speed it up by matching keypoints in the lapping area
vector<cv::KeyPoint> stereoLeft(mvKeys.begin() + monoLeft, mvKeys.end());
vector<cv::KeyPoint> stereoRight(mvKeysRight.begin() + monoRight, mvKeysRight.end());
cv::Mat stereoDescLeft = mDescriptors.rowRange(monoLeft, mDescriptors.rows);
cv::Mat stereoDescRight = mDescriptorsRight.rowRange(monoRight, mDescriptorsRight.rows);
mvLeftToRightMatch = vector<int>(Nleft,-1);
mvRightToLeftMatch = vector<int>(Nright,-1);
mvDepth = vector<float>(Nleft,-1.0f);
mvuRight = vector<float>(Nleft,-1);
mvStereo3Dpoints = vector<cv::Mat>(Nleft);
mnCloseMPs = 0;
//Perform a brute force between Keypoint in the left and right image
vector<vector<cv::DMatch>> matches;
BFmatcher.knnMatch(stereoDescLeft,stereoDescRight,matches,2);
int nMatches = 0;
int descMatches = 0;
//Check matches using Lowe's ratio
for(vector<vector<cv::DMatch>>::iterator it = matches.begin(); it != matches.end(); ++it){
if((*it).size() >= 2 && (*it)[0].distance < (*it)[1].distance * 0.7){
//For every good match, check parallax and reprojection error to discard spurious matches
cv::Mat p3D;
descMatches++;
float sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave], sigma2 = mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave];
float depth = static_cast<KannalaBrandt8*>(mpCamera)->TriangulateMatches(mpCamera2,mvKeys[(*it)[0].queryIdx + monoLeft],mvKeysRight[(*it)[0].trainIdx + monoRight],mRlr,mtlr,sigma1,sigma2,p3D);
if(depth > 0.0001f){
mvLeftToRightMatch[(*it)[0].queryIdx + monoLeft] = (*it)[0].trainIdx + monoRight;
mvRightToLeftMatch[(*it)[0].trainIdx + monoRight] = (*it)[0].queryIdx + monoLeft;
mvStereo3Dpoints[(*it)[0].queryIdx + monoLeft] = p3D.clone();
mvDepth[(*it)[0].queryIdx + monoLeft] = depth;
nMatches++;
}
}
}
}
bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) {
// 3D in absolute coordinates
2021-08-09 19:34:51 +08:00
cv::Matx31f Px = pMP->GetWorldPos2();
cv::Matx33f mRx;
cv::Matx31f mtx, twcx;
cv::Matx33f Rcw = mRcwx;
cv::Matx33f Rwc = mRcwx.t();
cv::Matx31f tcw = mOwx;
2020-12-01 11:58:17 +08:00
if(bRight){
2021-08-09 19:34:51 +08:00
cv::Matx33f Rrl = mTrlx.get_minor<3,3>(0,0);
cv::Matx31f trl = mTrlx.get_minor<3,1>(0,3);
mRx = Rrl * Rcw;
mtx = Rrl * tcw + trl;
twcx = Rwc * mTlrx.get_minor<3,1>(0,3) + tcw;
2020-12-01 11:58:17 +08:00
}
else{
2021-08-09 19:34:51 +08:00
mRx = mRcwx;
mtx = mtcwx;
twcx = mOwx;
2020-12-01 11:58:17 +08:00
}
// 3D in camera coordinates
2021-08-09 19:34:51 +08:00
cv::Matx31f Pcx = mRx * Px + mtx;
const float Pc_dist = cv::norm(Pcx);
const float &PcZ = Pcx(2);
2020-12-01 11:58:17 +08:00
// Check positive depth
if(PcZ<0.0f)
return false;
// Project in image and check it is not outside
cv::Point2f uv;
2021-08-09 19:34:51 +08:00
if(bRight) uv = mpCamera2->project(Pcx);
else uv = mpCamera->project(Pcx);
2020-12-01 11:58:17 +08:00
if(uv.x<mnMinX || uv.x>mnMaxX)
return false;
if(uv.y<mnMinY || uv.y>mnMaxY)
return false;
// Check distance is in the scale invariance region of the MapPoint
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
2021-08-09 19:34:51 +08:00
const cv::Matx31f POx = Px - twcx;
const float dist = cv::norm(POx);
2020-12-01 11:58:17 +08:00
if(dist<minDistance || dist>maxDistance)
return false;
// Check viewing angle
2021-08-09 19:34:51 +08:00
cv::Matx31f Pnx = pMP->GetNormal2();
2020-12-01 11:58:17 +08:00
2021-08-09 19:34:51 +08:00
const float viewCos = POx.dot(Pnx)/dist;
2020-12-01 11:58:17 +08:00
if(viewCos<viewingCosLimit)
return false;
// Predict scale in the image
const int nPredictedLevel = pMP->PredictScale(dist,this);
if(bRight){
pMP->mTrackProjXR = uv.x;
pMP->mTrackProjYR = uv.y;
pMP->mnTrackScaleLevelR= nPredictedLevel;
pMP->mTrackViewCosR = viewCos;
pMP->mTrackDepthR = Pc_dist;
}
else{
pMP->mTrackProjX = uv.x;
pMP->mTrackProjY = uv.y;
pMP->mnTrackScaleLevel= nPredictedLevel;
pMP->mTrackViewCos = viewCos;
pMP->mTrackDepth = Pc_dist;
}
return true;
}
cv::Mat Frame::UnprojectStereoFishEye(const int &i){
return mRwc*mvStereo3Dpoints[i]+mOw;
}
} //namespace ORB_SLAM