以文件mono_inertial_euroc.cc开始
从main函数开始讲解，##表示源码
判断参数是否大于等于5个，否则，输入有误，直接返回，正确执行参数如下
./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml ${dir}/MH01 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi
##
if(argc < 5) { cerr << endl << "Usage: ./mono_inertial_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) " << endl; return 1; }
计算有多少个可执行序列
## const int num_seq = (argc-3)/2;
将图像数据和IMU
##
for (seq = 0; seq<num_seq; seq++) {//数据路径，指向参数${dir}/MH01 string pathSeq(argv[(2*seq) + 3]); //数据时间戳路径，./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt string pathTimeStamps(argv[(2*seq) + 4]); string pathCam0 = pathSeq + "/mav0/cam0/data"; string pathImu = pathSeq + "/mav0/imu0/data.csv"; //将加载进来的图像数据和对应时间戳放入数组vstrImageFilenames以及vTimestampsCam中 LoadImages(pathCam0, pathTimeStamps, vstrImageFilenames[seq], vTimestampsCam[seq]);//获取三轴加速度计和三轴陀螺仪数据以及时间戳 LoadIMU(pathImu, vTimestampsImu[seq], vAcc[seq], vGyro[seq]);//保存对应序列的图像的数据量 nImages[seq] = vstrImageFilenames[seq].size(); //如果有多个图像序列，那么将总的图像数据量保存下来 tot_images += nImages[seq]; //imu对应的数据量 nImu[seq] = vTimestampsImu[seq].size();// Find first imu to be considered, supposing imu measurements start first //找到IMU的每一个序列第一个大于图像的imu时间戳 //图像序列 - - - - - - //IMU序列 - - - - - - 把第一个大于图像序列的时间戳找到 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0]) first_imu[seq]++; //将大于第一个图像的imu时间戳回滚回来，为方便后续做预积分 first_imu[seq]--; // first imu measurement to be considered }
初始化SLAM系统变量
## // Create SLAM system. It initializes all system threads and gets ready to process frames. //argv[1] = ./Vocabulary/ORBvoc.txt //argv[2] = ./Examples/Monocular-Inertial/EuRoC.yaml ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
进入到上面ORB_SLAM3::System的构造函数，只看相对重点部分
##
/** * @brief 系统构造函数，会启动其他线程 * @param strVocFile 词袋文件路径./Vocabulary/ORBvoc.txt * @param strSettingsFile 配置文件路径./Examples/Monocular-Inertial/EuRoC.yaml * @param sensor 传感器类型 * @param bUseViewer 是否使用可视化界面，默认为true * @param initFr initFr表示初始化帧的id,默认值0 * @param strSequence 序列名,在跟踪线程和局部建图线程用得到，默认值std::string() */ System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer, const int initFr, const string &strSequence): mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false), mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false) {// 读取配置文件,strSettingsFile配置文件所在路径./Examples/Monocular-Inertial/EuRoC.yaml cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); // 查看配置文件版本，不同版本有不同处理方法 cv::FileNode node = fsSettings["File.version"]; if(!node.empty() && node.isString() && node.string() == "1.0") { settings_ = new Settings(strSettingsFile,mSensor);//需要进入该函数，TODO1 // 保存及加载地图的名字，在配置文件里面没有对应的内容，后续可以自行添加用于定位 mStrLoadAtlasFromFile = settings_->atlasLoadFile(); mStrSaveAtlasToFile = settings_->atlasSaveFile(); } // ORBSLAM3新加的多地图管理功能，这里加载Atlas标识符 bool loadedAtlas = false; if(mStrLoadAtlasFromFile.empty()) {// 建立一个新的ORB字典 mpVocabulary = new ORBVocabulary(); // 读取预训练好的ORB字典 bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);//创建关键帧数据库 mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//创建多地图，参数0表示初始化关键帧id为0 mpAtlas = new Atlas(0);//需要进入该函数，TODO2 }// 如果是有imu的传感器类型，设置mbIsInertial标志为true if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD) mpAtlas->SetInertialSensor();
// 创建跟踪线程（位于主线程） mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);//需要进入该函数，TODO3 //创建并开启local mapping线程 mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);//后续讲解的时候，再进入分析 mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper); mpLocalMapper->mInitFr = initFr;//初始化帧的id，默认为0 // 设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃 if(settings_) mpLocalMapper->mThFarPoints = settings_->thFarPoints();//20 else mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];// 创建并开启闭环线程 mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); //后续讲解时，再进入分析 mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser); // 设置线程间的指针 mpTracker->SetLocalMapper(mpLocalMapper); mpTracker->SetLoopClosing(mpLoopCloser); mpLocalMapper->SetTracker(mpTracker); mpLocalMapper->SetLoopCloser(mpLoopCloser); mpLoopCloser->SetTracker(mpTracker); mpLoopCloser->SetLocalMapper(mpLocalMapper); }
进入到构造函数，TODO1
##
// configFile=./Examples/Monocular-Inertial/EuRoC.yaml Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) {// Open settings file./Examples/Monocular-Inertial/EuRoC.yaml cv::FileStorage fSettings(configFile, cv::FileStorage::READ);//读取相机1的内参和畸变参数 readCamera1(fSettings); //需要进入该函数，TODO1-1// 如果是双目，则读取第二个相机的配置 if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) { readCamera2(fSettings); }//读取图像信息 readImageInfo(fSettings); //需要进入该函数，TODO1-2//读取IMU参数 readIMU(fSettings); if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD) {
//读取RGBD配置 readRGBD(fSettings); //需要进入该函数，TODO1-3 } //提取ORB特征相关信息 readORB(fSettings); if (bNeedToRectify_) { precomputeRectificationMaps(); } }
readCamera1(fSettings); //需要进入该函数，TODO1-1
##
void Settings::readCamera1(cv::FileStorage &fSettings) {//相机类型 string cameraModel = readParameter<string>(fSettings, "Camera.type", found); vector<float> vCalibration;
//针孔相机类型 if (cameraModel == "PinHole") { cameraType_ = PinHole;//相机内参 float fx = readParameter<float>(fSettings, "Camera1.fx", found); float fy = readParameter<float>(fSettings, "Camera1.fy", found); float cx = readParameter<float>(fSettings, "Camera1.cx", found); float cy = readParameter<float>(fSettings, "Camera1.cy", found); vCalibration = {fx, fy, cx, cy}; //构建针孔相机模型 calibration1_ = new Pinhole(vCalibration); //畸变参数的获取 vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found); vPinHoleDistorsion1_[1] = readParameter<float>(fSettings, "Camera1.k2", found); vPinHoleDistorsion1_[2] = readParameter<float>(fSettings, "Camera1.p1", found); vPinHoleDistorsion1_[3] = readParameter<float>(fSettings, "Camera1.p2", found); }else if (cameraModel == "KannalaBrandt8")//鱼眼相机的镜头类型，广角镜头 { cameraType_ = KannalaBrandt; // Read intrinsic parameters float fx = readParameter<float>(fSettings, "Camera1.fx", found); float fy = readParameter<float>(fSettings, "Camera1.fy", found); float cx = readParameter<float>(fSettings, "Camera1.cx", found); float cy = readParameter<float>(fSettings, "Camera1.cy", found); float k0 = readParameter<float>(fSettings, "Camera1.k1", found); float k1 = readParameter<float>(fSettings, "Camera1.k2", found); float k2 = readParameter<float>(fSettings, "Camera1.k3", found); float k3 = readParameter<float>(fSettings, "Camera1.k4", found); vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; calibration1_ = new KannalaBrandt8(vCalibration); originalCalib1_ = new KannalaBrandt8(vCalibration); if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) { int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found); int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found); vector<int> vOverlapping = {colBegin, colEnd};//记录重叠的开始和结束列 static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea = vOverlapping; } } }
readImageInfo(fSettings); //需要进入该函数，TODO1-2
##
void Setting::readImageInfo(cv::FileStorage &fSettings){
bool found;//读取原始的相机分辨率 int originalRows = readParameter<int>(fSettings, "Camera.height", found); int originalCols = readParameter<int>(fSettings, "Camera.width", found); originalImSize_.width = originalCols; originalImSize_.height = originalRows; //类型为cv::Size newImSize_ = originalImSize_; //读取新的相机的高度 int newHeigh = readParameter<int>(fSettings, "Camera.newHeight", found, false); //如果有这个参数，则执行下面 if (found) { //需要去修改大小 bNeedToResize1_ = true; newImSize_.height = newHeigh; if (!bNeedToRectify_) { // Update calibration //新的高度除以原始的高度,假设原始高度为4，新的高度为2，则scaleRowFactor为0.5
float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height; //对fy和cy进行参数的改变,根据内参矩阵将相机坐标系下的点转换到图像坐标系下，对高度v(或y)方向（也就是行方向），统一做调整
//v = fy * Y/Z + cy，将相机坐标系下的点映射到经过修改后的像素坐标系下 calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1); calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3); if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) { calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1); calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3); } } } //读取新的相机的宽度 int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found, false); //有这个值，则执行下面 if (found) { bNeedToResize1_ = true; newImSize_.width = newWidth; if (!bNeedToRectify_) { // Update calibration float scaleColFactor = (float)newImSize_.width / (float)originalImSize_.width; //对fx和cx进行参数的改变，根据内参矩阵将相机坐标系下的点转换到图像坐标系下，对宽度u方向（也就是列方向），统一做调整 calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0); calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2); if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) { calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0); calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2); if (cameraType_ == KannalaBrandt) { static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[0] *= scaleColFactor; static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[1] *= scaleColFactor; static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[0] *= scaleColFactor; static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[1] *= scaleColFactor; } } } } //读取相机频率 fps_ = readParameter<int>(fSettings, "Camera.fps", found); //图像为RGB格式 bRGB_ = (bool)readParameter<int>(fSettings, "Camera.RGB", found); }
readIMU(fSettings); //需要进入该函数，TODO1-3
##
void Settings::readRGBD(cv::FileStorage &fSettings) { bool found; depthMapFactor_ = readParameter<float>(fSettings, "RGBD.DepthMapFactor", found); thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found); b_ = readParameter<float>(fSettings, "Stereo.b", found); bf_ = b_ * calibration1_->getParameter(0);//焦距与基线的乘积，多说一句视差的概念：d（视差）=UL - UR,左右图的横坐标之差，3D点的深度z = f*b/d;视差与深度z成反比; }
mpAtlas = new Atlas(0);//需要进入该函数，TODO2
//初始化地图数据集
##
Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid), mHasViewer(false) { mpCurrentMap = static_cast<Map *>(NULL); CreateNewMap();//TODO2-1 }
CreateNewMap();TODO2-1
##
/** * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。 * */ void Atlas::CreateNewMap() { // 锁住地图集 unique_lock<mutex> lock(mMutexAtlas);// 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出 if (mpCurrentMap) { // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1 if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // 将当前地图储存起来，其实就是把mIsInUse标记为false mpCurrentMap->SetStoredMap(); } mpCurrentMap = new Map(mnLastInitKFidMap); //新建地图 mpCurrentMap->SetCurrentMap(); //设置为活跃地图 mspMaps.insert(mpCurrentMap); //插入地图集 }
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);//需要进入该函数，TODO3
##
/** * @brief 跟踪线程构造函数 * @param pSys 系统类指针 * @param pVoc 词典 * @param pFrameDrawer 画图像的 * @param pMapDrawer 画地图的 * @param pAtlas atlas * @param pKFDB 关键帧词典数据库 * @param strSettingPath 参数文件路径 * @param sensor 传感器类型 * @param settings 参数类 * @param _strSeqName 序列名字，没用到 */ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Atlas *pAtlas, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq) : mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false), mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mbReadyToInitializate(false), mpSystem(pSys), mpViewer(NULL), bStepByStep(false), mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr), mpLastKeyFrame(static_cast<KeyFrame*>(NULL)) { // Load camera parameters from settings file // Step 1 从配置文件中加载相机参数 if(settings){ newParameterLoader(settings);//TODO3-1 } //记录帧的ID号 initID = 0; lastID = 0; }
newParameterLoader(settings);TODO3-1
## /** * @brief 根据参数类读取参数，可快速略过不看 * @param settings 参数类 */ void Tracking::newParameterLoader(Settings *settings) { //读取相机部分省略// 读取特征点参数 int nFeatures = settings->nFeatures();//1200 要读取的特征点数目 int nLevels = settings->nLevels();//8 图像金字塔的层数 int fIniThFAST = settings->initThFAST();//20 特征点检测阈值，用于提取较明显的角点 int fMinThFAST = settings->minThFAST();//7 当图像纹理不丰富的时候，可选用这个阈值来提取角点 float fScaleFactor = settings->scaleFactor();//1.2 图像金字塔层间的尺度 //创建ORB特征点提取器 mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);//TODO3-1-1 if(mSensor==System::STEREO || mSensor==System::IMU_STEREO) mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); //初始特征提取器 if(mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR) mpIniORBextractor = new ORBextractor(5*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);//读取imu参数 Sophus::SE3f Tbc = settings->Tbc(); mImuFreq = settings->imuFrequency(); mImuPer = 0.001; //1.0 / (double) mImuFreq; float Ng = settings->noiseGyro(); float Na = settings->noiseAcc(); float Ngw = settings->gyroWalk(); float Naw = settings->accWalk(); const float sf = sqrt(mImuFreq);
//主要注意高斯噪声离散=连续/sqrt(delta_t),随机游走离散=连续*sqrt(delta_t) mpImuCalib = new IMU::Calib(Tbc,Ng*sf,Na*sf,Ngw/sf,Naw/sf); //预积分的初始化 mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); }
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);//TODO3-1-1
//特征点提取器的构造函数 ORBextractor::ORBextractor(int _nfeatures, //要提取的特征点数目1200 float _scaleFactor, //图像金字塔的缩放系数1.2 int _nlevels, //图像金字塔的层数8 int _iniThFAST, //可以提取出最明显的角点的阈值20 int _minThFAST): //图像纹理不丰富，设置角点提取较低阈值，为了达到想要的特征点数目7 nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), iniThFAST(_iniThFAST), minThFAST(_minThFAST) { //存储图像金字塔每层图像缩放系数，类型为vector mvScaleFactor.resize(nlevels); //存储图像金字塔每层图像相对初始图像缩放因子的平方 mvLevelSigma2.resize(nlevels); //对于初始图像，这两个参数都是1 mvScaleFactor[0]=1.0f; mvLevelSigma2[0]=1.0f; //计算图像金字塔中图像相当于初始图像的缩放系数 for(int i=1; i<nlevels; i++) { //累乘计算出来 mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor; //每层图像相对于初始图像缩放因子的平方 mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i]; } //保存上面参数的倒数 mvInvScaleFactor.resize(nlevels); mvInvLevelSigma2.resize(nlevels); for(int i=0; i<nlevels; i++) { mvInvScaleFactor[i]=1.0f/mvScaleFactor[i]; mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i]; } //调整图像金字塔vector的图像层数 mvImagePyramid.resize(nlevels); //每层需要提取出来的特征点个数，需要根据图像金字塔设定的层数来调整 mnFeaturesPerLevel.resize(nlevels); //图片降采样缩放系数的倒数 float factor = 1.0f / scaleFactor; //每个单位缩放系数所希望的特征点个数 float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));//TODO-3-1-1-1 //用于在特征点个数分配的，特征点的累计计数清空 int sumFeatures = 0; //开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面） for( int level = 0; level < nlevels-1; level++ ) { //分配 cvRound : 返回个参数最接近的整数值 mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);//每层需要分配到的特征点数 //累计 sumFeatures += mnFeaturesPerLevel[level]; //乘系数 nDesiredFeaturesPerScale *= factor; } //由于前面的特征点个数取整操作，可能会导致剩余一些特征点个数没有被分配，所以这里就将这个余出来的特征点分配到最高的图层中 mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0); //成员变量pattern的长度，也就是点的个数，这里的512表示512个点（上面的数组中是存储的坐标所以是256*2*2） const int npoints = 512; //获取用于计算BRIEF描述子的随机采样点点集头指针，并将bit_pattern_31_类型的int[]强制转换为Points* const Point* pattern0 = (const Point*)bit_pattern_31_; //将在全局变量区域的随机采样点以cv::point格式复制到当前类对象中的成员变量中 std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern)); //预先计算圆形patch中行的结束位置 //+1中的1表示那个圆的中间行 umax.resize(HALF_PATCH_SIZE + 1);//HALF_PATCH_SIZE,使用灰度质心法计算特征点的方向信息时，图像块的大小,或者说是直径 //cvFloor返回不大于参数的最大整数值，cvCeil返回不小于参数的最小整数值，cvRound则是四舍五入 int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1); //计算圆的最大行号，+1应该是把中间行也给考虑进去了//这里的二分之根2就是对应那个45°圆心角 int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2); //半径的平方 const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE; //利用圆的方程计算每行像素的u坐标边界（max）
//对应下图中的umax[v]对应u轴从D到C，v对应v轴从0到vmax,即D到B for (v = 0; v <= vmax; ++v) umax[v] = cvRound(sqrt(hp2 - v * v)); //结果都是大于0的结果，表示u坐标在这一行的边界,比如v=0，则u=半径；存储了第一象限中四分之一圆的下半部分，每行u的最大值； // Make sure we are symmetric //这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称
//v轴上从E到B，u轴上从A到C for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)//TODO-3-1-1-2 { while (umax[v0] == umax[v0 + 1]) ++v0; umax[v] = v0; ++v0; } }
TODO-3-1-1-1特征点数量的分配计算
思路：总特征点数一定，根据图像金字塔的层数，层数越高面积越小，对应的特征点就越少，这样将总特征点数目根据面积比例均摊到每层图像上来。
假设需要提取的特征点数目为N，金字塔总共有m层，第0层图像的宽高分别为w,H,对应的面积为C=W *H,图像金字塔缩放因子为s， s属于(0,1)之间，这里的m=8，s = 1/1.2，
则整个金字塔总图像面积为：
S = H * W * (s2)0 + H * W * (s2)1 + ... + H * W * (s2)(m-1)
= H* W * (1 - (s2)m)/ (1 - s2) = C * (1 - (s2)m) / (1 - s2) 等比数列求和：Sn=a1(1-q^n)/(1-q)
那么单位面积应该分配的特征点数量为：
Navg = N / S = N / (C * (1 - (s2)m) / (1 - s2) ) = (N * (1-s2)) / (C (1 - (s2)m))
第0层应该得到的特征点数量为：
N0 = H * W * Navg = (N * (1-s2)) / ( (1 - (s2)m))
则第i层应该分配的特征点数量为：
Ni =C *(s2)i * (N * (1-s2)) / (C *(1 - (s2)m))
在代码中不是按照面积均摊，而是按照面积的开方来均摊特征点的，即将上面的s2 换成s即可。
//TODO-3-1-1-2 求圆内的坐标范围
现在回到文件mono_inertial_euroc.cc中来
## // Pass the image to the SLAM system
// vImuMeas是位于im图像之前的关键帧到im图像之间的imu数据的集合 SLAM.TrackMonocular(im,tframe,vImuMeas);
进入到跟踪函数
## /** * @brief 单目/单目VIO跟踪 * * @param[in] im 灰度图像 * @param[in] timestamp 图像时间戳 * @param[in] vImuMeas 上一帧到当前帧图像之间的IMU测量值 * @param[in] filename 调试用的文件名 * @return Sophus::SE3f 当前帧位姿Tcw */ Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas, string filename) {//clone 是完全的深拷贝，在内存中申请新的空间 //copyTo 也是深拷贝，但是否申请新的内存空间，取决于dst矩阵头中的大小信息是否与src一至，若一致则只深拷贝并不申请新的空间，否则先申请空间后再进行拷贝． cv::Mat imToFeed = im.clone();// Check mode change {// mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式 //纯定位模式，默认为0 if(mbActivateLocalizationMode) { mpLocalMapper->RequestStop();//关闭局部建图线程// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新 mpTracker->InformOnlyTracking(true); // 关闭线程可以使得别的线程得到更多的资源 mbActivateLocalizationMode = false; } }// 如果是单目VIO模式，把IMU数据存储到队列mlQueueImuData if (mSensor == System::IMU_MONOCULAR) for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) mpTracker->GrabImuData(vImuMeas[i_imu]); // 计算相机位姿,timestamp为图像时间戳,imToFeed要处理的图像，filename调试使用的文件 Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);return Tcw; }
进入到函数mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);
## /** * @brief 输入左目RGB或RGBA图像，输出世界坐标系到该帧相机坐标系的变换矩阵 * * @param im 图像 * @param timestamp 时间戳 * @param filename 文件名字，貌似调试用的 * * Step 1 ：将彩色图像转为灰度图像 * Step 2 ：构造Frame * Step 3 ：跟踪 */ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename) {将彩色图像转为灰度图像 mImGray = im;构造Frame类 if(mSensor == System::IMU_MONOCULAR) { // 判断该帧是不是初始化 if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET { // mImGray, // 左目图像 // imGrayRight, // 右目图像 // timestamp, // 时间戳 // mpORBextractorLeft, // 左目特征提取器 // mpORBextractorRight, // 右目特征提取器 // mpORBVocabulary, // 字典 // mK, // 内参矩阵 // mDistCoef, // 去畸变参数 // mbf, // 基线长度 // mThDepth, // 远点,近点的区分阈值 // mpCamera // 相机模型 mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);//TODO1 } else mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib); } // t0存储未初始化时的第1帧图像时间戳 if (mState==NO_IMAGES_YET) t0=timestamp; lastID = mCurrentFrame.mnId; //跟踪 Track();//TODO2 // 返回当前帧的位姿 return mCurrentFrame.GetPose(); }
进入到帧构造函数TODO1
##
// mImGray, // 左目图像 // timestamp, // 时间戳 // mpORBextractorLeft, // 左目特征提取器 // mpORBVocabulary, // 字典 // mpCamera // 相机模型 // mDistCoef, // 去畸变参数 // mbf, // 基线长度 // mThDepth, // 远点,近点的区分阈值 //pPrevF //上一关键帧 //ImuCalib imu的标定参数 // 单目模式 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()), 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)), mbIsSet(false), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(nullptr), mbHasPose(false), mbHasVelocity(false) { //帧的ID 自增 mnId=nNextId++;//获取图像金字塔的层数 8 mnScaleLevels = mpORBextractorLeft->GetLevels(); //获取每层的缩放因子1.2 mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); //计算每层缩放因子的自然对数 mfLogScaleFactor = log(mfScaleFactor); //获取各层图像的缩放因子 mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); //获取各层图像的缩放因子的倒数 mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); //获取sigma^2 mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); //获取sigma^2的倒数 mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); //对这个单目图像进行提取特征点, 第一个参数0-左图， 1-右图 ExtractORB(0,imGray,0,1000); //TODO1-1 // 提取特征点的个数 N = mvKeys.size();// 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 UndistortKeyPoints();//TODO-1-2// 由于单目相机无法直接获得立体信息，所以这里要给右图像对应点和深度赋值-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);//计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行 if(mbInitialComputations) { // 计算去畸变后图像的边界 ComputeImageBounds(imGray);//TODO-1-3 // 表示一个图像像素相当于多少个图像网格列（宽） 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); mvStereo3Dpoints = vector<Eigen::Vector3f>(0); monoLeft = -1; monoRight = -1; // 将特征点分配到图像网格中 AssignFeaturesToGrid();//TODO-1-4 }
特征点提取 TODO-1-1
## // 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中 //位于ORBextractor.cc里面的int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,OutputArray _descriptors, std::vector<int> &vLappingArea) monoLeft = (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors,vLapping);
进入到对应的函数里面
## /** * @brief 用仿函数（重载括号运算符）方法来计算图像特征点 * * @param[in] _image 输入原始图的图像 * @param[in] _mask 掩膜mask * @param[in & out] _keypoints 存储特征点关键点的向量 * @param[in & out] _descriptors 存储特征点描述子的矩阵 */ int ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints, OutputArray _descriptors, std::vector<int> &vLappingArea) {// 构建图像金字塔 ComputePyramid(image);//TODO1-1-1// 存储所有的特征点，vector第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点 vector < vector<KeyPoint> > allKeypoints; //使用四叉树的方式计算每层图像的特征点并进行分配 ComputeKeyPointsOctTree(allKeypoints);//TODO1-1-2//拷贝图像描述子到新的矩阵descriptors Mat descriptors; //统计整个图像金字塔中的特征点 int nkeypoints = 0; //统计每层图像金字塔的特征点个数 for (int level = 0; level < nlevels; ++level) nkeypoints += (int)allKeypoints[level].size(); //如果本图像没有任何的特征点 if( nkeypoints == 0 ) //调用cv::mat类的.realse方法，清空矩阵的引用计数，强制释放矩阵的数据 _descriptors.release(); else { //如果图像金字塔中有特征点，那么就创建这个存储描述子的矩阵，注意这个矩阵是存储整个图像金字塔中特征点的描述子 _descriptors.create(nkeypoints, //矩阵的行数，为特征点的总个数 32, //矩阵的列数，对应为使用32*8=256位描述子 CV_8U); //矩阵元素的格式 //获取这个描述子的矩阵信息 descriptors = _descriptors.getMat(); } _keypoints = vector<cv::KeyPoint>(nkeypoints); //因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以在这里设置了Offset变量来保存“寻址”时的偏移量， //辅助进行在总描述子mat中的定位 int offset = 0; int monoIndex = 0, stereoIndex = nkeypoints-1; for (int level = 0; level < nlevels; ++level) { //获取在allKeypoints中当前层特征点容器的句柄 vector<KeyPoint>& keypoints = allKeypoints[level]; //本层的特征点数 int nkeypointsLevel = (int)keypoints.size(); if(nkeypointsLevel==0) continue;// 深拷贝当前金字塔所在层级的图像，并对其进行高斯模糊 Mat workingMat = mvImagePyramid[level].clone(); // 提取特征点的时候，使用的是清晰的原图像；这里计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊 GaussianBlur(workingMat, //源图像 workingMat, //输出图像 Size(7, 7), //高斯滤波器kernel大小，必须为正的奇数 2, //高斯滤波在x方向的标准差 2, //高斯滤波在y方向的标准差 BORDER_REFLECT_101);//边缘拓展点插值类型// desc存储当前图层的描述子 Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U); //计算高斯模糊后图像的描述子 computeDescriptors(workingMat, //高斯模糊之后的图层图像 keypoints, //当前图层中的特征点集合 desc, //存储计算之后的描述子 pattern); //随机采样点集 // 更新偏移量的值 offset += nkeypointsLevel; // Scale keypoint coordinates // 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下，得到所有层特征点在第0层里的坐标放到_keypoints里面 float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor); int i = 0; for (vector<KeyPoint>::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){ // Scale keypoint coordinates if (level != 0){ // 特征点本身直接乘缩放倍数就可以了 keypoint->pt *= scale; } //vLappingArea if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){ _keypoints.at(stereoIndex) = (*keypoint); desc.row(i).copyTo(descriptors.row(stereoIndex)); stereoIndex--; } else{ _keypoints.at(monoIndex) = (*keypoint); desc.row(i).copyTo(descriptors.row(monoIndex)); monoIndex++; } i++; } return monoIndex; }
ComputePyramid(image);//计算图像金字塔TODO-1-1-1，对每层图像进行了，把源图像拷贝到目的图像的中央，四面填充指定的像素。
## /** * 构建图像金字塔 * @param image 输入原图像，对所有像素点上提取出FAST角点的 */ void ORBextractor::ComputePyramid(cv::Mat image) { for (int level = 0; level < nlevels; ++level) { float scale = mvInvScaleFactor[level]; Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale)); Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2); Mat temp(wholeSize, image.type()), masktemp; mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); // Compute the resized image //计算第0层以上resize后的图像 if( level != 0 ) { //将上一层金字塔图像根据设定sz缩放到当前层级 resize(mvImagePyramid[level-1], //输入图像 mvImagePyramid[level], //输出图像 sz, //输出图像的尺寸 0, //水平方向上的缩放系数，留0表示自动计算 0, //垂直方向上的缩放系数，留0表示自动计算 cv::INTER_LINEAR); //图像缩放的差值算法类型，这里的是线性插值算法 //把源图像拷贝到目的图像的中央，四面填充指定的像素。图片如果已经拷贝到中间，只填充边界 copyMakeBorder(mvImagePyramid[level], //源图像 temp, //目标图像（此时其实就已经有大了一圈的尺寸了） EDGE_THRESHOLD, EDGE_THRESHOLD, //top & bottom 需要扩展的border大小 EDGE_THRESHOLD, EDGE_THRESHOLD, //left & right 需要扩展的border大小 BORDER_REFLECT_101+BORDER_ISOLATED); //扩充方式，opencv给出的解释： /*Various border types, image boundaries are denoted with '|' * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba * BORDER_WRAP: cdefgh|abcdefgh|abcdefg * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' */ } else { //对于底层图像，直接就扩充边界了 copyMakeBorder(image, //源图像 temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, BORDER_REFLECT_101); } } }
//使用四叉树的方式计算每层图像的特征点并进行分配，ComputeKeyPointsOctTree(allKeypoints);//TODO1-1-2
## //计算四叉树的特征点 void ORBextractor::ComputeKeyPointsOctTree( vector<vector<KeyPoint> >& allKeypoints) { //重新调整图像层数 allKeypoints.resize(nlevels); //图像cell的尺寸，是个正方形，可以理解为边长in像素坐标 const float W = 35;//遍历所有图像 for (int level = 0; level < nlevels; ++level) { //计算这层图像的坐标边界,其实就是在最初的原始图像周围增加了3个像素的边界，3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆 const int minBorderX = EDGE_THRESHOLD-3; const int minBorderY = minBorderX; const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; //存储需要进行平均分配的特征点 vector<cv::KeyPoint> vToDistributeKeys; //这里预分配的空间大小是nfeatures*10 vToDistributeKeys.reserve(nfeatures*10); //计算进行特征点提取的图像区域尺寸 const float width = (maxBorderX-minBorderX); const float height = (maxBorderY-minBorderY); //计算网格在当前层的图像有的行数和列数，划分成35*35的像素网格大小 const int nCols = width/W; const int nRows = height/W; //计算每个图像网格所占的像素行数和列数 const int wCell = ceil(width/nCols); const int hCell = ceil(height/nRows); //以网格为单位，开始遍历图像网格 for(int i=0; i<nRows; i++) { //计算当前网格初始行坐标，也就是y方向的坐标 const float iniY =minBorderY+i*hCell; //计算当前网格最大的行坐标，这里的+6=+3+3，即考虑到了多出来3是为了在每个cell边界像素进行FAST特征点提取用 float maxY = iniY+hCell+6; //如果初始的行坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域 if(iniY>=maxBorderY-3) continue; //如果图像的大小导致不能够正好划分出来整齐的图像网格，那么最大的maxY就等于最大的边界maxBorderY if(maxY>maxBorderY) maxY = maxBorderY; //开始列的遍历 for(int j=0; j<nCols; j++) { //计算初始的列坐标，x坐标 const float iniX =minBorderX+j*wCell; //计算这列网格的最大列坐标，+6的含义和前面相同 float maxX = iniX+wCell+6; //判断坐标是否在图像中 if(iniX>=maxBorderX-3) continue; //越界，则取最大的maxBorderX值 if(maxX>maxBorderX) maxX = maxBorderX; // FAST提取特征点, 自适应阈值 //这个向量存储这个网格cell中的特征点 vector<cv::KeyPoint> vKeysCell; //调用opencv的库函数来检测FAST角点 FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像，这里就是当前遍历到的图像块 vKeysCell, //存储角点位置的容器 iniThFAST, //检测阈值 true); //使能非极大值抑制 //如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点 if(vKeysCell.empty()) { //那么就使用更低的阈值来进行重新检测 FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), //待检测的图像 vKeysCell, //存储角点位置的容器 minThFAST, //更低的检测阈值 true); //使能非极大值抑制 } //当图像cell中检测到FAST角点 if(!vKeysCell.empty()) { //遍历其中的所有FAST角点，恢复其在图像金字塔下当前图层的坐标 for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) { //NOTICE 到目前为止，这些角点的坐标都是基于图像网格cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标 (*vit).pt.x+=j*wCell; (*vit).pt.y+=i*hCell; //然后将其加入到”等待被均匀分配“的特征点容器中 vToDistributeKeys.push_back(*vit); } } } }//声明一个对当前图层的特征点的容器的引用 vector<KeyPoint> & keypoints = allKeypoints[level]; //并且调整其大小为欲提取出来的特征点个数，一般不可能提取出这么多的特征点 keypoints.reserve(nfeatures);//返回值是一个保存有特征点的vector容器，含有剔除后的保留下来的特征点，特征点的坐标是在当前图层坐标系下的；TODO1-1-2-1 keypoints = DistributeOctTree(vToDistributeKeys, //当前图层提取出来的特征点 //NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的 minBorderX, maxBorderX, //当前图层图像的边界，而这里的坐标却都是在“边缘扩充图像”下的 minBorderY, maxBorderY, mnFeaturesPerLevel[level], //希望保留下来的当前层图像的特征点个数，在计算图像金字塔每层特征点分配的时候计算出来的 level); //当前层图像所在的图层 //PATCH_SIZE是对于底层的初始图像来说的，现在要根据当前图层的尺度缩放倍数进行缩放得到缩放后的PATCH大小 和特征点的方向计算有关 const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; // Add border to coordinates and scale information //获取剔除过程后保留下来的特征点数目 const int nkps = keypoints.size(); //然后开始遍历这些特征点，恢复其在当前图层图像坐标系下的坐标 for(int i=0; i<nkps ; i++) { //对每一个保留下来的特征点，恢复到相对于当前图层“边缘扩充图像下”的坐标系的坐标 keypoints[i].pt.x+=minBorderX; keypoints[i].pt.y+=minBorderY; //记录特征点来源的图像金字塔图层 keypoints[i].octave=level; //记录计算方向的patch，缩放后对应的大小， 又被称作为特征点半径 keypoints[i].size = scaledPatchSize; } } // compute orientations //然后计算这些特征点的方向信息，注意这里还是分层计算的 TODO-1-1-2-2 for (int level = 0; level < nlevels; ++level) computeOrientation(mvImagePyramid[level], //对应的图层的图像 allKeypoints[level], //这个图层中提取并保留下来的特征点容器 umax); //以及PATCH的横坐标边界 }
进入到TODO1-1-2-1函数 keypoints = DistributeOctTree(...)
## /** * @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发 * * @param[in] vToDistributeKeys 等待进行分配到四叉树中的特征点 * @param[in] minX 当前图层的图像的边界，坐标都是在“半径扩充图像”坐标系下的坐标 * @param[in] maxX * @param[in] minY * @param[in] maxY * @param[in] N 希望提取出的特征点个数 * @param[in] level 指定的金字塔图层，并未使用 * @return vector<cv::KeyPoint> 已经均匀分散好的特征点vector容器 */ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) { // Compute how many initial nodes//计算应该生成的初始节点个数，根节点的数量nIni是根据边界的宽高比值确定的，一般是1或者2,宽图像没问题，如果是长图像可能会有问题 const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY)); //第一个初始节点的x方向有多少个像素 const float hX = static_cast<float>(maxX-minX)/nIni; //存储有提取器节点的列表 list<ExtractorNode> lNodes; //存储初始提取器节点指针的vector vector<ExtractorNode*> vpIniNodes; //然后重新设置其大小 vpIniNodes.resize(nIni); //生成初始提取器节点 for(int i=0; i<nIni; i++) { //生成一个提取器节点 ExtractorNode ni; //设置提取器节点的图像边界 //注意这里和提取FAST角点区域相同，都是“半径扩充图像”，特征点坐标从0 开始 ni.UL = cv::Point2i(hX*static_cast<float>(i),0); //UpLeft ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0); //UpRight ni.BL = cv::Point2i(ni.UL.x,maxY-minY); //BottomLeft ni.BR = cv::Point2i(ni.UR.x,maxY-minY); //BottomRight //重设vkeys大小 ni.vKeys.reserve(vToDistributeKeys.size()); //将刚才生成的提取节点添加到列表中 //虽然这里的ni是局部变量，但是由于这里的push_back()是拷贝参数的内容到一个新的对象中然后再添加到列表中 //所以当本函数退出之后这里的内存不会成为“野指针” lNodes.push_back(ni); //存储这个初始的提取器节点句柄 vpIniNodes[i] = &lNodes.back(); } //将特征点分配到子提取器节点中 for(size_t i=0;i<vToDistributeKeys.size();i++) { //获取这个特征点对象 const cv::KeyPoint &kp = vToDistributeKeys[i]; //按特征点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点） vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);//应该将所有特征点分配给了初始节点 } // 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点 list<ExtractorNode>::iterator lit = lNodes.begin(); while(lit!=lNodes.end()) { //如果初始的提取器节点所分配到的特征点个数为1 if(lit->vKeys.size()==1) { //那么就标志位置位，表示此节点不可再分 lit->bNoMore=true; //更新迭代器 lit++; } ///如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它 else if(lit->vKeys.empty()) //注意，由于是直接删除了它，所以这里的迭代器没有必要更新；否则反而会造成跳过元素的情况 lit = lNodes.erase(lit); else //如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器 lit++; } //结束标志位清空 bool bFinish = false; //记录迭代次数 int iteration = 0; //声明一个vector用于存储节点的vSize和句柄对 //这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄 vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode; //调整大小，意思是一个初始化节点将“分裂”成为四个 vSizeAndPointerToNode.reserve(lNodes.size()*4); //根据特征点分布,利用4叉树方法对图像进行划分区域 while(!bFinish) { //更新迭代次数计数器，只是记录，未起到作用 iteration++; //保存当前节点个数 int prevSize = lNodes.size(); //重新定位迭代器指向列表头部 lit = lNodes.begin(); //需要展开的节点计数，这个一直保持累计，不清零 int nToExpand = 0;//这个变量只是统计了某个循环中的点，记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄 vSizeAndPointerToNode.clear(); // 将目前的子区域进行划分 //开始遍历列表中所有的提取器节点，并进行分解或者保留 while(lit!=lNodes.end()) { //如果提取器节点只有一个特征点， if(lit->bNoMore) {//那么就没有必要再进行细分了 lit++; continue; } else {//如果当前的提取器节点具有超过一个的特征点，那么就要进行继续细分 ExtractorNode n1,n2,n3,n4; //再细分成四个子区域 lit->DivideNode(n1,n2,n3,n4); //如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中 if(n1.vKeys.size()>0) { //注意这里也是添加到列表前面的 lNodes.push_front(n1); //再判断其中子提取器节点中的特征点数目是否大于1 if(n1.vKeys.size()>1) { //如果有超过一个的特征点，那么“待展开的节点计数++” nToExpand++; //保存这个特征点数目和节点指针的信息 vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));// lNodes.front().lit 和前面的迭代的lit 不同，只是名字相同而已 // lNodes.front().lit是node结构体里的一个指针用来记录节点的位置 // 迭代的lit 是while循环里作者命名的遍历的指针名称 lNodes.front().lit = lNodes.begin(); } } //后面的操作都是相同的 if(n2.vKeys.size()>0) { lNodes.push_front(n2); if(n2.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n3.vKeys.size()>0) { lNodes.push_front(n3); if(n3.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n4.vKeys.size()>0) { lNodes.push_front(n4); if(n4.vKeys.size()>1) { nToExpand++; vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } //当这个母节点expand之后就从列表中删除它了 //分裂方式是后加的先分裂，先加的后分裂。 lit=lNodes.erase(lit); //继续下一次循环，其实这里加不加这句话的作用都是一样的 continue; }//判断当前遍历到的节点中是否有超过一个的特征点 }//遍历列表中的所有提取器节点//停止这个过程的条件有两个，满足其中一个即可： //1、当前的节点数已经超过了要求的特征点数 //2、当前所有的节点中都只包含一个特征点 if((int)lNodes.size()>=N //判断是否超过了要求的特征点数 || (int)lNodes.size()==prevSize) //prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的 //节点区域中只有一个特征点，已经不能够再细分了 { //停止标志置位 bFinish = true; } // Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数 //可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3，如果上面已有的节点数lNodes加上nToExpandx3大于N，再进行划分提取 else if(((int)lNodes.size()+nToExpand*3)>N) { //如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出 //这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系，而是前者是累计计数，后者只保存某一个循环的 //一直循环，直到结束标志位被置位 while(!bFinish) { //获取当前的list中的节点个数 prevSize = lNodes.size(); //Prev这里是应该是保留的意思吧，保留那些还可以分裂的节点的信息, 这里是深拷贝 vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; //清空 vSizeAndPointerToNode.clear(); // 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序 // 优先分裂特征点多的节点，使得特征点密集的区域保留更少的特征点 //! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end(),compareNodes); //遍历这个存储了pair对的vector，注意是从后往前遍历 for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--) { ExtractorNode n1,n2,n3,n4; //对每个需要进行分裂的节点进行分裂 vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4); // Add childs if they contain points //其实这里的节点可以说是二级子节点了，执行和前面一样的操作 if(n1.vKeys.size()>0) { lNodes.push_front(n1); if(n1.vKeys.size()>1) { //因为这里还有对于vSizeAndPointerToNode的操作，所以前面才会备份vSizeAndPointerToNode中的数据 //为可能的、后续的又一次for循环做准备 vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n2.vKeys.size()>0) { lNodes.push_front(n2); if(n2.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n3.vKeys.size()>0) { lNodes.push_front(n3); if(n3.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } if(n4.vKeys.size()>0) { lNodes.push_front(n4); if(n4.vKeys.size()>1) { vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); lNodes.front().lit = lNodes.begin(); } } //删除母节点，在这里其实应该是一级子节点 lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit); //判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数 //作者的思想其实就是这样的，再分裂了一次之后判断下一次分裂是否会超过N，如果不是那么就放心大胆地全部进行分裂（因为少了一个判断因此 //其运算速度会稍微快一些），如果会那么就引导到这里进行最后一次分裂 if((int)lNodes.size()>=N) break; }//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂，直到刚刚达到或者超过要求的特征点个数 //这里理想中应该是一个for循环就能够达成结束条件了，但是作者想的可能是，有些子节点所在的区域会没有特征点，因此很有可能一次for循环之后 //的数目还是不能够满足要求，所以还是需要判断结束条件并且再来一次 //判断是否达到了停止条件 if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) bFinish = true; } } }// 保留每个区域响应值最大的一个兴趣点 vector<cv::KeyPoint> vResultKeys; //调整大小为要提取的特征点数目 vResultKeys.reserve(nfeatures); //遍历这个节点列表 for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++) { //得到这个节点区域中的特征点容器句柄 vector<cv::KeyPoint> &vNodeKeys = lit->vKeys; //得到指向第一个特征点的指针，后面作为最大响应值对应的关键点 cv::KeyPoint* pKP = &vNodeKeys[0]; //用第1个关键点响应值初始化最大响应值 float maxResponse = pKP->response; //开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了 for(size_t k=1;k<vNodeKeys.size();k++) { //更新最大响应值 if(vNodeKeys[k].response>maxResponse) { //更新pKP指向具有最大响应值的keypoints pKP = &vNodeKeys[k]; maxResponse = vNodeKeys[k].response; } } //将这个节点区域中的响应值最大的特征点加入最终结果容器 vResultKeys.push_back(*pKP); } //返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点 return vResultKeys; }
computeOrientation计算特征点方向TODO-1-1-2-2
## /** * @brief 计算特征点的方向 * @param[in] image 特征点所在当前金字塔的图像 * @param[in & out] keypoints 特征点向量 * @param[in] umax 每个特征点所在图像区块的每行的边界 u_max 组成的vector */ static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax) { // 遍历所有的特征点 for (vector<KeyPoint>::iterator keypoint = keypoints.begin(), keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) { // 调用IC_Angle 函数计算这个特征点的方向 keypoint->angle = IC_Angle(image, //特征点所在的图层的图像 keypoint->pt, //特征点在这张图像中的坐标 umax); //每个特征点所在图像区块的每行的边界 u_max 组成的vector } }
进一步查看函数IC_Angle
## /** * @brief 这个函数用于计算特征点的方向，这里是返回角度作为方向。 * 计算特征点方向是为了使得提取的特征点具有旋转不变性。 * 方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向 * @param[in] image 要进行操作的某层金字塔图像 * @param[in] pt 当前特征点的坐标 * @param[in] u_max 图像块的每一行的坐标边界 u_max * @return float 返回特征点的角度，范围为[0,360)角度，精度为0.3° */ static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max) { //图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权 int m_01 = 0, m_10 = 0; //获得这个特征点所在的图像块的中心点坐标灰度值的指针center const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));//这条v=0中心线的计算需要特殊对待 //由于是中心行+若干行对，PATCH_SIZE是个奇数,u为x方向,处理v=0时的情况 for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) //注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权 m_10 += u * center[u]; //这里的step1表示这个图像一行包含的字节总数。 int step = (int)image.step1(); //注意这里是以v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度 for (int v = 1; v <= HALF_PATCH_SIZE; ++v) { // Proceed over the two lines //本来m_01应该是一列一列地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行 int v_sum = 0; // 获取某行像素横坐标的最大范围，注意这里的图像块是圆形的！ int d = u_max[v]; //在坐标范围内挨个像素遍历，实际是一次遍历2个 // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) = x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y)) // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) = y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y)) for (int u = -d; u <= d; ++u) { //得到需要进行加运算和减运算的像素灰度值 //val_plus：在中心线下方x=u时的的像素灰度值 //val_minus：在中心线上方x=u时的像素灰度值 int val_plus = center[u + v*step], val_minus = center[u - v*step]; //在v（y轴）上，2行所有像素灰度值之差 v_sum += (val_plus - val_minus); //u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行 m_10 += u * (val_plus + val_minus); } //将这一行上的和按照y坐标加权 m_01 += v * v_sum; } //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3° return fastAtan2((float)m_01, (float)m_10); }
UndistortKeyPoints();//使用OpenCV的矫正函数、内参对提取到的特征点进行矫正TODO-1-2
## /** * @brief 用内参对特征点去畸变，结果报存在mvKeysUn中 * */ void Frame::UndistortKeyPoints() { // 如果第一个畸变参数为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函数进行畸变矫正// 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; }// 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变 // 为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） // cv::undistortPoints最后一个矩阵为空矩阵时，得到的点为归一化坐标点 mat=mat.reshape(2); cv::undistortPoints(mat,mat, static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK); // 调整回只有一个通道，回归我们正常的处理方式 mat=mat.reshape(1);// 存储校正后的特征点 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; } }
ComputeImageBounds(imGray);//由于图像经过去畸变，需要重新计算图像的边界TODO-1-3
## /** * @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; // 和前面校正特征点一样的操作，将这几个边界点作为输入进行校正 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; } }
AssignFeaturesToGrid();//将特征点分配到图像网格中 ，TODO-1-4
## /** * @brief 特征分网格 */ void Frame::AssignFeaturesToGrid() {// 给存储特征点的网格数组 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); } } //遍历每个特征点，将每个特征点在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); } } }
Track();//跟踪TODO2
## /** * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪 * track包含两部分：估计运动、跟踪局部地图 * * Step 1：初始化 * Step 2：跟踪 * Step 3：记录位姿信息，用于轨迹复现 */ void Tracking::Track() {// 如果局部建图里认为IMU有问题，重置当前活跃地图 if(mpLocalMapper->mbBadImu) { mpSystem->ResetActiveMap(); return; } // 从Atlas中取出当前active的地图 Map* pCurrentMap = mpAtlas->GetCurrentMap();
// 创建新地图,里面状态改变为NO_IMAGES_YET，TODO-2-1
CreateMapInAtlas();
// IMU模式下设置IMU的Bias参数,还要保证上一帧存在 if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpLastKeyFrame) mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias()); // 使用上一帧的bias作为当前帧的初值 if(mState==NO_IMAGES_YET) { mState = NOT_INITIALIZED; } mLastProcessedState=mState; // Step 4 IMU模式且没有创建地图的情况下对IMU数据进行预积分 if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mbCreatedMap) { // IMU数据进行预积分 PreintegrateIMU();//后续讲解这次不讲 } mbCreatedMap = false;// 地图更新时加锁。保证地图不会发生变化// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图 unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate); mbMapUpdated = false; // 判断地图id是否更新了 int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex(); int nMapChangeIndex = pCurrentMap->GetLastMapChange(); if(nCurMapChangeIndex>nMapChangeIndex) { // 检测到地图更新了 pCurrentMap->SetLastMapChange(nCurMapChangeIndex); mbMapUpdated = true; } // 初始化 if(mState==NOT_INITIALIZED) { if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD) { // 双目RGBD相机的初始化共用一个函数,TODO-2-2 StereoInitialization(); } else { // 单目初始化,TODO-2-3 MonocularInitialization(); } if(mState!=OK) // If rightly initialized, mState=OK { // 如果没有成功初始化，直接返回 mLastFrame = Frame(mCurrentFrame); return; } if(mpAtlas->GetAllMaps().size() == 1) { // 如果当前地图是第一个地图，记录当前帧id为第一帧 mnFirstFrameId = mCurrentFrame.mnId; } } else { // System is initialized. Track Frame. // 系统成功初始化，下面是具体跟踪过程,——————————————————————————————————————————————————————————————下次讲解 bool bOK; // Initial camera pose estimation using motion model or relocalization (if tracking is lost) // mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式 // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking if(!mbOnlyTracking) {// Local Mapping is activated. This is the normal behaviour, unless // you explicitly activate the "only tracking" mode. // 跟踪进入正常SLAM模式，有地图更新 // 如果正常跟踪 if(mState==OK) { // Local Mapping might have changed some MapPoints tracked in last frame // Step 6.1 检查并更新上一帧被替换的MapPoints // 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查 CheckReplacedInLastFrame(); // Step 6.2 运动模型是空的并且imu未初始化或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪 // 第一个条件,如果运动模型为空并且imu未初始化,说明是刚开始第一帧跟踪，或者已经跟丢了。 // 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们用重定位帧来恢复位姿 // mnLastRelocFrameId 上一次重定位的那一帧 if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2) { Verbose::PrintMess("TRACK: Track with respect to the reference KF ", Verbose::VERBOSITY_DEBUG); bOK = TrackReferenceKeyFrame(); } else { Verbose::PrintMess("TRACK: Track with motion model", Verbose::VERBOSITY_DEBUG); // 用恒速模型跟踪。所谓的恒速就是假设上上帧到上一帧的位姿=上一帧的位姿到当前帧位姿 // 根据恒速模型设定当前帧的初始位姿，用最近的普通帧来跟踪当前的普通帧 // 通过投影的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点所对应3D点的投影误差即可得到位姿 bOK = TrackWithMotionModel(); if(!bOK) bOK = TrackReferenceKeyFrame(); // 根据恒速模型失败了，只能根据参考关键帧来跟踪 } // 新增了一个状态RECENTLY_LOST，主要是结合IMU看看能不能拽回来 // Step 6.3 如果经过跟踪参考关键帧、恒速模型跟踪都失败的话，并满足一定条件就要标记为RECENTLY_LOST或LOST if (!bOK) { // 条件1：如果当前帧距离上次重定位成功不到1s // mnFramesToResetIMU 表示经过多少帧后可以重置IMU，一般设置为和帧率相同，对应的时间是1s // 条件2：单目+IMU 或者 双目+IMU模式 // 同时满足条件1，2，标记为LOST if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) && (mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD)) { mState = LOST; } else if(pCurrentMap->KeyFramesInMap()>10) { // cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl; // 条件1：当前地图中关键帧数目较多（大于10） // 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）或者非IMU模式 // 同时满足条件1，2，则将状态标记为RECENTLY_LOST，后面会结合IMU预测的位姿看看能不能拽回来 mState = RECENTLY_LOST; // 记录丢失时间 mTimeStampLost = mCurrentFrame.mTimeStamp; } else { mState = LOST; } } } else // 跟踪不正常按照下面处理 { // 如果是RECENTLY_LOST状态 if (mState == RECENTLY_LOST) { Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL); // bOK先置为true bOK = true; if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)) { // IMU模式下可以用IMU来预测位姿，看能否拽回来 // Step 6.4 如果当前地图中IMU已经成功初始化，就用IMU数据预测位姿 if(pCurrentMap->isImuInitialized()) PredictStateIMU(); else bOK = false; // 如果IMU模式下当前帧距离跟丢帧超过5s还没有找回（time_recently_lost默认为5s） // 放弃了，将RECENTLY_LOST状态改为LOST if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost) { mState = LOST; Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL); bOK=false; } } else { // Step 6.5 纯视觉模式则进行重定位。主要是BOW搜索，EPnP求解位姿 // Relocalization bOK = Relocalization(); //std::cout << "mCurrentFrame.mTimeStamp:" << to_string(mCurrentFrame.mTimeStamp) << std::endl; //std::cout << "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl; if(mCurrentFrame.mTimeStamp-mTimeStampLost>3.0f && !bOK) { // 纯视觉模式下重定位失败，状态为LOST mState = LOST; Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL); bOK=false; } } } else if (mState == LOST) // 上一帧为最近丢失且重定位失败时 { // Step 6.6 如果是LOST状态 // 开启一个新地图 Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL); if (pCurrentMap->KeyFramesInMap()<10) { // 当前地图中关键帧数目小于10，重置当前地图 mpSystem->ResetActiveMap(); Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL); }else CreateMapInAtlas(); // 当前地图中关键帧数目超过10，创建新地图 // 干掉上一个关键帧 if(mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL); Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); return; } } } else // 纯定位模式 { // Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode) // 只进行跟踪tracking，局部地图不工作 if(mState==LOST) { if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL); // Step 6.1 LOST状态进行重定位 bOK = Relocalization(); } else { // mbVO是mbOnlyTracking为true时的才有的一个变量 // mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉) // mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跟丢 if(!mbVO) { // In last frame we tracked enough MapPoints in the map // Step 6.2 如果跟踪状态正常，使用恒速模型或参考关键帧跟踪 if(mbVelocity) { // 优先使用恒速模型跟踪 bOK = TrackWithMotionModel(); } else { // 如果恒速模型不被满足,那么就只能够通过参考关键帧来跟踪 bOK = TrackReferenceKeyFrame(); } } else { // In last frame we tracked mainly "visual odometry" points. // We compute two camera poses, one from motion model and one doing relocalization. // If relocalization is sucessfull we choose that solution, otherwise we retain // the "visual odometry" solution. // mbVO为true，表明此帧匹配了很少（小于10）的地图点，要跟丢的节奏，既做跟踪又做重定位 // MM=Motion Model,通过运动模型进行跟踪的结果 bool bOKMM = false; // 通过重定位方法来跟踪的结果 bool bOKReloc = false; // 运动模型中构造的地图点 vector<MapPoint*> vpMPsMM; // 在追踪运动模型后发现的外点 vector<bool> vbOutMM; // 运动模型得到的位姿 Sophus::SE3f TcwMM; // Step 6.3 当运动模型有效的时候,根据运动模型计算位姿 if(mbVelocity) { bOKMM = TrackWithMotionModel(); // 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量 vpMPsMM = mCurrentFrame.mvpMapPoints; vbOutMM = mCurrentFrame.mvbOutlier; TcwMM = mCurrentFrame.GetPose(); } // Step 6.4 使用重定位的方法来得到当前帧的位姿 bOKReloc = Relocalization(); // Step 6.5 根据前面的恒速模型、重定位结果来更新状态 if(bOKMM && !bOKReloc) { // 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果 mCurrentFrame.SetPose(TcwMM); mCurrentFrame.mvpMapPoints = vpMPsMM; mCurrentFrame.mvbOutlier = vbOutMM; // 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数 if(mbVO) { // 更新当前帧的地图点被找到次数，注意跟观测次数区分，这是两个概念 for(int i =0; i<mCurrentFrame.N; i++) { // 如果这个特征点形成了地图点,并且也不是外点的时候 if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) { // 增加被找到次数 mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); } } } } else if(bOKReloc) { // 只要重定位成功整个跟踪过程正常进行（重定位与跟踪，更相信重定位） mbVO = false; } // 有一个成功我们就认为执行成功了 bOK = bOKReloc || bOKMM; } } } // 将最新的关键帧作为当前帧的参考关键帧 // mpReferenceKF先是上一时刻的参考关键帧，如果当前为新关键帧则变成当前关键帧，如果不是新的关键帧则先为上一帧的参考关键帧，而后经过更新局部关键帧重新确定 if(!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now(); double timePosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPosePred - time_StartPosePred).count(); vdPosePred_ms.push_back(timePosePred); #endif #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now(); #endif // Step 7 在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化 // 在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 // local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系 // 前面主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints， // 然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化 // If we have an initial estimation of the camera pose and matching. Track the local map. if(!mbOnlyTracking) { if(bOK) { // 局部地图跟踪 bOK = TrackLocalMap(); } if(!bOK) cout << "Fail to track local map!" << endl; } else { // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes // the camera we will use the local map again. if(bOK && !mbVO) bOK = TrackLocalMap(); } // 到此为止跟踪确定位姿阶段结束，下面开始做收尾工作和为下一帧做准备 // 查看到此为止时的两个状态变化 // bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true -->OK 1 跟踪局部地图成功 // \ \ \---局部地图跟踪失败---false // \ \---当前帧跟踪失败---false // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true -->OK 2 重定位 // \ \---局部地图跟踪失败---false // \---重定位失败---false // // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK -->OK 1 跟踪局部地图成功 // \ \ \---局部地图跟踪失败---OK -->OK 3 正常跟踪 // \ \---当前帧跟踪失败---非OK // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---非OK // \ \---局部地图跟踪失败---非OK // \---重定位失败---非OK（传不到这里，因为直接return了） // 由上图可知当前帧的状态OK的条件是跟踪局部地图成功，重定位或正常跟踪都可 // Step 8 根据上面的操作来判断是否追踪成功 if(bOK) // 此时还OK才说明跟踪成功了 mState = OK; else if (mState == OK) // 由上图可知只有当第一阶段跟踪成功，但第二阶段局部地图跟踪失败时执行 { // 状态变为最近丢失 if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) { Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL); if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2()) { // IMU模式下IMU没有成功初始化或者没有完成IMU BA，则重置当前地图 cout << "IMU is not or recently initialized. Reseting active map..." << endl; mpSystem->ResetActiveMap(); } mState=RECENTLY_LOST; } else mState=RECENTLY_LOST; // 上一个版本这里直接判定丢失 LOST // 被注释掉了，记录丢失时间 /*if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames) {*/ mTimeStampLost = mCurrentFrame.mTimeStamp; //} } // Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified) // 这段貌似没啥作用 if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) && (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && pCurrentMap->isImuInitialized()) { // TODO check this situation Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL); Frame* pF = new Frame(mCurrentFrame); pF->mpPrevFrame = new Frame(mLastFrame); // Load preintegration pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame); } // 下面代码没有用 if(pCurrentMap->isImuInitialized()) { if(bOK) { // 当前帧距离上次重定位帧刚好等于1s，重置（还未实现 TODO） if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU)) { cout << "RESETING FRAME!!!" << endl; ResetFrameIMU(); } else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30)) mLastBias = mCurrentFrame.mImuBias; // 没啥用，后面会重新赋值后传给普通帧 } } #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now(); double timeLMTrack = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLMTrack - time_StartLMTrack).count(); vdLMTrack_ms.push_back(timeLMTrack); #endif // Update drawer // 更新显示线程中的图像、特征点、地图点等信息 mpFrameDrawer->Update(this); if(mCurrentFrame.isSet()) mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); // 查看到此为止时的两个状态变化 // bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true // \ \ \---局部地图跟踪失败---false // \ \---当前帧跟踪失败---false // \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true // \ \---局部地图跟踪失败---false // \---重定位失败---false // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK // \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST） // \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST) // \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK // \ \ \---局部地图跟踪失败---LOST // \ \---重定位失败---LOST（传不到这里，因为直接return了） // \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了） // Step 9 如果跟踪成功 或 最近刚刚跟丢，更新速度，清除无效地图点，按需创建关键帧 if(bOK || mState==RECENTLY_LOST) { // Update motion model // Step 9.1 更新恒速运动模型 TrackWithMotionModel 中的mVelocity if(mLastFrame.isSet() && mCurrentFrame.isSet()) { Sophus::SE3f LastTwc = mLastFrame.GetPose().inverse(); // mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc mVelocity = mCurrentFrame.GetPose() * LastTwc; mbVelocity = true; } else { // 否则没有速度 mbVelocity = false; } // 使用IMU积分的位姿显示 if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); // Clean VO matches // Step 9.2 清除观测不到的地图点 for(int i=0; i<mCurrentFrame.N; i++) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; if(pMP) if(pMP->Observations()<1) { mCurrentFrame.mvbOutlier[i] = false; mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); } } // Delete temporal MapPoints // Step 9.3 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd） // 上个步骤中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除 // 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中 for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++) { MapPoint* pMP = *lit; delete pMP; } // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露 mlpTemporalPoints.clear(); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now(); #endif // 判断是否需要插入关键帧 bool bNeedKF = NeedNewKeyFrame(); // Check if we need to insert a new keyframe // if(bNeedKF && bOK) // Step 9.4 根据条件来判断是否插入关键帧 // 需要同时满足下面条件1和2 // 条件1：bNeedKF=true，需要插入关键帧 // 条件2：bOK=true跟踪成功 或 IMU模式下的RECENTLY_LOST模式且mInsertKFsLost为true if(bNeedKF && (bOK || (mInsertKFsLost && mState==RECENTLY_LOST && (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)))) CreateNewKeyFrame(); // 创建关键帧，对于双目或RGB-D会产生新的地图点 #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now(); double timeNewKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndNewKF - time_StartNewKF).count(); vdNewKF_ms.push_back(timeNewKF); #endif // We allow points with high innovation (considererd outliers by the Huber Function) // pass to the new keyframe, so that bundle adjustment will finally decide // if they are outliers or not. We don't want next frame to estimate its position // with those points so we discard them in the frame. Only has effect if lastframe is tracked // 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点 // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉 // Step 9.5 删除那些在BA中检测为外点的地图点 for(int i=0; i<mCurrentFrame.N;i++) { if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i]) mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); } } // Reset if the camera get lost soon after initialization // Step 10 如果第二阶段跟踪失败，跟踪状态为LOST if(mState==LOST) { // 如果地图中关键帧小于10，重置当前地图，退出当前跟踪 if(pCurrentMap->KeyFramesInMap()<=10) // 上一个版本这里是5 { mpSystem->ResetActiveMap(); return; } if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) if (!pCurrentMap->isImuInitialized()) { // 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪 Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET); mpSystem->ResetActiveMap(); return; } // 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图 CreateMapInAtlas(); // 新增加了个return return; } // 确保已经设置了参考关键帧 if(!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; // 保存上一帧的数据,当前帧变上一帧 mLastFrame = Frame(mCurrentFrame); } // 查看到此为止 // mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK // \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST） // \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST) // \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK // \ \ \---局部地图跟踪失败---LOST // \ \---重定位失败---LOST（传不到这里，因为直接return了） // \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了） // last.记录位姿信息，用于轨迹复现 // Step 11 记录位姿信息，用于最后保存所有的轨迹 if(mState==OK || mState==RECENTLY_LOST) { // Store frame pose information to retrieve the complete camera trajectory afterwards. // Step 11：记录位姿信息，用于最后保存所有的轨迹 if(mCurrentFrame.isSet()) { // 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1 Sophus::SE3f Tcr_ = mCurrentFrame.GetPose() * mCurrentFrame.mpReferenceKF->GetPoseInverse(); mlRelativeFramePoses.push_back(Tcr_); mlpReferences.push_back(mCurrentFrame.mpReferenceKF); mlFrameTimes.push_back(mCurrentFrame.mTimeStamp); mlbLost.push_back(mState==LOST); } else { // This can happen if tracking is lost // 如果跟踪失败，则相对位姿使用上一次值 mlRelativeFramePoses.push_back(mlRelativeFramePoses.back()); mlpReferences.push_back(mlpReferences.back()); mlFrameTimes.push_back(mlFrameTimes.back()); mlbLost.push_back(mState==LOST); } } }
// CreateMapInAtlas();//创建新地图,里面状态改变为NO_IMAGES_YET，TODO-2-1
##
/** * @brief 在Atlas中保存当前地图，创建新地图，所有跟状态相关的变量全部重置 * 1. 前后两帧对应的时间戳反了 * 2. imu模式下前后帧超过1s * 3. 上一帧为最近丢失且重定位失败时 * 4. 重定位成功，局部地图跟踪失败 */ void Tracking::CreateMapInAtlas() { mnLastInitFrameId = mCurrentFrame.mnId; mpAtlas->CreateNewMap(); if (mSensor==System::IMU_STEREO || mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_RGBD) mpAtlas->SetInertialSensor(); // mpAtlas中map的mbIsInertial=true mbSetInit=false; mnInitialFrameId = mCurrentFrame.mnId+1; mState = NO_IMAGES_YET; // Restart the variable with information about the last KF mbVelocity = false; mbVO = false; // Init value for know if there are enough MapPoints in the last KF if(mSensor == System::MONOCULAR || mSensor == System::IMU_MONOCULAR) { mbReadyToInitializate = false; } if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpImuPreintegratedFromLastKF) { delete mpImuPreintegratedFromLastKF; mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); //_________________________________________后续讲解 } if(mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL); if(mpReferenceKF) mpReferenceKF = static_cast<KeyFrame*>(NULL); mLastFrame = Frame(); mCurrentFrame = Frame(); mvIniMatches.clear(); mlQueueImuData.clear(); mbCreatedMap = true; }
StereoInitialization(); // 双目RGBD相机的初始化共用一个函数,TODO-2-2
## /* * @brief 双目和rgbd的地图初始化，比单目简单很多 * * 由于具有深度信息，直接生成MapPoints */ void Tracking::StereoInitialization() { // 初始化要求当前帧的特征点超过500 if(mCurrentFrame.N>500) { if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) { if (!mCurrentFrame.mpImuPreintegrated || !mLastFrame.mpImuPreintegrated) { cout << "not IMU meas" << endl; return; } //判断相邻两帧之间的加速度值够不够用于初始化 if (!mFastInit && (mCurrentFrame.mpImuPreintegratedFrame->avgA-mLastFrame.mpImuPreintegratedFrame->avgA).norm()<0.5) { cout << "not enough acceleration" << endl; return; } if(mpImuPreintegratedFromLastKF) delete mpImuPreintegratedFromLastKF; mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; }// 设定初始位姿为单位旋转，0平移，imu模式下设置的是相机位姿 if (mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) { Eigen::Matrix3f Rwb0 = mCurrentFrame.mImuCalib.mTcb.rotationMatrix(); Eigen::Vector3f twb0 = mCurrentFrame.mImuCalib.mTcb.translation(); Eigen::Vector3f Vwb0; Vwb0.setZero(); mCurrentFrame.SetImuPoseVelocity(Rwb0, twb0, Vwb0); } else mCurrentFrame.SetPose(Sophus::SE3f()); // Create KeyFrame // 将当前帧构造为初始关键帧 KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);// 在地图中添加该初始关键帧 mpAtlas->AddKeyFrame(pKFini); // Create MapPoints and asscoiate to KeyFrame if(!mpCamera2){ // 为每个特征点构造MapPoint for(int i=0; i<mCurrentFrame.N;i++) { // 只有具有正深度的点才会被构造地图点 float z = mCurrentFrame.mvDepth[i]; if(z>0) { // 通过反投影得到该特征点的世界坐标系下3D坐标 Eigen::Vector3f x3D; mCurrentFrame.UnprojectStereo(i, x3D); MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap());//TODO-2-2-1 // 为该MapPoint添加属性： // a.观测到该MapPoint的关键帧 // b.该MapPoint的描述子 // c.该MapPoint的平均观测方向和深度范围 pNewMP->AddObservation(pKFini,i);//TODO-2-2-2 pKFini->AddMapPoint(pNewMP,i); pNewMP->ComputeDistinctiveDescriptors();//TODO-2-2-3 pNewMP->UpdateNormalAndDepth();//TODO-2-2-4 mpAtlas->AddMapPoint(pNewMP); mCurrentFrame.mvpMapPoints[i]=pNewMP; } } } else{ for(int i = 0; i < mCurrentFrame.Nleft; i++){ int rightIndex = mCurrentFrame.mvLeftToRightMatch[i]; if(rightIndex != -1){ Eigen::Vector3f x3D = mCurrentFrame.mvStereo3Dpoints[i]; MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap()); pNewMP->AddObservation(pKFini,i); pNewMP->AddObservation(pKFini,rightIndex + mCurrentFrame.Nleft); pKFini->AddMapPoint(pNewMP,i); pKFini->AddMapPoint(pNewMP,rightIndex + mCurrentFrame.Nleft); pNewMP->ComputeDistinctiveDescriptors(); pNewMP->UpdateNormalAndDepth(); mpAtlas->AddMapPoint(pNewMP); mCurrentFrame.mvpMapPoints[i]=pNewMP; mCurrentFrame.mvpMapPoints[rightIndex + mCurrentFrame.Nleft]=pNewMP; } } }
MapPoint* pNewMP = new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap());//创建地图点TODO-2-2-1
## /** * @brief 构造函数 */ MapPoint::MapPoint(const Eigen::Vector3f &Pos, Map* pMap, Frame* pFrame, const int &idxF): mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0), mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap), mnOriginMapId(pMap->GetId()) { SetWorldPos(Pos); Eigen::Vector3f Ow; if(pFrame -> Nleft == -1 || idxF < pFrame -> Nleft){ Ow = pFrame->GetCameraCenter();// 获取(左目)相机的中心在世界坐标系下的坐标 } else{ Eigen::Matrix3f Rwl = pFrame->GetRwc(); Eigen::Vector3f tlr = pFrame->GetRelativePoseTlr().translation(); Eigen::Vector3f twl = pFrame->GetOw(); Ow = Rwl * tlr + twl; } mNormalVector = mWorldPos - Ow; mNormalVector = mNormalVector / mNormalVector.norm();//在相机坐标系下的方向 Eigen::Vector3f PC = mWorldPos - Ow; const float dist = PC.norm(); const int level = (pFrame -> Nleft == -1) ? pFrame->mvKeysUn[idxF].octave : (idxF < pFrame -> Nleft) ? pFrame->mvKeys[idxF].octave : pFrame -> mvKeysRight[idxF].octave; const float levelScaleFactor = pFrame->mvScaleFactors[level]; const int nLevels = pFrame->mnScaleLevels; mfMaxDistance = dist*levelScaleFactor; mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1]; pFrame->mDescriptors.row(idxF).copyTo(mDescriptor); // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. unique_lock<mutex> lock(mpMap->mMutexPointCreation); mnId=nNextId++; }
pNewMP->AddObservation(pKFini,i);//TODO-2-2-2
## /** * @brief 给地图点添加观测 * * 记录哪些 KeyFrame 的那个特征点能观测到该 地图点 * 并增加观测的相机数目nObs，单目+1，双目或者rgbd+2 * 这个函数是建立关键帧共视关系的核心函数，能共同观测到某些地图点的关键帧是共视关键帧 * @param pKF KeyFrame * @param idx MapPoint在KeyFrame中的索引 */ void MapPoint::AddObservation(KeyFrame* pKF, int idx) { unique_lock<mutex> lock(mMutexFeatures); tuple<int,int> indexes; if(mObservations.count(pKF)){ indexes = mObservations[pKF]; } else{ indexes = tuple<int,int>(-1,-1); } if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){ get<1>(indexes) = idx; } else{ get<0>(indexes) = idx; } // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引 mObservations[pKF]=indexes; if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0) nObs+=2; else nObs++; }
pNewMP->ComputeDistinctiveDescriptors();//TODO-2-2-3
## /** * @brief 计算地图点具有代表性的描述子 * * 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子 * 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值 */ void MapPoint::ComputeDistinctiveDescriptors() { // Retrieve all observed descriptors vector<cv::Mat> vDescriptors; map<KeyFrame*,tuple<int,int>> observations; // 获取所有观测，跳过坏点 { unique_lock<mutex> lock1(mMutexFeatures); if(mbBad) return; observations=mObservations; } if(observations.empty()) return; vDescriptors.reserve(observations.size()); //遍历观测到3d点的所有关键帧，获得orb描述子，并插入到vDescriptors中 for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { // mit->first取观测到该地图点的关键帧 // mit->second取该地图点在关键帧中的索引 KeyFrame* pKF = mit->first; if(!pKF->isBad()) { tuple<int,int> indexes = mit -> second; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); if(leftIndex != -1) { vDescriptors.push_back(pKF->mDescriptors.row(leftIndex)); } if(rightIndex != -1) { vDescriptors.push_back(pKF->mDescriptors.row(rightIndex)); } } } if(vDescriptors.empty()) return; // 获得这些描述子两两之间的距离 // N表示为一共多少个描述子 const size_t N = vDescriptors.size(); float Distances[N][N]; for(size_t i=0;i<N;i++) { // 和自己的距离当然是0 Distances[i][i]=0; for(size_t j=i+1;j<N;j++) { int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]); Distances[i][j]=distij; Distances[j][i]=distij; } } // 选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值 int BestMedian = INT_MAX; int BestIdx = 0; for(size_t i=0;i<N;i++) { // 第i个描述子到其它所有所有描述子之间的距离 vector<int> vDists(Distances[i],Distances[i]+N); sort(vDists.begin(),vDists.end()); // 获得中值 int median = vDists[0.5*(N-1)]; // 寻找最小的中值 if(median<BestMedian) { BestMedian = median; BestIdx = i; } } { unique_lock<mutex> lock(mMutexFeatures); // 最好的描述子，该描述子相对于其他描述子有最小的距离中值 mDescriptor = vDescriptors[BestIdx].clone(); } }
pNewMP->UpdateNormalAndDepth();//更新平均观测方向以及观测距离范围TODO-2-2-4
/** * @brief 更新平均观测方向以及观测距离范围 * * 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要更新相应变量 * 创建新的关键帧的时候会调用 */ void MapPoint::UpdateNormalAndDepth() { map<KeyFrame*,tuple<int,int>> observations; KeyFrame* pRefKF; Eigen::Vector3f Pos; { unique_lock<mutex> lock1(mMutexFeatures); unique_lock<mutex> lock2(mMutexPos); if(mbBad) return; observations = mObservations; // 获得观测到该地图点的所有关键帧 pRefKF = mpRefKF; // 观测到该点的参考关键帧（第一次创建时的关键帧） Pos = mWorldPos; // 地图点在世界坐标系中的位置 } if(observations.empty()) return; // 计算该地图点的法线方向，也就是朝向等信息。 // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向 // 初始值为0向量，累加为归一化向量，最后除以总数n Eigen::Vector3f normal; normal.setZero(); int n = 0; for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { KeyFrame* pKF = mit->first; tuple<int,int> indexes = mit -> second; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); if(leftIndex != -1) { Eigen::Vector3f Owi = pKF->GetCameraCenter(); Eigen::Vector3f normali = Pos - Owi; normal = normal + normali / normali.norm(); n++; } if(rightIndex != -1) { Eigen::Vector3f Owi = pKF->GetRightCameraCenter(); Eigen::Vector3f normali = Pos - Owi; normal = normal + normali / normali.norm(); n++; } } Eigen::Vector3f PC = Pos - pRefKF->GetCameraCenter(); // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示） const float dist = PC.norm(); // 该点到参考关键帧相机的距离 //该地图点在参考关键帧上的索引 tuple<int ,int> indexes = observations[pRefKF]; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); int level; if(pRefKF -> NLeft == -1) { level = pRefKF->mvKeysUn[leftIndex].octave; } else if(leftIndex != -1) { level = pRefKF -> mvKeys[leftIndex].octave; } else{ level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave; } //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave; const float levelScaleFactor = pRefKF->mvScaleFactors[level]; // 当前金字塔层对应的缩放倍数 const int nLevels = pRefKF->mnScaleLevels; // 金字塔层数 { unique_lock<mutex> lock3(mMutexPos); // 使用方法见PredictScale函数前的注释 mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限 mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限 mNormalVector = normal/n; // 获得地图点平均的观测方向 } }
MonocularInitialization(); // 单目初始化,TODO-2-3
##
/* * @brief 单目的地图初始化 * * 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云 * 得到初始两帧的匹配、相对运动、初始MapPoints * * Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧 * Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧 * Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对 * Step 4：如果初始化的两帧之间的匹配点太少，重新初始化 * Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints * Step 6：删除那些无法进行三角化的匹配点 * Step 7：将三角化得到的3D点包装成MapPoints */ void Tracking::MonocularInitialization() { // Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个 if(!mbReadyToInitializate) { // Set Reference Frame // 单目初始帧的特征点数必须大于100 if(mCurrentFrame.mvKeys.size()>100) { // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame mInitialFrame = Frame(mCurrentFrame); // 用当前帧更新上一帧 mLastFrame = Frame(mCurrentFrame); // mvbPrevMatched 记录"上一帧"所有特征点 mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size()); for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++) mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt; // 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id fill(mvIniMatches.begin(),mvIniMatches.end(),-1); // 初始化预积分 if (mSensor == System::IMU_MONOCULAR) { if(mpImuPreintegratedFromLastKF) { delete mpImuPreintegratedFromLastKF; } mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib); mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; } // 下一帧准备做单目初始化了 mbReadyToInitializate = true; return; } } else // 第二帧来了 { // Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器 if (((int)mCurrentFrame.mvKeys.size()<=100)||((mSensor == System::IMU_MONOCULAR)&&(mLastFrame.mTimeStamp-mInitialFrame.mTimeStamp>1.0))) { mbReadyToInitializate = false; return; } // Find correspondences // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对 // 0.9 表示最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7 // true 表示检查特征点的方向 ORBmatcher matcher(0.9,true); // 对 mInitialFrame,mCurrentFrame 进行特征点匹配 // mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标 // mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引TODO-2-3-1 int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100); // Check if there are enough correspondences // Step 4 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化 if(nmatches<100) { mbReadyToInitializate = false; return; } // Step 5 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints TODO-2-3-2 Sophus::SE3f Tcw; vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches) if(mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated)) { // Step 6 初始化成功后，删除那些无法进行三角化的匹配点 for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++) { if(mvIniMatches[i]>=0 && !vbTriangulated[i]) { mvIniMatches[i]=-1; nmatches--; } } // Set Frame Poses // Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵 mInitialFrame.SetPose(Sophus::SE3f()); // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵 mCurrentFrame.SetPose(Tcw); // Step 8 创建初始化地图点MapPoints // Initialize函数会得到mvIniP3D， // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量， // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中,TODO-2-3-3 CreateInitialMapMonocular(); } } }
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);// 在mInitialFrame与mCurrentFrame中找匹配的特征点对 TODO-2-3-1
## int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize) { int nmatches=0; // F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间 vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1); // Step 1 构建旋转直方图，HISTO_LENGTH = 30 vector<int> rotHist[HISTO_LENGTH]; // 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量 for(int i=0;i<HISTO_LENGTH;i++) rotHist[i].reserve(500); const float factor = HISTO_LENGTH/360.0f;// 匹配点对距离，注意是按照F2特征点数目分配空间 vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX); // 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间 vector<int> vnMatches21(F2.mvKeysUn.size(),-1); // 遍历帧1中的所有特征点 for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++) { cv::KeyPoint kp1 = F1.mvKeysUn[i1]; int level1 = kp1.octave; // 只使用原始图像上提取的特征点 if(level1>0) continue; // Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 // vbPrevMatched 输入的是参考帧 F1的特征点 // windowSize = 100，输入最大最小金字塔层级 均为0 TODO-2-3-1-1 vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1); // 没有候选特征点，跳过 if(vIndices2.empty()) continue;// 取出参考帧F1中当前遍历特征点对应的描述子 cv::Mat d1 = F1.mDescriptors.row(i1); int bestDist = INT_MAX; //最佳描述子匹配距离，越小越好 int bestDist2 = INT_MAX; //次佳描述子匹配距离 int bestIdx2 = -1; //最佳候选特征点在F2中的index // Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的 for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++) { size_t i2 = *vit; // 取出候选特征点对应的描述子 cv::Mat d2 = F2.mDescriptors.row(i2); // 计算两个特征点描述子距离 int dist = DescriptorDistance(d1,d2); if(vMatchedDistance[i2]<=dist) continue; // 如果当前匹配距离更小，更新最佳次佳距离 if(dist<bestDist) { bestDist2=bestDist; bestDist=dist; bestIdx2=i2; } else if(dist<bestDist2) { bestDist2=dist; } } // 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配 // 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值 if(bestDist<=TH_LOW) { // 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高 if(bestDist<(float)bestDist2*mfNNratio) { // 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉 if(vnMatches21[bestIdx2]>=0) { vnMatches12[vnMatches21[bestIdx2]]=-1; nmatches--; } // 次优的匹配关系，双向建立 // vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引 vnMatches12[i1]=bestIdx2; vnMatches21[bestIdx2]=i1; vMatchedDistance[bestIdx2]=bestDist; nmatches++; // Step 5 计算匹配点旋转角度差所在的直方图 if(mbCheckOrientation) { // 计算匹配特征点的角度差，这里单位是角度°，不是弧度 float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle; if(rot<0.0) rot+=360.0f; // 前面factor = HISTO_LENGTH/360.0f // bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin int bin = round(rot*factor); // 如果bin 满了又是一个轮回 if(bin==HISTO_LENGTH) bin=0; assert(bin>=0 && bin<HISTO_LENGTH); rotHist[bin].push_back(i1); } } } } // Step 6 筛除旋转直方图中“非主流”部分 if(mbCheckOrientation) { int ind1=-1; int ind2=-1; int ind3=-1; // 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引 ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); for(int i=0; i<HISTO_LENGTH; i++) { if(i==ind1 || i==ind2 || i==ind3) continue; // 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向” for(size_t j=0, jend=rotHist[i].size(); j<jend; j++) { int idx1 = rotHist[i][j]; if(vnMatches12[idx1]>=0) { vnMatches12[idx1]=-1; nmatches--; } } } } //Update prev matched // Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++) if(vnMatches12[i1]>=0) vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt; return nmatches; }
F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);// 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 TODO-2-3-1-1
## /** * @brief 给定一个坐标，返回区域内所有特征点 * @param x 给定点的x * @param y 给定点的y * @param r 搜索半径 * @param minLevel 金字塔下边界 * @param maxLevel 金字塔上边界 * @param bRight 是否是右相机 */ 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; }
mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,mCurrentFrame.mvKeysUn,mvIniMatches,Tcw,mvIniP3D,vbTriangulated);// 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints TODO-2-3-2
## /** 三角化恢复三维点 单目初始化时使用 * @param vKeys1 第一帧的关键点 * @param vKeys2 第二帧的关键点 * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 * @param R21 顾名思义 * @param t21 顾名思义 * @param vP3D 恢复出的三维点 * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 */ bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2, const std::vector<int> &vMatches12, Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated) { if (!tvr) { Eigen::Matrix3f K = this->toK_(); tvr = new TwoViewReconstruction(K); } return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, T21, vP3D, vbTriangulated); }
进入到函数Reconstruct
## /** * @brief 单目初始化重要的环节，先获得rt再通过三角化恢复3d点坐标 * @param vKeys1 第一帧的关键点 * @param vKeys2 第二帧的关键点 * @param vMatches12 匹配关系，长度与vKeys1一样，对应位置存放vKeys2中关键点的下标 * @param T21 顾名思义 * @param vP3D 恢复出的三维点 * @param vbTriangulated 是否三角化成功，用于统计匹配点数量 */ bool TwoViewReconstruction::Reconstruct( const std::vector<cv::KeyPoint> &vKeys1, const std::vector<cv::KeyPoint> &vKeys2, const vector<int> &vMatches12, Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated) { // 1. 准备工作，提取匹配关系及准备RANSAC mvKeys1.clear(); mvKeys2.clear(); mvKeys1 = vKeys1; mvKeys2 = vKeys2;// Reference Frame: 1, Current Frame: 2 // 填写mvMatches12，里面存放的是vKeys1，vKeys2匹配点的索引 // 存放匹配点的id mvMatches12.clear(); mvMatches12.reserve(mvKeys2.size()); // 长度与mvKeys1，表示对应位置的mvKeys1中的点是否有匹配关系 mvbMatched1.resize(mvKeys1.size()); for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) { if (vMatches12[i] >= 0) { mvMatches12.push_back(make_pair(i, vMatches12[i]));//i表示在参考帧中特征点的索引，vMatches12[i]表示当前帧特征点的索引 mvbMatched1[i] = true; } else mvbMatched1[i] = false; } const int N = mvMatches12.size(); // Indices for minimum set selection vector<size_t> vAllIndices; vAllIndices.reserve(N); vector<size_t> vAvailableIndices; // 使用vAllIndices为了保证8个点选不到同一个点 for (int i = 0; i < N; i++) { vAllIndices.push_back(i); } // Generate sets of 8 points for each RANSAC iteration // 默认200次 mvSets = vector<vector<size_t>>(mMaxIterations, vector<size_t>(8, 0)); DUtils::Random::SeedRandOnce(0); // 2. 先遍历把200次先取好 for (int it = 0; it < mMaxIterations; it++) { vAvailableIndices = vAllIndices; // Select a minimum set for (size_t j = 0; j < 8; j++) { int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1); int idx = vAvailableIndices[randi]; // 这句不多余，防止重复选择 mvSets[it][j] = idx; // 保证选不到同一个点，这么做的话可以删去vAvailableIndices已选点的索引 vAvailableIndices[randi] = vAvailableIndices.back(); vAvailableIndices.pop_back(); } } // Launch threads to compute in parallel a fundamental matrix and a homography vector<bool> vbMatchesInliersH, vbMatchesInliersF; float SH, SF; Eigen::Matrix3f H, F; // 3. 双线程分别计算 // 加ref为了提供引用 thread threadH(&TwoViewReconstruction::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));//TODO-2-3-2-1 thread threadF(&TwoViewReconstruction::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));//TODO-2-3-2-2 // Wait until both threads have finished threadH.join(); threadF.join(); // Compute ratio of scores if (SH + SF == 0.f) return false; float RH = SH / (SH + SF); float minParallax = 1.0; // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) // 4. 看哪个分高用哪个，分别是通过H重建与通过F重建 // ORBSLAM2这里的值是0.4， TOSEE if (RH > 0.50) // if(RH>0.40) { // cout << "Initialization from Homography" << endl; return ReconstructH(vbMatchesInliersH, H, mK, T21, vP3D, vbTriangulated, minParallax, 50);//TODO-2-3-2-3 } else // if(pF_HF>0.6) { // cout << "Initialization from Fundamental" << endl; return ReconstructF(vbMatchesInliersF, F, mK, T21, vP3D, vbTriangulated, minParallax, 50);//TODO-2-3-2-4 } }
thread threadH(&TwoViewReconstruction::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));//计算H矩阵TODO-2-3-2-1
一对点提供两个约束等式，单应矩阵H总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供8个约束方程就可以求解。
## /** * @brief 计算H矩阵，同时计算得分与内点 * @param vbMatchesInliers 经过H矩阵验证，是否为内点，大小为mvMatches12 * @param score 得分 * @param H21 1到2的H矩阵 */ void TwoViewReconstruction::FindHomography(vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &H21) { // Number of putative matches // 匹配成功的个数 const int N = mvMatches12.size(); // Normalize coordinates vector<cv::Point2f> vPn1, vPn2; Eigen::Matrix3f T1, T2; // 像素坐标标准化，先去均值，再除均长 Normalize(mvKeys1, vPn1, T1); Normalize(mvKeys2, vPn2, T2); Eigen::Matrix3f T2inv = T2.inverse(); // Best Results variables score = 0.0; vbMatchesInliers = vector<bool>(N, false); // Iteration variables vector<cv::Point2f> vPn1i(8); vector<cv::Point2f> vPn2i(8); Eigen::Matrix3f H21i, H12i; vector<bool> vbCurrentInliers(N, false); float currentScore; // Perform all RANSAC iterations and save the solution with highest score // 计算归一化后的H矩阵 p2 = H21*p1 for (int it = 0; it < mMaxIterations; it++) { // Select a minimum set for (size_t j = 0; j < 8; j++) { int idx = mvSets[it][j]; vPn1i[j] = vPn1[mvMatches12[idx].first]; vPn2i[j] = vPn2[mvMatches12[idx].second]; } // 计算标准化后的H矩阵 Eigen::Matrix3f Hn = ComputeH21(vPn1i, vPn2i);//TODO-2-3-2-1-1 // 恢复正常H H21i = T2inv * Hn * T1; H12i = H21i.inverse(); currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); // mSigma默认为1 TODO-2-3-2-1-2 if (currentScore > score) { H21 = H21i; vbMatchesInliers = vbCurrentInliers; score = currentScore; } } }
Eigen::Matrix3f Hn = ComputeH21(vPn1i, vPn2i);//计算H矩阵TODO-2-3-2-1-1
## /** * @brief 从特征点匹配求homography（normalized DLT） * |x'| | h1 h2 h3 ||x| * |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子 * |1 | | h7 h8 h9 ||1| * 使用DLT(direct linear tranform)求解该模型 * x' = a H x * ---> (x') 叉乘 (H x) = 0 * ---> Ah = 0 * A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 | * |-x -y -1 0 0 0 xx' yx' x'| * 通过SVD求解Ah = 0，A'A最小特征值对应的特征向量即为解 * @param vP1 归一化后的点, in reference frame * @param vP2 归一化后的点, in current frame * @return 单应矩阵 * @see Multiple View Geometry in Computer Vision - Algorithm 4.2 p109 */ Eigen::Matrix3f TwoViewReconstruction::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) { const int N = vP1.size(); Eigen::MatrixXf A(2 * N, 9); for (int i = 0; i < N; i++) { const float u1 = vP1[i].x; const float v1 = vP1[i].y; const float u2 = vP2[i].x; const float v2 = vP2[i].y; A(2 * i, 0) = 0.0; A(2 * i, 1) = 0.0; A(2 * i, 2) = 0.0; A(2 * i, 3) = -u1; A(2 * i, 4) = -v1; A(2 * i, 5) = -1; A(2 * i, 6) = v2 * u1; A(2 * i, 7) = v2 * v1; A(2 * i, 8) = v2; A(2 * i + 1, 0) = u1; A(2 * i + 1, 1) = v1; A(2 * i + 1, 2) = 1; A(2 * i + 1, 3) = 0.0; A(2 * i + 1, 4) = 0.0; A(2 * i + 1, 5) = 0.0; A(2 * i + 1, 6) = -u2 * u1; A(2 * i + 1, 7) = -u2 * v1; A(2 * i + 1, 8) = -u2; } Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullV); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> H(svd.matrixV().col(8).data()); return H; }
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); // mSigma默认为1 ，校验计算的H矩阵，并计算得分 TODO-2-3-2-1-2
算法中的卡方检测剔除外点策略
## /** * @brief 检查结果 * @param H21 顾名思义 * @param H12 顾名思义 * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 * @param sigma 默认为1 */ float TwoViewReconstruction::CheckHomography(const Eigen::Matrix3f &H21, const Eigen::Matrix3f &H12, vector<bool> &vbMatchesInliers, float sigma) { const int N = mvMatches12.size(); const float h11 = H21(0, 0); const float h12 = H21(0, 1); const float h13 = H21(0, 2); const float h21 = H21(1, 0); const float h22 = H21(1, 1); const float h23 = H21(1, 2); const float h31 = H21(2, 0); const float h32 = H21(2, 1); const float h33 = H21(2, 2); const float h11inv = H12(0, 0); const float h12inv = H12(0, 1); const float h13inv = H12(0, 2); const float h21inv = H12(1, 0); const float h22inv = H12(1, 1); const float h23inv = H12(1, 2); const float h31inv = H12(2, 0); const float h32inv = H12(2, 1); const float h33inv = H12(2, 2); vbMatchesInliers.resize(N); float score = 0; // 基于卡方检验计算出的阈值 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 const float th = 5.991; const float invSigmaSquare = 1.0 / (sigma * sigma); for (int i = 0; i < N; i++) { bool bIn = true; const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; const float u1 = kp1.pt.x; const float v1 = kp1.pt.y; const float u2 = kp2.pt.x; const float v2 = kp2.pt.y; // Reprojection error in first image // x2in1 = H12*x2 // 计算投影误差，2投1 1投2这么做，计算累计的卡方检验分数，分数越高证明内点与误差越优，这么做为了平衡误差与内点个数，不是说内点个数越高越好，也不是说误差越小越好 const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv); const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv; const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv; const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1); const float chiSquare1 = squareDist1 * invSigmaSquare; if (chiSquare1 > th) bIn = false; else score += th - chiSquare1; // Reprojection error in second image // x1in2 = H21*x1 const float w1in2inv = 1.0 / (h31 * u1 + h32 * v1 + h33); const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv; const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv; const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2); const float chiSquare2 = squareDist2 * invSigmaSquare; if (chiSquare2 > th) bIn = false; else score += th - chiSquare2; if (bIn) vbMatchesInliers[i] = true; else vbMatchesInliers[i] = false; } return score; }
thread threadF(&TwoViewReconstruction::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));//计算F矩阵，TODO-2-3-2-2
## /** * @brief 计算F矩阵，同时计算得分与内点 * @param vbMatchesInliers 经过F矩阵验证，是否为内点，大小为mvMatches12 * @param score 得分 * @param F21 1到2的F矩阵 */ void TwoViewReconstruction::FindFundamental(vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &F21) { // Number of putative matches const int N = vbMatchesInliers.size(); // Normalize coordinates vector<cv::Point2f> vPn1, vPn2; Eigen::Matrix3f T1, T2; Normalize(mvKeys1, vPn1, T1); Normalize(mvKeys2, vPn2, T2);//TODO-2-3-2-2-1 Eigen::Matrix3f T2t = T2.transpose(); // Best Results variables score = 0.0; vbMatchesInliers = vector<bool>(N, false); // Iteration variables vector<cv::Point2f> vPn1i(8); vector<cv::Point2f> vPn2i(8); Eigen::Matrix3f F21i; vector<bool> vbCurrentInliers(N, false); float currentScore; // Perform all RANSAC iterations and save the solution with highest score for (int it = 0; it < mMaxIterations; it++) { // Select a minimum set for (int j = 0; j < 8; j++) { int idx = mvSets[it][j]; vPn1i[j] = vPn1[mvMatches12[idx].first]; vPn2i[j] = vPn2[mvMatches12[idx].second]; } Eigen::Matrix3f Fn = ComputeF21(vPn1i, vPn2i);//TODO-2-3-2-2-2 // FN得到的是基于归一化坐标的F矩阵，必须乘上归一化过程矩阵才是最后的基于像素坐标的F F21i = T2t * Fn * T1; currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);//TODO-2-3-2-2-3 if (currentScore > score) { F21 = F21i; vbMatchesInliers = vbCurrentInliers; score = currentScore; } } }
Normalize(mvKeys2, vPn2, T2);//TODO-2-3-2-2-1
## /** * @brief 像素坐标标准化，计算点集的横纵均值，与均值偏差的均值。最后返回的是变化矩阵T 直接乘以像素坐标的齐次向量即可获得去中心去均值后的特征点坐标 * @param vKeys 特征点 * @param vNormalizedPoints 去中心去均值后的特征点坐标 * @param T 变化矩阵 */ void TwoViewReconstruction::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, Eigen::Matrix3f &T) { float meanX = 0; float meanY = 0; const int N = vKeys.size(); vNormalizedPoints.resize(N); for (int i = 0; i < N; i++) { meanX += vKeys[i].pt.x; meanY += vKeys[i].pt.y; } // 1. 求均值 meanX = meanX / N; meanY = meanY / N; float meanDevX = 0; float meanDevY = 0; for (int i = 0; i < N; i++) { vNormalizedPoints[i].x = vKeys[i].pt.x - meanX; vNormalizedPoints[i].y = vKeys[i].pt.y - meanY; meanDevX += fabs(vNormalizedPoints[i].x); meanDevY += fabs(vNormalizedPoints[i].y); } // 2. 确定新原点后计算与新原点的距离均值 meanDevX = meanDevX / N; meanDevY = meanDevY / N; // 3. 去均值化 float sX = 1.0 / meanDevX; float sY = 1.0 / meanDevY; for (int i = 0; i < N; i++) { vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX; vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY; } // 4. 计算变化矩阵 T.setZero(); T(0, 0) = sX; T(1, 1) = sY; T(0, 2) = -meanX * sX; T(1, 2) = -meanY * sY; T(2, 2) = 1.f; }
计算F矩阵的原理
//TODO-2-3-2-2-2
## /** * @brief 从特征点匹配求fundamental matrix（normalized 8点法） * x'Fx = 0 整理可得：Af = 0 * A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 | * 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解 * @param vP1 归一化后的点, in reference frame * @param vP2 归一化后的点, in current frame * @return 基础矩阵 * @see Multiple View Geometry in Computer Vision - Algorithm 11.1 p282 (中文版 p191) */ Eigen::Matrix3f TwoViewReconstruction::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) { const int N = vP1.size(); Eigen::MatrixXf A(N, 9); for (int i = 0; i < N; i++) { const float u1 = vP1[i].x; const float v1 = vP1[i].y; const float u2 = vP2[i].x; const float v2 = vP2[i].y; A(i, 0) = u2 * u1; A(i, 1) = u2 * v1; A(i, 2) = u2; A(i, 3) = v2 * u1; A(i, 4) = v2 * v1; A(i, 5) = v2; A(i, 6) = u1; A(i, 7) = v1; A(i, 8) = 1; } Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Fpre(svd.matrixV().col(8).data()); Eigen::JacobiSVD<Eigen::Matrix3f> svd2(Fpre, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Vector3f w = svd2.singularValues(); // 这里注意计算完要强制让第三个奇异值为0 w(2) = 0; return svd2.matrixU() * Eigen::DiagonalMatrix<float, 3>(w) * svd2.matrixV().transpose(); }
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);//TODO-2-3-2-2-3
## /** * @brief 检查结果 * @param F21 顾名思义 * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 * @param sigma 默认为1 */ float TwoViewReconstruction::CheckFundamental(const Eigen::Matrix3f &F21, vector<bool> &vbMatchesInliers, float sigma) { const int N = mvMatches12.size(); const float f11 = F21(0, 0); const float f12 = F21(0, 1); const float f13 = F21(0, 2); const float f21 = F21(1, 0); const float f22 = F21(1, 1); const float f23 = F21(1, 2); const float f31 = F21(2, 0); const float f32 = F21(2, 1); const float f33 = F21(2, 2); vbMatchesInliers.resize(N); float score = 0; // 基于卡方检验计算出的阈值 自由度为1的卡方分布，显著性水平为0.05，对应的临界阈值 const float th = 3.841; // 基于卡方检验计算出的阈值 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值 const float thScore = 5.991; const float invSigmaSquare = 1.0 / (sigma * sigma); for (int i = 0; i < N; i++) { bool bIn = true; const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first]; const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second]; const float u1 = kp1.pt.x; const float v1 = kp1.pt.y; const float u2 = kp2.pt.x; const float v2 = kp2.pt.y; // Reprojection error in second image // l2=F21x1=(a2,b2,c2) // 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2) const float a2 = f11 * u1 + f12 * v1 + f13; const float b2 = f21 * u1 + f22 * v1 + f23; const float c2 = f31 * u1 + f32 * v1 + f33; // 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b) const float num2 = a2 * u2 + b2 * v2 + c2; const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2); const float chiSquare1 = squareDist1 * invSigmaSquare; // 自由度为1是因为这里的计算是点到线的距离，判定分数自由度为2的原因可能是为了与H矩阵持平 if (chiSquare1 > th) bIn = false; else score += thScore - chiSquare1; // Reprojection error in second image // l1 =x2tF21=(a1,b1,c1) // 与上面相同只不过反过来了 const float a1 = f11 * u2 + f21 * v2 + f31; const float b1 = f12 * u2 + f22 * v2 + f32; const float c1 = f13 * u2 + f23 * v2 + f33; const float num1 = a1 * u1 + b1 * v1 + c1; const float squareDist2 = num1 * num1 / (a1 * a1 + b1 * b1); const float chiSquare2 = squareDist2 * invSigmaSquare; if (chiSquare2 > th) bIn = false; else score += thScore - chiSquare2; if (bIn) vbMatchesInliers[i] = true; else vbMatchesInliers[i] = false; } return score; }
ReconstructH(vbMatchesInliersH, H, mK, T21, vP3D, vbTriangulated, minParallax, 50);//使用H矩阵来恢复3D点的位置，TODO-2-3-2-3
## /** * @brief 从H恢复R t * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition * 参考文献：Motion and structure from motion in a piecewise plannar environment * 这篇参考文献和下面的代码使用了Faugeras SVD-based decomposition算法 * @see * - Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988. * - Deeper understanding of the homography decomposition for vision-based control * 设平面法向量 n = (a, b, c)^t 有一点(x, y, z)在平面上，则ax + by + cz = d 即 1/d * n^t * x = 1 其中d表示 * x' = R*x + t 从下面开始x 与 x'都表示归一化坐标 * λ2*x' = R*(λ1*x) + t = R*(λ1*x) + t * 1/d * n^t * (λ1*x) * x' = λ*(R + t * 1/d * n^t) * x = H^ * x * 对应图像坐标 u' = G * u G = KH^K.inv * H^ ~= d*R + t * n^t = U∧V^t ∧ = U^t * H^ * V = d*U^t * R * V + (U^t * t) * (V^t * n)^t * s = det(U) * det(V) s 有可能是 1 或 -1 ∧ = s^2 * d*U^t * R * V + (U^t * t) * (V^t * n)^t = (s*d) * s * U^t * R * V + (U^t * t) * (V^t * n)^t * 令 R' = s * U^t * R * V t' = U^t * t n' = V^t * n d' = s * d * ∧ = d' * R' + t' * n'^t 所以∧也可以认为是一个单应矩阵，其中加入s只为说明有符号相反的可能 * 设∧ = | d1, 0, 0 | 取e1 = (1, 0, 0)^t e2 = (0, 1, 0)^t e3 = (0, 0, 1)^t * | 0, d2, 0 | * | 0, 0, d3 | * n' = (a1, 0, 0)^t + (0, b1, 0)^t + (0, 0, c1)^t = a1*e1 + b1*e2 + c1*e3 * * ∧ = [d1*e1, d2*e2, d3*e3] = [d' * R' * e1, d' * R' * e2, d' * R' * e3] + [t' * a1, t' * b1, t' * c1] * ==> d1*e1 = d' * R' * e1 + t' * a1 * d2*e2 = d' * R' * e2 + t' * b1 * d3*e3 = d' * R' * e3 + t' * c1 * * * 上面式子每两个消去t可得 * d'R'(b1e1 - a1e2) = d1b1e1 - d2a1e2 * d'R'(c1e2 - b1e3) = d2c1e1 - d3b1e3 同时取二范数，因为旋转对二范数没影响，所以可以约去 * d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1 * * (d'^2 - d2^2)*a1^2 + (d'^2 - d1^2)*b1^2 = 0 * (d'^2 - d3^2)*b1^2 + (d'^2 - d2^2)*c1^2 = 0 令 d'^2 - d1^2 = x1 d'^2 - d2^2 = x2 d'^2 - d3^2 = x3 * (d'^2 - d1^2)*c1^2 + (d'^2 - d3^2)*a1^2 = 0 * * * | x2 x1 0 | | a1^2 | * | 0 x3 x2 | * | b1^2 | = 0 ===> x1 * x2 * x3 = 0 有(d'^2 - d1^2) * (d'^2 - d2^2) * (d'^2 - d3^2) = 0 * | x3 0 x1 | | c1^2 | * 由于d1 >= d2 >= d3 所以d' = d2 or -d2 * * ----------------------------------------------------------------------------------------------------------------------------------------------------------------- * 下面分情况讨论，当d' > 0 再根据a1^2 + b1^2 + c1^2 = 1 ??????哪来的根据，不晓得 * 能够求得a1 = ε1 * sqrt((d1^2 - d2^2) / (d1^2 - d3^2)) * b1 = 0 * c1 = ε2 * sqrt((d2^2 - d3^2) / (d1^2 - d3^2)) 其中ε1 ε2 可以为正负1 * 结果带入 d2*e2 = d' * R' * e2 + t' * b1 => R' * e2 = e2 * | cosθ 0 -sinθ | * ==> R' = | 0 1 0 | n' 与 R' 带入 d'R'(a1e3 - c1e1) = d3a1e3 - d1c1e1 * | sinθ 0 cosθ | * | cosθ 0 -sinθ | | -c1 | | -d1c1 | * d' * | 0 1 0 | * | 0 | = | 0 | 能够解出 sinθ 与 cosθ * | sinθ 0 cosθ | | a1 | | d3a1 | * * 到此为止得到了 R' 再根据 d1*e1 = d' * R' * e1 + t' * a1 * d2*e2 = d' * R' * e2 + t' * b1 * d3*e3 = d' * R' * e3 + t' * c1 * * 求得 t' = (d1 - d3) * (a1, 0, c1)^t * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 * @param H21 顾名思义 * @param K 相机内参 * @param T21 旋转平移（要输出的） * @param vP3D 恢复的三维点（要输出的） * @param vbTriangulated 大小与vbMatchesInliers，表示哪个点被重建了 * @param minParallax 1 * @param minTriangulated 50 */ bool TwoViewReconstruction::ReconstructH( vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K, Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) { // 统计了合法的匹配，后面用于对比重建出的点数 int N = 0; for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++) if (vbMatchesInliers[i]) N++; // We recover 8 motion hypotheses using the method of Faugeras et al. // Motion and structure from motion in a piecewise planar environment. // International Journal of Pattern Recognition and Artificial Intelligence, 1988 // step1：SVD分解Homography // 因为特征点是图像坐标系，所以讲H矩阵由相机坐标系换算到图像坐标系 Eigen::Matrix3f invK = K.inverse(); Eigen::Matrix3f A = invK * H21 * K; Eigen::JacobiSVD<Eigen::Matrix3f> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f U = svd.matrixU(); Eigen::Matrix3f V = svd.matrixV(); Eigen::Matrix3f Vt = V.transpose(); Eigen::Vector3f w = svd.singularValues(); float s = U.determinant() * Vt.determinant(); float d1 = w(0); float d2 = w(1); float d3 = w(2); // SVD分解的正常情况是特征值降序排列 if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001) { return false; } vector<Eigen::Matrix3f> vR; vector<Eigen::Vector3f> vt, vn; vR.reserve(8); vt.reserve(8); vn.reserve(8); // n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1 // step2：计算法向量 // 法向量n'= [x1 0 x3] float aux1 = sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)); float aux3 = sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3)); float x1[] = {aux1, aux1, -aux1, -aux1}; float x3[] = {aux3, -aux3, aux3, -aux3}; // case d'=d2 // step3：恢复旋转矩阵 // step3.1：计算 sin(theta)和cos(theta)，case d'=d2 float aux_stheta = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 + d3) * d2); float ctheta = (d2 * d2 + d1 * d3) / ((d1 + d3) * d2); float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; // step3.2：计算四种旋转矩阵R，t // 计算旋转矩阵 R‘，计算ppt中公式18 // | ctheta 0 -aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | |-aux3| // | ctheta 0 aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | | aux3| // | ctheta 0 aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | |-aux3| // | ctheta 0 -aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | | aux3| for (int i = 0; i < 4; i++) { Eigen::Matrix3f Rp; Rp.setZero(); Rp(0, 0) = ctheta; Rp(0, 2) = -stheta[i]; Rp(1, 1) = 1.f; Rp(2, 0) = stheta[i]; Rp(2, 2) = ctheta; Eigen::Matrix3f R = s * U * Rp * Vt; vR.push_back(R); Eigen::Vector3f tp; tp(0) = x1[i]; tp(1) = 0; tp(2) = -x3[i]; tp *= d1 - d3; // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度 // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 Eigen::Vector3f t = U * tp; vt.push_back(t / t.norm()); Eigen::Vector3f np; np(0) = x1[i]; np(1) = 0; np(2) = x3[i]; Eigen::Vector3f n = V * np; if (n(2) < 0) n = -n; vn.push_back(n); } // case d'=-d2 // step3.3：计算 sin(theta)和cos(theta)，case d'=-d2 float aux_sphi = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 - d3) * d2); float cphi = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2); float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; // step3.4：计算四种旋转矩阵R，t // 计算旋转矩阵 R‘ for (int i = 0; i < 4; i++) { Eigen::Matrix3f Rp; Rp.setZero(); Rp(0, 0) = cphi; Rp(0, 2) = sphi[i]; Rp(1, 1) = -1; Rp(2, 0) = sphi[i]; Rp(2, 2) = -cphi; Eigen::Matrix3f R = s * U * Rp * Vt; vR.push_back(R); Eigen::Vector3f tp; tp(0) = x1[i]; tp(1) = 0; tp(2) = x3[i]; tp *= d1 + d3; Eigen::Vector3f t = U * tp; vt.push_back(t / t.norm()); Eigen::Vector3f np; np(0) = x1[i]; np(1) = 0; np(2) = x3[i]; Eigen::Vector3f n = V * np; if (n(2) < 0) n = -n; vn.push_back(n); } int bestGood = 0; int secondBestGood = 0; int bestSolutionIdx = -1; float bestParallax = -1; vector<cv::Point3f> bestP3D; vector<bool> bestTriangulated; // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax) // We reconstruct all hypotheses and check in terms of triangulated points and parallax // step4：d'=d2和d'=-d2分别对应8组(R t)，通过恢复3D点并判断是否在相机正前方的方法来确定最优解 for (size_t i = 0; i < 8; i++) { float parallaxi; vector<cv::Point3f> vP3Di; vector<bool> vbTriangulatedi;
//TODO-2-3-2-3-1 int nGood = CheckRT(vR[i], vt[i], mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3Di, 4.0 * mSigma2, vbTriangulatedi, parallaxi); // 保留最优的和次优的 if (nGood > bestGood) { secondBestGood = bestGood; bestGood = nGood; bestSolutionIdx = i; bestParallax = parallaxi; bestP3D = vP3Di; bestTriangulated = vbTriangulatedi; } else if (nGood > secondBestGood) { secondBestGood = nGood; } } // step5：通过判断最优是否明显好于次优，从而判断该次Homography分解是否成功 if (secondBestGood < 0.75 * bestGood && bestParallax >= minParallax && bestGood > minTriangulated && bestGood > 0.9 * N) { T21 = Sophus::SE3f(vR[bestSolutionIdx], vt[bestSolutionIdx]); vbTriangulated = bestTriangulated; return true; } return false; }
CheckRT(vR[i], vt[i], mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3Di, 4.0 * mSigma2, vbTriangulatedi, parallaxi);//检查位姿的有效性TODO-2-3-2-3-1
## /** * @brief 进行cheirality check，从而进一步找出F分解后最合适的解 * @param R 旋转 * @param t 平移 * @param vKeys1 特征点 * @param vKeys2 特征点 * @param vMatches12 匹配关系 * @param vbMatchesInliers 匹配关系是否有效 * @param K 内参 * @param vP3D 三维点 * @param th2 误差半径 * @param vbGood 大小与mvKeys1一致，表示哪个点被重建了 * @param parallax */ int TwoViewReconstruction::CheckRT( const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers, const Eigen::Matrix3f &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax) { // Calibration parameters const float fx = K(0, 0); const float fy = K(1, 1); const float cx = K(0, 2); const float cy = K(1, 2); vbGood = vector<bool>(vKeys1.size(), false); vP3D.resize(vKeys1.size()); vector<float> vCosParallax; vCosParallax.reserve(vKeys1.size()); // Camera 1 Projection Matrix K[I|0] // 步骤1：得到一个相机的投影矩阵 // 以第一个相机的光心作为世界坐标系 Eigen::Matrix<float, 3, 4> P1; P1.setZero(); P1.block<3, 3>(0, 0) = K; Eigen::Vector3f O1; O1.setZero(); // Camera 2 Projection Matrix K[R|t] // 步骤2：得到第二个相机的投影矩阵 Eigen::Matrix<float, 3, 4> P2; P2.block<3, 3>(0, 0) = R; P2.block<3, 1>(0, 3) = t; P2 = K * P2; // 第二个相机的光心在世界坐标系下的坐标 Eigen::Vector3f O2 = -R.transpose() * t; int nGood = 0; for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) { if (!vbMatchesInliers[i]) continue; // kp1和kp2是匹配特征点 const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first]; const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second]; Eigen::Vector3f p3dC1; Eigen::Vector3f x_p1(kp1.pt.x, kp1.pt.y, 1); Eigen::Vector3f x_p2(kp2.pt.x, kp2.pt.y, 1); // 步骤3：利用三角法恢复三维点p3dC1，TODO-2-3-2-3-1-1 GeometricTools::Triangulate(x_p1, x_p2, P1, P2, p3dC1); if (!isfinite(p3dC1(0)) || !isfinite(p3dC1(1)) || !isfinite(p3dC1(2))) { vbGood[vMatches12[i].first] = false; continue; } // Check parallax // 步骤4：计算视差角余弦值 Eigen::Vector3f normal1 = p3dC1 - O1; float dist1 = normal1.norm(); Eigen::Vector3f normal2 = p3dC1 - O2; float dist2 = normal2.norm(); float cosParallax = normal1.dot(normal2) / (dist1 * dist2); // 步骤5：判断3D点是否在两个摄像头前方 // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth) // 步骤5.1：3D点深度为负，在第一个摄像头后方，淘汰 if (p3dC1(2) <= 0 && cosParallax < 0.99998) continue; // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth) // 步骤5.2：3D点深度为负，在第二个摄像头后方，淘汰 Eigen::Vector3f p3dC2 = R * p3dC1 + t; if (p3dC2(2) <= 0 && cosParallax < 0.99998) continue; // 步骤6：计算重投影误差 // Check reprojection error in first image // 计算3D点在第一个图像上的投影误差 float im1x, im1y; float invZ1 = 1.0 / p3dC1(2); im1x = fx * p3dC1(0) * invZ1 + cx; im1y = fy * p3dC1(1) * invZ1 + cy; float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y); // 步骤6.1：重投影误差太大，跳过淘汰 // 一般视差角比较小时重投影误差比较大 if (squareError1 > th2) continue; // Check reprojection error in second image // 计算3D点在第二个图像上的投影误差 float im2x, im2y; float invZ2 = 1.0 / p3dC2(2); im2x = fx * p3dC2(0) * invZ2 + cx; im2y = fy * p3dC2(1) * invZ2 + cy; float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y); // 步骤6.1：重投影误差太大，跳过淘汰 // 一般视差角比较小时重投影误差比较大 if (squareError2 > th2) continue; // 步骤7：统计经过检验的3D点个数，记录3D点视差角 vCosParallax.push_back(cosParallax); vP3D[vMatches12[i].first] = cv::Point3f(p3dC1(0), p3dC1(1), p3dC1(2)); nGood++; if (cosParallax < 0.99998) vbGood[vMatches12[i].first] = true; } // 7 得到3D点中较小的视差角，并且转换成为角度制表示 if (nGood > 0) { // 从小到大排序，注意vCosParallax值越大，视差越小 sort(vCosParallax.begin(), vCosParallax.end()); // !排序后并没有取最小的视差角，而是取一个较小的视差角 // 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大) // 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 size_t idx = min(50, int(vCosParallax.size() - 1)); // 将这个选中的角弧度制转换为角度制 parallax = acos(vCosParallax[idx]) * 180 / CV_PI; } else parallax = 0; return nGood; }
GeometricTools::Triangulate(x_p1, x_p2, P1, P2, p3dC1);//利用三角法恢复三维点p3dC1，TODO-2-3-2-3-1-1
/** * @brief 三角化获得三维点 * @param x_c1 点在关键帧1下的归一化坐标 * @param x_c2 点在关键帧2下的归一化坐标 * @param Tc1w 关键帧1投影矩阵 [K*R | K*t] * @param Tc2w 关键帧2投影矩阵 [K*R | K*t] * @param x3D 三维点坐标，作为结果输出 */ bool GeometricTools::Triangulate( Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, Eigen::Matrix<float,3,4> &Tc1w, Eigen::Matrix<float,3,4> &Tc2w, Eigen::Vector3f &x3D) { Eigen::Matrix4f A; // x = a*P*X， 左右两面乘Pc的反对称矩阵 a*[x]^ * P *X = 0 构成了A矩阵，中间涉及一个尺度a，因为都是归一化平面，但右面是0所以直接可以约掉不影响最后的尺度 // 0 -1 v P(0) -P.row(1) + v*P.row(2) // 1 0 -u * P(1) = P.row(0) - u*P.row(2) // -v u 0 P(2) u*P.row(1) - v*P.row(0) // 发现上述矩阵线性相关，所以取前两维，两个点构成了4行的矩阵，就是如下的操作，求出的是4维的结果[X,Y,Z,A]，所以需要除以最后一维使之为1，就成了[X,Y,Z,1]这种齐次形式 A.block<1,4>(0,0) = x_c1(0) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(0,0); A.block<1,4>(1,0) = x_c1(1) * Tc1w.block<1,4>(2,0) - Tc1w.block<1,4>(1,0); A.block<1,4>(2,0) = x_c2(0) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(0,0); A.block<1,4>(3,0) = x_c2(1) * Tc2w.block<1,4>(2,0) - Tc2w.block<1,4>(1,0); // 解方程 AX=0 Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV); Eigen::Vector4f x3Dh = svd.matrixV().col(3); if(x3Dh(3)==0) return false; // Euclidean coordinates x3D = x3Dh.head(3)/x3Dh(3); return true; }
ReconstructF(vbMatchesInliersF, F, mK, T21, vP3D, vbTriangulated, minParallax, 50);//TODO-2-3-2-4
## /** * @brief 通过基础矩阵重建 * @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 * @param F21 顾名思义 * @param K 相机内参 * @param T21 旋转平移（要输出的） * @param vP3D 恢复的三维点（要输出的） * @param vbTriangulated 大小与mvKeys1一致，表示哪个点被重建了 * @param minParallax 1 * @param minTriangulated 50 */ bool TwoViewReconstruction::ReconstructF( vector<bool> &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K, Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) { // 统计了合法的匹配，后面用于对比重建出的点数 int N = 0; for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++) if (vbMatchesInliers[i]) N++; // Compute Essential Matrix from Fundamental Matrix // 1. 计算本质矩阵 Eigen::Matrix3f E21 = K.transpose() * F21 * K; Eigen::Matrix3f R1, R2; Eigen::Vector3f t; // Recover the 4 motion hypotheses // 2. 分解本质矩阵，得到4对rt,TODO-2-3-2-4-1 DecomposeE(E21, R1, R2, t); Eigen::Vector3f t1 = t; Eigen::Vector3f t2 = -t; // Reconstruct with the 4 hyphoteses and check // 3. 使用四对结果分别重建 vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4; vector<bool> vbTriangulated1, vbTriangulated2, vbTriangulated3, vbTriangulated4; float parallax1, parallax2, parallax3, parallax4; int nGood1 = CheckRT(R1, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D1, 4.0 * mSigma2, vbTriangulated1, parallax1); int nGood2 = CheckRT(R2, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D2, 4.0 * mSigma2, vbTriangulated2, parallax2); int nGood3 = CheckRT(R1, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D3, 4.0 * mSigma2, vbTriangulated3, parallax3); int nGood4 = CheckRT(R2, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D4, 4.0 * mSigma2, vbTriangulated4, parallax4); // 统计重建出点的数量最大值 int maxGood = max(nGood1, max(nGood2, max(nGood3, nGood4))); // 起码要重建出超过百分之90的匹配点 int nMinGood = max(static_cast<int>(0.9 * N), minTriangulated); // 4. 看看有没有脱颖而出的结果，且最大值要高于要求的最低值，如果大家都差不多说明有问题，因为4个结果中只有一个会正常 // 大家都很多的话反而不正常了。。。 int nsimilar = 0; if (nGood1 > 0.7 * maxGood) nsimilar++; if (nGood2 > 0.7 * maxGood) nsimilar++; if (nGood3 > 0.7 * maxGood) nsimilar++; if (nGood4 > 0.7 * maxGood) nsimilar++; // If there is not a clear winner or not enough triangulated points reject initialization if (maxGood < nMinGood || nsimilar > 1) { return false; } // If best reconstruction has enough parallax initialize // 5. 使用最好的结果输出 if (maxGood == nGood1) { if (parallax1 > minParallax) { vP3D = vP3D1; vbTriangulated = vbTriangulated1; T21 = Sophus::SE3f(R1, t1); return true; } } else if (maxGood == nGood2) { if (parallax2 > minParallax) { vP3D = vP3D2; vbTriangulated = vbTriangulated2; T21 = Sophus::SE3f(R2, t1); return true; } } else if (maxGood == nGood3) { if (parallax3 > minParallax) { vP3D = vP3D3; vbTriangulated = vbTriangulated3; T21 = Sophus::SE3f(R1, t2); return true; } } else if (maxGood == nGood4) { if (parallax4 > minParallax) { vP3D = vP3D4; vbTriangulated = vbTriangulated4; T21 = Sophus::SE3f(R2, t2); return true; } } return false; }
DecomposeE(E21, R1, R2, t); // 分解本质矩阵，得到4对rt,TODO-2-3-2-4-1
/** * @brief 分解Essential矩阵 * 解释的比较好的博客：https://blog.csdn.net/weixin_44580210/article/details/90344511 * F矩阵通过结合内参可以得到Essential矩阵，分解E矩阵将得到4组解 * 这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t] * ## 反对称矩阵性质 * 多视图几何上定义：一个3×3的矩阵是本质矩阵的充要条件是它的奇异值中有两个相等而第三个是0，为什么呢？ * 首先我们知道 E=[t]×R=SR其中S为反对称矩阵，反对称矩阵有什么性质呢？ * 结论1：如果 S 是实的反对称矩阵，那么S=UBU^T，其中 B 为形如diag(a1Z，a2Z...amZ，0，0...0)的分块对角阵，其中 Z = [0, 1; -1, 0] * 反对称矩阵的特征矢量都是纯虚数并且奇数阶的反对称矩阵必是奇异的 * 那么根据这个结论我们可以将 S 矩阵写成 S=kUZU^⊤，而 Z 为 * | 0, 1, 0 | * |-1, 0, 0 | * | 0, 0, 0 | * Z = diag(1, 1, 0) * W W 为 * | 0,-1, 0 | * | 1, 0, 0 | * | 0, 0, 1 | * E=SR=Udiag(1,1,0)(WU^⊤R) 这样就证明了 E 拥有两个相等的奇异值 * * ## 恢复相机矩阵 * 假定第一个摄像机矩阵是P=[I∣0]，为了计算第二个摄像机矩阵P′，必须把 E 矩阵分解为反对成举着和旋转矩阵的乘积 SR。 * 还是根据上面的结论1，我们在相差一个常数因子的前提下有 S=UZU^T，我们假设旋转矩阵分解为UXV^T，注意这里是假设旋转矩阵分解形式为UXV^T，并不是旋转矩阵的svd分解， * 其中 UV都是E矩阵分解出的 * Udiag(1,1,0)V^T = E = SR = (UZU^T)(UXV^⊤) = U(ZX)V^T * 则有 ZX = diag(1,1,0)，因此 x=W或者 X=W^T * 结论：如果 E 的SVD分解为 Udiag(1,1,0)V^⊤，E = SR有两种分解形式，分别是： S = UZU^⊤ R = UWVTor UW^TV^⊤ * 接着分析，又因为St=0（自己和自己叉乘肯定为0嘛）以及∥t∥=1（对两个摄像机矩阵的基线的一种常用归一化），因此 t = U(0,0,1)^T = u3， * 即矩阵 U 的最后一列，这样的好处是不用再去求S了，应为t的符号不确定，R矩阵有两种可能，因此其分解有如下四种情况： * P′=[UWV^T ∣ +u3] or [UWV^T ∣ −u3] or [UW^TV^T ∣ +u3] or [UW^TV^T ∣ −u3] * @param E Essential Matrix * @param R1 Rotation Matrix 1 * @param R2 Rotation Matrix 2 * @param t Translation * @see Multiple View Geometry in Computer Vision - Result 9.19 p259 */ void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E, Eigen::Matrix3f &R1, Eigen::Matrix3f &R2, Eigen::Vector3f &t) { Eigen::JacobiSVD<Eigen::Matrix3f> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); // 对 t 有归一化，但是这个地方并没有决定单目整个SLAM过程的尺度 // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 Eigen::Matrix3f U = svd.matrixU(); Eigen::Matrix3f Vt = svd.matrixV().transpose(); t = U.col(2); t = t / t.norm(); Eigen::Matrix3f W; W.setZero(); W(0, 1) = -1; W(1, 0) = 1; W(2, 2) = 1; R1 = U * W * Vt; // 旋转矩阵有行列式为1的约束 if (R1.determinant() < 0) R1 = -R1; R2 = U * W.transpose() * Vt; if (R2.determinant() < 0) R2 = -R2; }
CreateInitialMapMonocular(); // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中,TODO-2-3-3
## /** * @brief 单目相机成功初始化后用三角化得到的点生成MapPoints * */ void Tracking::CreateInitialMapMonocular() { // Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧 KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); if(mSensor == System::IMU_MONOCULAR) pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL); // Step 1 将初始关键帧,当前关键帧的描述子转为BoW pKFini->ComputeBoW(); pKFcur->ComputeBoW(); // Insert KFs in the map // Step 2 将关键帧插入到地图 mpAtlas->AddKeyFrame(pKFini); mpAtlas->AddKeyFrame(pKFcur); // Step 3 用初始化得到的3D点来生成地图点MapPoints // mvIniMatches[i] 表示初始化两帧特征点匹配关系。 // 具体解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值,没有匹配关系的话，vMatches12[i]值为 -1 for(size_t i=0; i<mvIniMatches.size();i++) { // 没有匹配，跳过 if(mvIniMatches[i]<0) continue; //Create MapPoint. // 用三角化点初始化为空间点的世界坐标 Eigen::Vector3f worldPos; worldPos << mvIniP3D[i].x, mvIniP3D[i].y, mvIniP3D[i].z; // Step 3.1 用3D点构造地图点 MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap()); // Step 3.2 为该MapPoint添加属性： // a.观测到该MapPoint的关键帧 // b.该MapPoint的描述子 // c.该MapPoint的平均观测方向和深度范围 // 表示该KeyFrame的2D特征点和对应的3D地图点 pKFini->AddMapPoint(pMP,i); pKFcur->AddMapPoint(pMP,mvIniMatches[i]); // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到 pMP->AddObservation(pKFini,i); pMP->AddObservation(pKFcur,mvIniMatches[i]); // b.从众多观测到该MapPoint的特征点中挑选最有代表性的描述子 pMP->ComputeDistinctiveDescriptors(); // c.更新该MapPoint平均观测方向以及观测距离的范围 pMP->UpdateNormalAndDepth(); //Fill Current Frame structure // mvIniMatches下标i表示在初始化参考帧中的特征点的序号 // mvIniMatches[i]是初始化当前帧中的特征点的序号 mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP; mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false; //Add to Map mpAtlas->AddMapPoint(pMP); } // Update Connections // Step 3.3 更新关键帧间的连接关系 // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数 pKFini->UpdateConnections(); pKFcur->UpdateConnections(); std::set<MapPoint*> sMPs; sMPs = pKFini->GetMapPoints(); // Bundle Adjustment // Step 4 全局BA优化，同时优化所有位姿和三维点 Verbose::PrintMess("New Map created with " + to_string(mpAtlas->MapPointsInMap()) + " points", Verbose::VERBOSITY_QUIET); Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20); // Step 5 取场景的中值深度，用于尺度归一化 // 为什么是 pKFini 而不是 pKCur ? 答：都可以的，内部做了位姿变换了 float medianDepth = pKFini->ComputeSceneMedianDepth(2); float invMedianDepth; if(mSensor == System::IMU_MONOCULAR) invMedianDepth = 4.0f/medianDepth; // 4.0f else invMedianDepth = 1.0f/medianDepth; // 两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于50 if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<50) // TODO Check, originally 100 tracks { Verbose::PrintMess("Wrong initialization, reseting...", Verbose::VERBOSITY_QUIET); mpSystem->ResetActiveMap(); return; } // Step 6 将两帧之间的变换归一化到平均深度1的尺度下 // Scale initial baseline Sophus::SE3f Tc2w = pKFcur->GetPose(); // x/z y/z 将z归一化到1 Tc2w.translation() *= invMedianDepth; pKFcur->SetPose(Tc2w); // Scale points // Step 7 把3D点的尺度也归一化到1 // 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 答：是的，因为是同样的三维点 vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches(); for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++) { if(vpAllMapPoints[iMP]) { MapPoint* pMP = vpAllMapPoints[iMP]; pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); pMP->UpdateNormalAndDepth(); } } if (mSensor == System::IMU_MONOCULAR) { pKFcur->mPrevKF = pKFini; pKFini->mNextKF = pKFcur; pKFcur->mpImuPreintegrated = mpImuPreintegratedFromLastKF; mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKFcur->mpImuPreintegrated->GetUpdatedBias(),pKFcur->mImuCalib); } // Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点 mpLocalMapper->InsertKeyFrame(pKFini); mpLocalMapper->InsertKeyFrame(pKFcur); mpLocalMapper->mFirstTs=pKFcur->mTimeStamp; mCurrentFrame.SetPose(pKFcur->GetPose()); mnLastKeyFrameId=mCurrentFrame.mnId; mpLastKeyFrame = pKFcur; //mnLastRelocFrameId = mInitialFrame.mnId; mvpLocalKeyFrames.push_back(pKFcur); mvpLocalKeyFrames.push_back(pKFini); // 单目初始化之后，得到的初始地图中的所有点都是局部地图点 mvpLocalMapPoints=mpAtlas->GetAllMapPoints(); mpReferenceKF = pKFcur; mCurrentFrame.mpReferenceKF = pKFcur; // Compute here initial velocity vector<KeyFrame*> vKFs = mpAtlas->GetAllKeyFrames(); Sophus::SE3f deltaT = vKFs.back()->GetPose() * vKFs.front()->GetPoseInverse(); mbVelocity = false; Eigen::Vector3f phi = deltaT.so3().log(); double aux = (mCurrentFrame.mTimeStamp-mLastFrame.mTimeStamp)/(mCurrentFrame.mTimeStamp-mInitialFrame.mTimeStamp); phi *= aux; mLastFrame = Frame(mCurrentFrame); mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints); mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose()); mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.push_back(pKFini); // 初始化成功，至此，初始化过程完成 mState=OK; initID = pKFcur->mnId; }
对上面的总结
一、相机模型
1、针孔相机模型
将相机归一化坐标系下的点通过投影关系转换到像素坐标系上，如下
P为相机归一化坐标系上的点；K为内参矩阵。
2、畸变模型
为了获得更好的投影效果，在相机前方加入了透镜，从而对光线的传播产生了新的影响：a、透镜自身的形状对光线传播的影响；b、在机械组装过程中，透镜和成像平面不可能完全平行；
透镜形状引起的畸变称为径向畸变；透镜往往使得真实环境中的一条直线在图片中变成了曲线，越靠近图像的边缘，越明显。畸变主要分为：桶形畸变和枕形畸变；桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。
相机组装过程中不能使透镜和成像平面严格平行称为切向畸变。
考虑归一化平面上的任意点p，其坐标为[x,y]T,r表示点p与坐标系原点之间的距离，径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。
假设畸变呈多项式关系，如下：
[xdistorted,ydistorted]为畸变后点的归一化坐标；
切向畸变的模型：
将相机坐标系下的点投影到图像坐标系下：
查看文件Pinhole.cpp，根据上面原理详细分析里面的函数
/** * @brief 相机坐标系下的三维点投影到无畸变像素平面 * @param p3D 三维点 * @return 像素坐标 */ cv::Point2f Pinhole::project(const cv::Point3f &p3D) { //0表示fx，2表示cx，1表示fy，3表示cy return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2], mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); }
反投影函数
/** * @brief 反投影 * @param p2D 特征点 * @return 归一化坐标 */ cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) { return cv::Point3f( (p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], 1.f); }
对级约束
上面的F矩阵为F21，意思为将平面1中的点p1通过基础矩阵F21投影到平面2上，Fx1为平面2上的直线l2，在l2上的点x2乘以l2，则为零。
/** * @brief 极线约束 * @param pCamera2 右相机 * @param kp1 左相机特征点 * @param kp2 右相机特征点 * @param R12 2->1的旋转 * @param t12 2->1的平移 * @param sigmaLevel 特征点1的尺度的平方 * @param unc 特征点2的尺度的平方，1.2^2n * @return 三维点恢复的成功与否 */ bool Pinhole::epipolarConstrain( GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc) { // Compute Fundamental Matrix Eigen::Matrix3f t12x = Sophus::SO3f::hat(t12); Eigen::Matrix3f K1 = this->toK_(); Eigen::Matrix3f K2 = pCamera2->toK_(); Eigen::Matrix3f F12 = K1.transpose().inverse() * t12x * R12 * K2.inverse(); // Epipolar line in second image l = x1'F12 = [a b c] // u2, // (u1, v1, 1) * F12 * (v2,) = 0 --> (a, b, c) * (u2, v2, 1)^t = 0 --> a*u2 + b*v2 + c = 0 // 1 const float a = kp1.pt.x * F12(0, 0) + kp1.pt.y * F12(1, 0) + F12(2, 0); const float b = kp1.pt.x * F12(0, 1) + kp1.pt.y * F12(1, 1) + F12(2, 1); const float c = kp1.pt.x * F12(0, 2) + kp1.pt.y * F12(1, 2) + F12(2, 2); // 点到直线距离的公式 // d = |a*u2 + b*v2 + c| / sqrt(a^2 + b^2) const float num = a * kp2.pt.x + b * kp2.pt.y + c; const float den = a * a + b * b; if (den == 0) return false; const float dsqr = num * num / den; return dsqr < 3.84 * unc; }
单应矩阵(Homography, H)，它描述了两个平面之间的映射关系。
若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性进行运动估计。单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。设图像I1和I2有一对匹配好的特征点p1和p2，其对应的3D点为P，满足平面方程：
由点法式得：
根据矩阵变换之间的关系，有如下
于是，得到了一个直接描述图像坐标p1和p2之间的变换，把中间这部分记为H：
类似F矩阵的方法，可以先根据匹配点计算出H，然后将它分解为以计算旋转和平移。上式展开得，
由于上面等式不是普通等式，H可以乘以任意非零常数，在实际处理中令h9=1，于是有
自由度为8的单应矩阵可以通过4对匹配特征点算出，即求解以下的线性方程组：
这种做法把H矩阵看成了向量，通过解该向量的线性方程来恢复H，又称直接线性变换法(Direct Linear Transform，DLT)。
以上是视觉SLAM十四讲里的原理讲解；
下面另外一种解释：
为了化为齐次方程，左右两边同时叉乘p2，得到：
写成矩阵形式：
展开计算得到
写成齐次方程
转化为矩阵形式
则有AX =0；
对A进行SVD分解，取D对应的最后一个向量作为最优解。
下面如何对H矩阵进行分解得分R，t：
对A进行SVD分解
基础矩阵F的推导
在第一帧的坐标系下，设P的空间位置为P=[X,Y,Z]T.根据相机模型，可知两个像素点p1,p2的像素位置为
解释：有时会使用齐次坐标表示像素点。使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数，这通常用于表达一个投影关系，例如s1p1和p1成投影关系，它们在齐次坐标的意义下是相等的，称这种相等关系为尺度意义下相等，记作：
那么上述，两个投影关系可写为
现在，取：
这里的x1,x2是两个像素点的归一化平面上的坐标。带入上式，得
两边同时左乘t^,则有
然后，两侧同时左乘xT2:
由于左侧为零，得到一个简洁的式子：
重新代入p1,p2,有
上面两个式子都称为对级约束。
基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E,于是进一步简化对级约束：
下面利用八点法求解本质矩阵，
考虑一对匹配点，他们的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T。根据对极约束，有
把E写成向量的形式：e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T，
那么，对级约束可以写成与e有关的线性形式：
把所有点都放到一个方程中，变成线性方程组：
有了本质矩阵E之后，恢复出相机的运动R，t。这个过程是由奇异值分解(SVD)得到，设E的SVD为
根据线性方程解出的E，可能不满足E的内在性质——它的奇异值不一定为σ,σ,0的形式，这时我们会刻意地把Σ矩阵调整成上面的样子。通常的做法是，对E进行SVD，之后会得到奇异值矩阵Σ=diag(σ1,σ2,σ3)，不妨设σ1 >= σ2 >= σ3，取：
下面分别去看H和F矩阵，
先看H矩阵，
/** * @brief 计算H矩阵，同时计算得分与内点 * @param vbMatchesInliers 经过H矩阵验证，是否为内点，大小为mvMatches12 * @param score 得分 * @param H21 1到2的H矩阵 */ void TwoViewReconstruction::FindHomography(vector<bool> &vbMatchesInliers, float &score, Eigen::Matrix3f &H21) { // Number of putative matches // 匹配成功的个数 const int N = mvMatches12.size(); // Normalize coordinates vector<cv::Point2f> vPn1, vPn2; Eigen::Matrix3f T1, T2; // 像素坐标标准化，先去均值，再除均长 Normalize(mvKeys1, vPn1, T1); Normalize(mvKeys2, vPn2, T2); Eigen::Matrix3f T2inv = T2.inverse(); // Best Results variables score = 0.0; vbMatchesInliers = vector<bool>(N, false); // Iteration variables vector<cv::Point2f> vPn1i(8); vector<cv::Point2f> vPn2i(8); Eigen::Matrix3f H21i, H12i; vector<bool> vbCurrentInliers(N, false); float currentScore; // Perform all RANSAC iterations and save the solution with highest score // 计算归一化后的H矩阵 p2 = H21*p1 for (int it = 0; it < mMaxIterations; it++) { // Select a minimum set for (size_t j = 0; j < 8; j++) { int idx = mvSets[it][j]; vPn1i[j] = vPn1[mvMatches12[idx].first]; vPn2i[j] = vPn2[mvMatches12[idx].second]; } // 计算标准化后的H矩阵 Eigen::Matrix3f Hn = ComputeH21(vPn1i, vPn2i); // 恢复正常H H21i = T2inv * Hn * T1; H12i = H21i.inverse(); currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); // mSigma默认为1 if (currentScore > score) { H21 = H21i; vbMatchesInliers = vbCurrentInliers; score = currentScore; } } }
计算H矩阵
/** * @brief 从特征点匹配求homography（normalized DLT） * |x'| | h1 h2 h3 ||x| * |y'| = a | h4 h5 h6 ||y| 简写: x' = a H x, a为一个尺度因子 * |1 | | h7 h8 h9 ||1| * 使用DLT(direct linear tranform)求解该模型 * x' = a H x * ---> (x') 叉乘 (H x) = 0 * ---> Ah = 0 * A = | 0 0 0 -x -y -1 xy' yy' y'| h = | h1 h2 h3 h4 h5 h6 h7 h8 h9 | * |-x -y -1 0 0 0 xx' yx' x'| * 通过SVD求解Ah = 0，A'A最小特征值对应的特征向量即为解 * @param vP1 归一化后的点, in reference frame * @param vP2 归一化后的点, in current frame * @return 单应矩阵 * @see Multiple View Geometry in Computer Vision - Algorithm 4.2 p109 */ Eigen::Matrix3f TwoViewReconstruction::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) { const int N = vP1.size(); Eigen::MatrixXf A(2 * N, 9); for (int i = 0; i < N; i++) { const float u1 = vP1[i].x; const float v1 = vP1[i].y; const float u2 = vP2[i].x; const float v2 = vP2[i].y; A(2 * i, 0) = 0.0; A(2 * i, 1) = 0.0; A(2 * i, 2) = 0.0; A(2 * i, 3) = -u1; A(2 * i, 4) = -v1; A(2 * i, 5) = -1; A(2 * i, 6) = v2 * u1; A(2 * i, 7) = v2 * v1; A(2 * i, 8) = v2; A(2 * i + 1, 0) = u1; A(2 * i + 1, 1) = v1; A(2 * i + 1, 2) = 1; A(2 * i + 1, 3) = 0.0; A(2 * i + 1, 4) = 0.0; A(2 * i + 1, 5) = 0.0; A(2 * i + 1, 6) = -u2 * u1; A(2 * i + 1, 7) = -u2 * v1; A(2 * i + 1, 8) = -u2; } Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullV); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> H(svd.matrixV().col(8).data()); return H; }
下面根据H矩阵恢复3D点以及R,t
* @param vbMatchesInliers 匹配是否合法，大小为mvMatches12 * @param H21 从参考帧到当前帧的单应矩阵 * @param K 相机内参 * @param T21 旋转平移（要输出的） * @param vP3D 恢复的世界坐标系下三维点的坐标（要输出的） * @param vbTriangulated 大小与vbMatchesInliers，特征点是否成功三角化的标记 * @param minParallax 1，对特征点的三角化测量中，认为其测量有效时需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差），单位是角度 * @param minTriangulated 50，为了进行运动恢复，所需要的最少得三角化测量成功的点个数 */ bool TwoViewReconstruction::ReconstructH( vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K, Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) { // 统计匹配的特征点对中属于内点或有效点个数 int N = 0; for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++) if (vbMatchesInliers[i]) N++; // We recover 8 motion hypotheses using the method of Faugeras et al. // Motion and structure from motion in a piecewise planar environment. // International Journal of Pattern Recognition and Artificial Intelligence, 1988 // step1：SVD分解Homography // 因为特征点是图像坐标系，所以将H矩阵由相机坐标系换算到图像坐标系
// 参考SLAM十四讲第二版p170-p171
// H = K * (R - t * n / d) * K_inv
// 其中: K表示内参数矩阵
// K_inv 表示内参数矩阵的逆
// R 和 t 表示旋转和平移向量
// n 表示平面法向量
// 令 H = K * A * K_inv
// 则 A = k_inv * H * k
Eigen::Matrix3f invK = K.inverse();
Eigen::Matrix3f A = invK * H21 * K;//在尺度等价性的约束下得到 // 对矩阵A进行SVD分解
// A 等待被进行奇异值分解的矩阵
// w 奇异值矩阵
// U 奇异值分解左矩阵
// Vt 奇异值分解右矩阵，注意函数返回的是转置
// cv::SVD::FULL_UV 全部分解
// A = U * w * Vt Eigen::JacobiSVD<Eigen::Matrix3f> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f U = svd.matrixU(); Eigen::Matrix3f V = svd.matrixV(); Eigen::Matrix3f Vt = V.transpose(); Eigen::Vector3f w = svd.singularValues(); float s = U.determinant() * Vt.determinant(); float d1 = w(0); float d2 = w(1); float d3 = w(2); // SVD分解的正常情况是特征值降序排列d1>=d2>=d3 if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001) { return false; } //在ORBSLAM中没有对奇异值d1 d2 d3按照论文中描述的关系进行分类讨论，而是直接进行了计算
//定义8种情况下的旋转矩阵、平移向量和空间法向量 vector<Eigen::Matrix3f> vR; vector<Eigen::Vector3f> vt, vn; vR.reserve(8); vt.reserve(8); vn.reserve(8); //d' >0时的4组解
//根据论文eq.(12)有
//x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
//x2 = 0
//x3 = e3 * sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3))
//令aux1 = sqrt((d1 * d1 - d2 * d2)/ (d1 * d1 - d3 * d3))
// aux3 = sqrt((d2 * d2 - d3 * d3)/(d1 * d1 - d3 * d3))
//则x1 = e1 * aux1, x3 = e3 * aux2
//因为e1 e2 e3 = 1 or -1,x2 = 0,所以有x1 和x3的四种组合
//x1[] = {aux1, aux1, -aux1, -aux1}
//x3[] = {aux3, -aux3, aux3, -aux3} float aux1 = sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)); float aux3 = sqrt((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3)); float x1[] = {aux1, aux1, -aux1, -aux1}; float x3[] = {aux3, -aux3, aux3, -aux3}; //根据论文eq(13)有
//R’ = ctheta 0 -stheta
// 0 1 0
// stheta 0 ctheta
//又由于stheta = e1 * e3 * sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)/(d1 + d3)/d2)
//ctheta = (d2 * d2 + d1 * d3)/((d1 + d3) * d2)
//令aux_stheta = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)/(d1 + d3)/d2)
// stheta = e1 * e3 * aux_stheta
//由于e1 e3 可取1 or -1
//stheta = {aux_stheta,-aux_stheta, -aux_stheta, aux_stheta} float aux_stheta = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 + d3) * d2); float ctheta = (d2 * d2 + d1 * d3) / ((d1 + d3) * d2); float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; // step3.2：计算四种旋转矩阵R，t // 由于论文中eq(12)中x1和x3有四种解，得出论文中eq(14)tp=（d1 - d3）*[x1 0 -x3]T，也有四种解
//根据论文中eq(13)R‘ = ctheta 0 -stheta
// 0 1 0
// stheta 0 ctheta
// | ctheta 0 -aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | |-aux3| // | ctheta 0 aux_stheta| | aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | | aux3| // | ctheta 0 aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // |-aux_stheta 0 ctheta | |-aux3| // | ctheta 0 -aux_stheta| |-aux1| // Rp = | 0 1 0 | tp = | 0 | // | aux_stheta 0 ctheta | | aux3| for (int i = 0; i < 4; i++) { Eigen::Matrix3f Rp; Rp.setZero(); Rp(0, 0) = ctheta; Rp(0, 2) = -stheta[i]; Rp(1, 1) = 1.f; Rp(2, 0) = stheta[i]; Rp(2, 2) = ctheta; //根据Rp求出了R Eigen::Matrix3f R = s * U * Rp * Vt; vR.push_back(R); //根据tp求出了t Eigen::Vector3f tp; tp(0) = x1[i]; tp(1) = 0; tp(2) = -x3[i]; tp *= d1 - d3; // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度 // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变 //根据论文中eq(8)来恢复原始的t
Eigen::Vector3f t = U * tp; vt.push_back(t / t.norm()); Eigen::Vector3f np; np(0) = x1[i]; np(1) = 0; np(2) = x3[i]; //根据论文中eq(8)恢复平面法向量 Eigen::Vector3f n = V * np; if (n(2) < 0) n = -n; vn.push_back(n); } // case d'=-d2 ,对于d’小于0时的4组解
//根据论文中eq(15) // R’ = ctheta 0 stheta
// 0 -1 0
// stheta 0 -ctheta
//stheta = e1 * e3 * sqrt((d1 * d1 - d2 * d2)*(d2 * d2 - d3 * d3)/((d1 - d3) * d2))
//ctheta = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2)
//由于e1 和e3可以取 1 or -1，故有四组接
//根据论文eq(12)，有x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
//x2 = 0,x3 = e3 * sqrt((d2 * d2 - d3 * d3)/(d1 * d1 - d3 *d3))
//根据论文eq(16)求解平移向量tp = (d1 +d3) * [x1 0 x3]T float aux_sphi = sqrt((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) / ((d1 - d3) * d2); float cphi = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2); float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi}; // step3.4：计算四种旋转矩阵R，t // 计算旋转矩阵 R‘ for (int i = 0; i < 4; i++) { Eigen::Matrix3f Rp; Rp.setZero(); Rp(0, 0) = cphi; Rp(0, 2) = sphi[i]; Rp(1, 1) = -1; Rp(2, 0) = sphi[i]; Rp(2, 2) = -cphi; //根据Rp求出R Eigen::Matrix3f R = s * U * Rp * Vt; vR.push_back(R); Eigen::Vector3f tp; tp(0) = x1[i]; tp(1) = 0; tp(2) = x3[i]; tp *= d1 + d3; //根据tp求出t Eigen::Vector3f t = U * tp; vt.push_back(t / t.norm()); Eigen::Vector3f np; np(0) = x1[i]; np(1) = 0; np(2) = x3[i]; //求平面的法向量 Eigen::Vector3f n = V * np; if (n(2) < 0) n = -n; vn.push_back(n); } //最好的点 int bestGood = 0; int secondBestGood = 0;//次好的点 int bestSolutionIdx = -1;//最好的解的索引，初始值为-1 float bestParallax = -1;//最大的视差角 vector<cv::Point3f> bestP3D;//存储最好解对应的，对特征点对进行三角化测量的结果 vector<bool> bestTriangulated;//最佳解所对应的，那些可以被三角化测量的点的标记 // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax) // We reconstruct all hypotheses and check in terms of triangulated points and parallax // step4：d'=d2和d'=-d2分别对应8组(R t)，通过恢复3D点并判断是否在相机正前方点数最多的方法来确定最优解 for (size_t i = 0; i < 8; i++) { float parallaxi;//第i组解对应的比较大的视差角 vector<cv::Point3f> vP3Di;//三角化测量之后的特征点的空间坐标 vector<bool> vbTriangulatedi;//特征点对是否被三角化的标记
//nGood计算good点的数目；vR[i],vt[i]当前组解的旋转矩阵和平移向量,mvkeys1,mvkeys2特征点匹配对
//mvMatches12.vbMatchesInliers分别为特征匹配关系以及Inlier标记，
//存储三角化测量之后的特征点空间坐标，4 *msigma2为三角化过程中允许的最大重投影误差
//vbTriangulatedi 特征点是否被成功进行三角测量的标记
//parallaxi 这组解在三角化测量的时候的比较大的视差角 int nGood = CheckRT(vR[i], vt[i], mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3Di, 4.0 * mSigma2, vbTriangulatedi, parallaxi); // 保留最优的和次优的 if (nGood > bestGood) { secondBestGood = bestGood; bestGood = nGood; bestSolutionIdx = i; bestParallax = parallaxi; bestP3D = vP3Di; bestTriangulated = vbTriangulatedi; } else if (nGood > secondBestGood) { secondBestGood = nGood; } } // step5：通过判断最优是否明显好于次优，从而判断该次Homography分解是否成功
//good点数最优解明显大于次优解，这里取0.75经验值，视差角大于规定的阈值，good点数要大于规定的最小的被三角化的点数目
//good数要足够多，达到总数的90%以上 if (secondBestGood < 0.75 * bestGood && bestParallax >= minParallax && bestGood > minTriangulated && bestGood > 0.9 * N) { T21 = Sophus::SE3f(vR[bestSolutionIdx], vt[bestSolutionIdx]); vbTriangulated = bestTriangulated; return true; } return false; }
进入CheckRT函数，验证位姿的有效性，并恢复地图点
/** * @brief 进行cheirality check，从而进一步找出F分解后最合适的解 * @param R 旋转 * @param t 平移 * @param vKeys1 特征点 * @param vKeys2 特征点 * @param vMatches12 匹配关系 * @param vbMatchesInliers 匹配关系是否有效 * @param K 内参 * @param vP3D 三维点 * @param th2 误差半径 * @param vbGood 大小与mvKeys1一致，表示哪个点被重建了 * @param parallax */ int TwoViewReconstruction::CheckRT( const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers, const Eigen::Matrix3f &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax) { // Calibration parameters const float fx = K(0, 0); const float fy = K(1, 1); const float cx = K(0, 2); const float cy = K(1, 2); //将变量VbGoo以及3D点的大小设置为vKeys1关键点的数量 vbGood = vector<bool>(vKeys1.size(), false); vP3D.resize(vKeys1.size()); vector<float> vCosParallax; vCosParallax.reserve(vKeys1.size()); // Camera 1 Projection Matrix K[I|0] // 步骤1：得到一个相机的投影矩阵 // 以第一个相机的光心作为世界坐标系 Eigen::Matrix<float, 3, 4> P1; P1.setZero(); P1.block<3, 3>(0, 0) = K; Eigen::Vector3f O1; O1.setZero(); // Camera 2 Projection Matrix K[R|t] // 步骤2：得到第二个相机的投影矩阵 Eigen::Matrix<float, 3, 4> P2; P2.block<3, 3>(0, 0) = R; P2.block<3, 1>(0, 3) = t; P2 = K * P2; // 第二个相机的光心在世界坐标系下的坐标 Eigen::Vector3f O2 = -R.transpose() * t; int nGood = 0; for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) { if (!vbMatchesInliers[i]) continue; // kp1和kp2是匹配特征点 const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first]; const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second]; Eigen::Vector3f p3dC1; Eigen::Vector3f x_p1(kp1.pt.x, kp1.pt.y, 1); Eigen::Vector3f x_p2(kp2.pt.x, kp2.pt.y, 1); // 步骤3：利用三角法恢复三维点p3dC1 GeometricTools::Triangulate(x_p1, x_p2, P1, P2, p3dC1); if (!isfinite(p3dC1(0)) || !isfinite(p3dC1(1)) || !isfinite(p3dC1(2))) { vbGood[vMatches12[i].first] = false; continue; } // Check parallax // 步骤4：计算视差角余弦值 Eigen::Vector3f normal1 = p3dC1 - O1; float dist1 = normal1.norm(); Eigen::Vector3f normal2 = p3dC1 - O2; float dist2 = normal2.norm(); float cosParallax = normal1.dot(normal2) / (dist1 * dist2); // 步骤5：判断3D点是否在两个摄像头前方 // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth) // 步骤5.1：3D点深度为负，在第一个摄像头后方，淘汰 if (p3dC1(2) <= 0 && cosParallax < 0.99998) continue; // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth) // 步骤5.2：3D点深度为负，在第二个摄像头后方，淘汰 Eigen::Vector3f p3dC2 = R * p3dC1 + t; if (p3dC2(2) <= 0 && cosParallax < 0.99998) continue; // 步骤6：计算重投影误差 // Check reprojection error in first image // 计算3D点在第一个图像上的投影误差 float im1x, im1y; float invZ1 = 1.0 / p3dC1(2); im1x = fx * p3dC1(0) * invZ1 + cx; im1y = fy * p3dC1(1) * invZ1 + cy; float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y); // 步骤6.1：重投影误差太大，跳过淘汰 // 一般视差角比较小时重投影误差比较大 if (squareError1 > th2) continue; // Check reprojection error in second image // 计算3D点在第二个图像上的投影误差 float im2x, im2y; float invZ2 = 1.0 / p3dC2(2); im2x = fx * p3dC2(0) * invZ2 + cx; im2y = fy * p3dC2(1) * invZ2 + cy; float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y); // 步骤6.1：重投影误差太大，跳过淘汰 // 一般视差角比较小时重投影误差比较大 if (squareError2 > th2) continue; // 步骤7：统计经过检验的3D点个数，记录3D点视差角 vCosParallax.push_back(cosParallax); vP3D[vMatches12[i].first] = cv::Point3f(p3dC1(0), p3dC1(1), p3dC1(2)); nGood++; if (cosParallax < 0.99998) vbGood[vMatches12[i].first] = true; } // 7 得到3D点中较小的视差角，并且转换成为角度制表示 if (nGood > 0) { // 从小到大排序，注意vCosParallax值越大，视差越小 sort(vCosParallax.begin(), vCosParallax.end()); // !排序后并没有取最小的视差角，而是取一个较小的视差角 // 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大) // 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角 size_t idx = min(50, int(vCosParallax.size() - 1)); // 将这个选中的角弧度制转换为角度制 parallax = acos(vCosParallax[idx]) * 180 / CV_PI; } else parallax = 0; return nGood; }
3、广角镜头KannalaBrandt8，如鱼眼相机模型；
鱼眼镜头设计的目的在于拍摄大的视野，鱼眼镜头的视场可达到180~270度，在工程上一般视角超过140度的镜头统称为鱼眼镜头。
鱼眼相机一般结构如下图所示
鱼眼相机结构：一般由若干不同的透镜组合而成，在成像过程中，入射光线经过不同程度的折射，投影到尺度有限的成像平面上。
鱼眼相机与针孔相机成像光路对比：
鱼眼相机模型：由于鱼眼相机多元件结构折射关系的复杂性，在论文“A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses”中提出单位球面投影模型，用以简化该折射关系，如下图所示
一般将鱼眼相机成像过程分解成两步：先将三维空间点线性的投影到虚拟单位球面上；然后将单位球面上的点投影到图像平面上，这个过程是非线性的。
下面只关注X轴上的映射关系，Y轴类似，正视上面图2，根据投影函数的不同（投影函数是关于θ的函数），得到如下图形，
从图像平面可以看出，根据不同投影函数，得到从图像中心点到相同三维点对应的不同图像坐标；可以看出f*tan(θ)等价于针孔相机模型；
为了利于标定，在论文中提出了Kannala-Brandt模型，以2tan(θ/2)为例，焦距*θd为图像中心到投影点的距离，θd=θ+k1θ3+k2θ5+k3θ7+k4θ9 = 2tan(θ/2)，可以用一个统一的投影函数表示：
r=f * θd
三维点P的成像过程:
fx = 1/△x ,fy=1/△y;
图像平面上的点反投影得到3D点
3、RGB-D相机模型
原理：RGB-D相机是通过主动测量每个像素的深度来实现像素深度值的确定。
1）通过红外结构光(Structured Light)原理测量像素距离，如Kinect1，Project Tango 1，Intel RealSense等
在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。
2）通过飞行时间(Time-of-Flight,TOF)原理测量像素距离，如Kinect2等
在TOF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。
RGB-D相机会自动完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。
4、双目相机模型
原理：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。
根据上图右侧的两个相似三角形的关系，可得
整理可得，
其中d定义为左右图的横坐标之差，称为视差。
二、ORB特征点提取相关原理
三、双目及RGBD初始化
四、单目初始化
