从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); } else{ cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); bool b_parse_cam = ParseCamParamFile(fSettings); if(!b_parse_cam) { std::cout << "*Error with the camera parameters in the config file*" << std::endl; } // Load ORB parameters bool b_parse_orb = ParseORBParamFile(fSettings); if(!b_parse_orb) { std::cout << "*Error with the ORB parameters in the config file*" << std::endl; } bool b_parse_imu = true; if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO || sensor==System::IMU_RGBD) { b_parse_imu = ParseIMUParamFile(fSettings); if(!b_parse_imu) { std::cout << "*Error with the IMU parameters in the config file*" << std::endl; } mnFramesToResetIMU = mMaxFrames; } if(!b_parse_cam || !b_parse_orb || !b_parse_imu) { std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; try { throw -1; } catch(exception &e) { } } } initID = 0; lastID = 0; mbInitWith3KFs = false; mnNumDataset = 0; // 遍历下地图中的相机，然后打印出来了 vector<GeometricCamera*> vpCams = mpAtlas->GetAllCameras(); std::cout << "There are " << vpCams.size() << " cameras in the atlas" << std::endl; for(GeometricCamera* pCam : vpCams) { std::cout << "Camera " << pCam->GetId(); if(pCam->GetType() == GeometricCamera::CAM_PINHOLE) { std::cout << " is pinhole" << std::endl; } else if(pCam->GetType() == GeometricCamera::CAM_FISHEYE) { std::cout << " is fisheye" << std::endl; } else { std::cout << " is unknown" << std::endl; } } #ifdef REGISTER_TIMES vdRectStereo_ms.clear(); vdResizeImage_ms.clear(); vdORBExtract_ms.clear(); vdStereoMatch_ms.clear(); vdIMUInteg_ms.clear(); vdPosePred_ms.clear(); vdLMTrack_ms.clear(); vdNewKF_ms.clear(); vdTrackTotal_ms.clear(); #endif }