文章目录

ROS
- ros_mono_interial.cc
- - Function
  - 主函数
  - imu抓取
  - image 抓取
  - image与imu同步
ORB_SLAM3::System
- Function
- 构造
- TrackMonocular 单目跟踪
- - TrackStereo
  - TrackRGBD
- Shutdown 关闭
- SaveTrajectoryTUM 按TUM保存轨迹
- 其他数据的保存
Tracking
- 核心函数
- - 构造
  - GrabImageMonocular
  - Track 跟踪
- 相关函数-地图集
- - CreateMapInAtlas
  - ResetActiveMap
- 相关函数-参数读取
- - ParseCamParamFile 读取相机文件
  - ParseORBParamFile 读取orb参数
  - ParseIMUParamFile 读取imu参数
- 相关函数-Imu相关
- - PreintegrateIMU IMU预积分
  - PredictStateIMU 利用IMU计算位姿
  - ComputeGyroBias 角度零偏
  - ComputeVelocitiesAccBias 加速度零偏
- 相关函数-初始化
- - MonocularInitialization 单目初始化
  - CreateInitialMapMonocular 创建初始化单目地图
- 相关函数-跟踪
- - TrackWithMotionModel 恒速模型跟踪
  - UpdateLastFrame 更新上一Frame
  - TrackReferenceKeyFrame 跟踪参考关键帧
  - TrackLocalMap 跟踪局部地图
- 相关函数-关键帧
- - Relocalization 重定位
  - NeedNewKeyFrame 关键帧选取策略
  - CreateNewKeyFrame 创建新的关键帧

ROS

程序分为ROS接口和非ROS接口的。
主程序 main 在ROS/ORB_SLAM3/src/中：

ros_mono_interial.cc

Function

主函数：读取参数，并创建了slam系统，订阅两topic+创建同步线程。
回调函数：只将数据放入队列，上锁
同步线程：1s一次，先数据同步，后进行跟踪

主函数

读取参数 argc
创建SLAM系统，它初始化所有系统线程并准备好处理帧。
订阅IMU+CAMERA topic，回调中将数据放入队列
数据同步线程 ( Image 与 imu)

int main(int argc, char **argv)
{ros::init(argc, argv, "Mono_Inertial");ros::NodeHandle n("~");ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);bool bEqual = false;if(argc < 3 || argc > 4){cerr << endl << "Usage: rosrun ORB_SLAM3 Mono_Inertial path_to_vocabulary path_to_settings [do_equalize]" << endl;ros::shutdown();return 1;}if(argc==4){std::string sbEqual(argv[3]);if(sbEqual == "true")bEqual = true;}// Create SLAM system. It initializes all system threads and gets ready to process frames.ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR,true);ImuGrabber imugb;ImageGrabber igb(&SLAM,&imugb,bEqual); // TODO// Maximum delay, 5 secondsros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb);std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);ros::spin();return 0;
}

imu抓取

class ImuGrabber
{public:ImuGrabber(){};void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);queue<sensor_msgs::ImuConstPtr> imuBuf;std::mutex mBufMutex;
};
// 直接放入imubuf中
void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{mBufMutex.lock();imuBuf.push(imu_msg);mBufMutex.unlock();return;
}

image 抓取

class ImageGrabber
{public:ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), mbClahe(bClahe){}void GrabImage(const sensor_msgs::ImageConstPtr& msg);cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);void SyncWithImu();queue<sensor_msgs::ImageConstPtr> img0Buf;std::mutex mBufMutex;ORB_SLAM3::System* mpSLAM;ImuGrabber *mpImuGb;const bool mbClahe;cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
};// 回调就是将其数据放入队列
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg)
{mBufMutex.lock();if (!img0Buf.empty())img0Buf.pop();img0Buf.push(img_msg);mBufMutex.unlock();
}

image与imu同步

步骤：

确保imu时间是否包含住image时间，否则continue
取出 image + vector
基于参数确定图像是否需要直方图均衡。

void ImageGrabber::SyncWithImu()
{while(1){cv::Mat im;double tIm = 0;if (!img0Buf.empty()&&!mpImuGb->imuBuf.empty()){tIm = img0Buf.front()->header.stamp.toSec();// 保证：imu 最新时间比 image 新if(tIm>mpImuGb->imuBuf.back()->header.stamp.toSec())continue;{  // 取出图片this->mBufMutex.lock();im = GetImage(img0Buf.front());img0Buf.pop();this->mBufMutex.unlock();}// 取出 小于image时间的imu数据，{time，线加速度，角加速度}vector<ORB_SLAM3::IMU::Point> vImuMeas;mpImuGb->mBufMutex.lock();if(!mpImuGb->imuBuf.empty()){// Load imu measurements from buffervImuMeas.clear();while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tIm){double t = mpImuGb->imuBuf.front()->header.stamp.toSec();cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));mpImuGb->imuBuf.pop();}}mpImuGb->mBufMutex.unlock();if(mbClahe)mClahe->apply(im,im);mpSLAM->TrackMonocular(im,tIm,vImuMeas);}std::chrono::milliseconds tSleep(1);std::this_thread::sleep_for(tSleep);}
}
// cv::CLAHE 是另外一种直方图均衡算法，CLAHE 和 AHE 的区别在于前者对区域对比度实行了限制，并且利用插值来加快计算。它能有效的增强或改善图像（局部）对比度，从而获取更多图像相关边缘信息有利于分割。还能够有效改善 AHE 中放大噪声的问题。另外，CLAHE 的有一个用途是被用来对图像去雾。

ros-image->Mat

cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
{// Copy the ros image message to cv::Mat.cv_bridge::CvImageConstPtr cv_ptr;try{cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}if(cv_ptr->image.type()==0){return cv_ptr->image.clone();}else{std::cout << "Error type" << std::endl;return cv_ptr->image.clone();}
}

ORB_SLAM3::System

Function

main主函数定义了System类，他创建了跟踪、局部地图线程、闭环线程。
- 跟踪：image+imu同步线程中。
System.cc这个文件主要是SLAM系统的初始化、跟踪线程的入口以及轨迹的保存。

构造

构建字典并加载已有字典
建立关键帧库和地图集
创建显示 Frame和地图的类，由viewer调用
创建跟踪线程
创建局部地图线程
创建闭环线程

System::System(
const string &strVocFile  //词典文件路径
, const string &strSettingsFile //配置文件路径
, const eSensor sensor  //传感器类型
,const bool bUseViewer // 是否使用可视化界面
, const int initFr // 初始化Frame
, const string &strSequence
, const string &strLoadingFile):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false)
{// 打印 什么传感器if(mSensor==MONOCULAR) //单目cout << "Monocular" << endl;else if(mSensor==STEREO) //双目cout << "Stereo" << endl;else if(mSensor==RGBD)cout << "RGB-D" << endl;else if(mSensor==IMU_MONOCULAR) //IMU+单目cout << "Monocular-Inertial" << endl;else if(mSensor==IMU_STEREO) // IMU+双目cout << "Stereo-Inertial" << endl;//Check settings file //将配置文件名转换成为字符串 且只读cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);//如果打开失败，就输出调试信息，然后退出if(!fsSettings.isOpened()){ cerr << "Failed to open settings file at: " << strSettingsFile << endl;exit(-1);}// 加载Atlas的标志，目前没有用bool loadedAtlas = false;//Load ORB Vocabularycout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;// 建立一个新的ORB字典，并加载已有的orb字典词袋mpVocabulary = new ORBVocabulary();bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);// 如果加载失败，就输出调试信息，并退出if(!bVocLoad){cerr << "Wrong path to vocabulary. " << endl;cerr << "Falied to open at: " << strVocFile << endl;exit(-1);}cout << "Vocabulary loaded!" << endl << endl;//Create KeyFrame Database 创建关键帧数据库mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//Create the Atlas  创建地图集，参数0表示初始化关键帧id为0mpAtlas = new Atlas(0);// 若使用了imu，则mpAtlas设定imu传感器if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)mpAtlas->SetInertialSensor();// 这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用mpFrameDrawer = new FrameDrawer(mpAtlas);mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile);//Initialize the Tracking thread  初始化跟踪线程//（它将存在于执行的主线程中，即调用此构造函数的线程中）cout << "Seq. Name: " << strSequence << endl;mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence);//创建并开启local mapping线程，回调函数为LocalMapping::Run()mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO, strSequence);mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);//设置区分远点和近点的阈值mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];if(mpLocalMapper->mThFarPoints!=0){cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;mpLocalMapper->mbFarPoints = true;}elsempLocalMapper->mbFarPoints = false;// 创建并开启闭环线程，回调函数为LoopClosing::Run()mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); // mSensor!=MONOCULAR);mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);//Initialize the Viewer thread and launchif(bUseViewer) //如果指定了，程序的运行过程中需要运行可视化部分{mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);mptViewer = new thread(&Viewer::Run, mpViewer);mpTracker->SetViewer(mpViewer);mpLoopCloser->mpViewer = mpViewer;mpViewer->both = mpFrameDrawer->both;}//Set pointers between threads 设置进程间的指针mpTracker->SetLocalMapper(mpLocalMapper);mpTracker->SetLoopClosing(mpLoopCloser);mpLocalMapper->SetTracker(mpTracker);mpLocalMapper->SetLoopCloser(mpLoopCloser);mpLoopCloser->SetTracker(mpTracker);mpLoopCloser->SetLocalMapper(mpLocalMapper);// Fix verbosity 设置打印信息的等级，只有小于这个等级的信息才会被打印Verbose::SetTh(Verbose::VERBOSITY_QUIET);}

TrackMonocular 单目跟踪

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{   // 单目 和 IMU+单目if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR){cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;exit(-1);}// Check mode change 检查是否有运行模式的改变{unique_lock<mutex> lock(mMutexMode);if(mbActivateLocalizationMode) //激活定位模式？，局部建图线程会关闭{  mpLocalMapper->RequestStop();//请求停止局部建图线程// Wait until Local Mapping has effectively stoppedwhile(!mpLocalMapper->isStopped()){usleep(1000);}// 运行到这里的时候，局部建图部分就真正地停止了// 通知Tracking线程，现在只进行跟踪，不建图mpTracker->InformOnlyTracking(true);// 设置为false，避免重复进行以上操作mbActivateLocalizationMode = false;}if(mbDeactivateLocalizationMode) //取消定位模式？，重新打开局部建图线程{   // 仅跟踪设置为false，localmap =Release,同时避免重复mpTracker->InformOnlyTracking(false);mpLocalMapper->Release(); //局部建图器要开始工作呢mbDeactivateLocalizationMode = false; //防止重复执行}}// Check reset  检查是否有复位的操作{unique_lock<mutex> lock(mMutexReset);if(mbReset) // 是否有复位请求？{   //有，追踪器复位+清除标志+不重置活跃子图mpTracker->Reset();mbReset = false;mbResetActiveMap = false;}else if(mbResetActiveMap) // 重置活跃地图？{cout << "SYSTEM-> Reseting active map in monocular case" << endl;mpTracker->ResetActiveMap();mbResetActiveMap = false;}}// 若imu+单目，则 tracking 中抓imu数据if (mSensor == System::IMU_MONOCULAR)for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)mpTracker->GrabImuData(vImuMeas[i_imu]);// 开始跟踪，获取相机位姿的估计结果  ****运动估计的函数cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);unique_lock<mutex> lock2(mMutexState);// 记录track追踪的状态mTrackingState = mpTracker->mState; ///获取当前帧追踪到的地图点向量指针mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;// 获取当前帧追踪到的关键帧特征点向量的指针mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;// //返回获得的相机运动估计return Tcw;
}

TrackStereo

双目或双目+IMU

TrackRGBD

RGBD

Shutdown 关闭

void System::Shutdown()
{// 对局部建图线程和回环检测线程发送终止请求mpLocalMapper->RequestFinish();mpLoopCloser->RequestFinish();//如果使用了可视化窗口查看器if(mpViewer){ //向查看器发送终止请求mpViewer->RequestFinish();//等待，直到真正地停止while(!mpViewer->isFinished())usleep(5000);}// Wait until all thread have effectively stopped// 等所有线程工作完成后关闭while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA()){if(!mpLocalMapper->isFinished())cout << "mpLocalMapper is not finished" << endl;if(!mpLoopCloser->isFinished())cout << "mpLoopCloser is not finished" << endl;if(mpLoopCloser->isRunningGBA()){cout << "mpLoopCloser is running GBA" << endl;cout << "break anyway..." << endl;break;}usleep(5000);}if(mpViewer) pangolin::BindToContext("ORB-SLAM2: Map Viewer");#ifdef REGISTER_TIMESmpTracker->PrintTimeStats();
#endif
}

SaveTrajectoryTUM 按TUM保存轨迹

以TUM格式(tx,ty,tz,qx,qy,qz,qw)保存所有成功定位的帧的位姿
关键帧是相对于其参考关键帧（由 BA 和姿势图优化）存储的。我们需要首先获得关键帧姿势，然后连接相对变换。未定位的帧（跟踪失败）不会保存。

void System::SaveTrajectoryTUM(const string &filename)
{cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;// 只有在传感器为双目或者RGBD时才可以工作if(mSensor==MONOCULAR){cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;return;}// 获取地图集中所有的关键帧，并给予Id排序vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);// Transform all keyframes so that the first keyframe is at the origin.转换所有关键帧，使第一个关键帧位于原点.// After a loop closure the first keyframe might not be at the origin.闭环后，第一个关键帧可能不在原点。cv::Mat Two = vpKFs[0]->GetPoseInverse();ofstream f;f.open(filename.c_str());// 用一般的方式输出浮点型，例如Ｃ＋＋程序在控制台显示大一点的数，显示的时候使用了科学计数法，使用该命令即可像一般的方式显示f << fixed;// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).// We need to get first the keyframe pose and then concatenate the relative transformation.// Frames not localized (tracking failure) are not saved.// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag// which is true when tracking failed (lbL).//  对于每一帧，我们都有一个参考关键帧 (lRit)、时间戳 (lT) 和跟踪失败时为真的标志 (lbL)。list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();  // 关键帧listlist<double>::iterator lT = mpTracker->mlFrameTimes.begin();//时间戳list<bool>::iterator lbL = mpTracker->mlbLost.begin();//是否跟踪失败for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++){if(*lbL) //如果跟踪失败，则跳过，不保存这一帧continue;KeyFrame* pKF = *lRit;//取出参考关键帧cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);// If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.//如果参考关键帧是坏的(可能是检查冗余时被剔除了)，则用其父帧，并记录父帧到原参考关键帧的位姿变换while(pKF->isBad()) {  // 更新关键帧变换矩阵的初始值，Trw = Trw*pKF->mTcp;//并且更新到原关键帧的父关键帧pKF = pKF->GetParent();}// 最终得到的是参考关键帧相对于世界坐标系的变换Trw = Trw*pKF->GetPose()*Two;// 在此基础上得到相机当前帧相对于世界坐标系的变换cv::Mat Tcw = (*lit)*Trw;//然后分解出旋转矩阵cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//以及平移向量cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);//用四元数表示旋转vector<float> q = Converter::toQuaternion(Rwc);//然后按照给定的格式输出到文件中f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;}f.close();// cout << endl << "trajectory saved!" << endl;
}

其他数据的保存

以TUM格式保存所有关键帧的位姿
void SaveKeyFrameTrajectoryTUM(const string &filename);
以EuRoC格式保存所有帧的位姿
void SaveTrajectoryEuRoC(const string &filename);
以EuRoC格式保存所有关键帧的位姿
void SaveKeyFrameTrajectoryEuRoC(const string &filename);
以KITTI格式保存所有帧的位姿
void SaveTrajectoryKITTI(const string &filename);

Tracking

核心函数

构造

namespace ORB_SLAM3
{Tracking::Tracking(
System *pSys   // systerm 中 this指针
, ORBVocabulary* pVoc  // BOW词袋，包括了已加载的词袋
, FrameDrawer *pFrameDrawer //帧绘制器
, MapDrawer *pMapDrawer // 地图点绘制器
, Atlas *pAtlas // 地图集句柄
, KeyFrameDatabase* pKFDB  // 关键帧产生的词袋数据库
, const string &strSettingPath // 配置文件路径
, const int sensor // 传感器类型
, const string &_nameSeq):mState(NO_IMAGES_YET) // 当前状态还没准备好, mSensor(sensor), mTrackedFr(0), mbStep(false),mbOnlyTracking(false) // 处于slam模式, mbMapUpdated(false), mbVO(false), //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB),mpInitializer(static_cast<Initializer*>(NULL)), //暂时给地图初始化器设置为空指针mpSystem(pSys), mpViewer(NULL),mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), time_recently_lost_visual(2.0),mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr)
{// Load camera parameters from settings filecv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);///< step 1: 从配置文件中加载相机参数// Load ORB parameters 加载相机参数文件// 相机参数，两类：//        1. PinHole: fx,fy,cx,cy,k1,k2,p1,p2  --k3//     2. KannalaBrandt8:fx,fy,cx,cy,k1,k2,k3,k4 其他就不支持了// STEREO or IMU_STEREO: br// fps , fps==0 ? 30 : fps// RGB ? RGB : BGR// STEREO || RGBD || IMU_STEREO ? ThDepth (深度阈值（近/远点))// RGBD ? DepthMapFactor// 若上述都存在且真实，则 return true 否则 falsebool b_parse_cam = ParseCamParamFile(fSettings);if(!b_parse_cam) // 读取失败则打印error{std::cout << "*Error with the camera parameters in the config file*" << std::endl;}///< step 2: 加载ORB特征点有关的参数，并构造ORB提取对象// nFeatures: 1000 每一帧提取的特征点数 1000// scaleFactor: 1.2 图像建立金字塔时的变化尺度 1.2// nLevels: 8 尺度金字塔的层数 8// iniThFAST: 20 提取fast特征点的默认阈值 20// minThFAST: 7 如果默认阈值提取不出足够fast特征点，则使用最小阈值 7/// 构造orb提取对象：构造左目提取器，若双目或imu_双目时？再构造右目提取器，若单目或imu_单目时？构造mpIniORBextractor/// 特征数：mpIni=5*(mpLeft=mpRight)bool b_parse_orb = ParseORBParamFile(fSettings);if(!b_parse_orb){std::cout << "*Error with the ORB parameters in the config file*" << std::endl;}initID = 0; lastID = 0;// Load IMU parameters bool b_parse_imu = true; // 加载 imu 参数if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO) // 只使用imu时加载{   // 外参 Tcw 4*4 ///< **IMU noise (Use those from VINS-mono)**// IMU.NoiseGyro: 0.00016 # rad/s^0.5 // IMU.NoiseAcc: 0.0028 # m/s^1.5// IMU.GyroWalk: 0.000022 # rad/s^1.5// IMU.AccWalk: 0.00086 # m/s^2.5// IMU.Frequency: 200b_parse_imu = ParseIMUParamFile(fSettings);if(!b_parse_imu){std::cout << "*Error with the IMU parameters in the config file*" << std::endl;}mnFramesToResetIMU = mMaxFrames;}mbInitWith3KFs = false;mnNumDataset = 0;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){}}#ifdef REGISTER_TIMESvdRectStereo_ms.clear();vdORBExtract_ms.clear();vdStereoMatch_ms.clear();vdIMUInteg_ms.clear();vdPosePred_ms.clear();vdLMTrack_ms.clear();vdNewKF_ms.clear();vdTrackTotal_ms.clear();vdUpdatedLM_ms.clear();vdSearchLP_ms.clear();vdPoseOpt_ms.clear();
#endifvnKeyFramesLM.clear();vnMapPointsLM.clear();
}

读取参数文件时，是否存在以及是否real案例：

 // Camera calibration parameterscv::FileNode node = fSettings["Camera.fx"];if(!node.empty() && node.isReal()){fx = node.real();}else{std::cerr << "*Camera.fx parameter doesn't exist or is not a real number*" << std::endl;b_miss_params = true;}

GrabImageMonocular

输入：左目RGB或RGBA图像
1、将图像转为mImGray并初始化mCurrentFrame
2、进行tracking过程
输出：世界坐标系到该帧相机坐标系的变换矩阵

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, string filename)
{mImGray = im;// step 1 ：将RGB或RGBA图像转为灰度图像if(mImGray.channels()==3){if(mbRGB)cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);elsecvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY);}else if(mImGray.channels()==4){if(mbRGB)cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY);elsecvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY);}// step 2 ：构造Frame  if (mSensor == System::MONOCULAR)   // 若是单目{  // 《状态没有初始化or没有图片or lastid与初始id间隔太小》时？//  初始特征提取；否则orb特征提取，提取特征值的数量不一样if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET ||(lastID - initID) < mMaxFrames)mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);else  mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth);}else if(mSensor == System::IMU_MONOCULAR) // 单目Imu{// 《状态没有初始化or没有图片》时，初始特征提取；否则orb特征提取if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET){mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);}elsemCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);}if (mState==NO_IMAGES_YET)t0=timestamp;mCurrentFrame.mNameFile = filename;mCurrentFrame.mnDataset = mnNumDataset;#ifdef REGISTER_TIMESvdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
#endiflastID = mCurrentFrame.mnId;// step 3 ：跟踪Track();// 返回当前帧的位姿return mCurrentFrame.mTcw.clone();
}

Track 跟踪

若 localmap中imu数据有问题，则系统：重置活跃地图ResetActiveMap
地图集中取出当前地图
mState!=NO_IMAGES_YET：
1. 当前帧时间回跳时，清空imu数据，并创建地图CreateMapInAtlas，return
2. 当前帧时间与上一帧相差了1s以上时，若地图集未使用imu则return，否则：
  1. imu未初始化成功，则重置活跃子图ResetActiveMap
  2. 若imu初始化成功时：且GetIniertialBA2成功时在地图集中创建地图CreateMapInAtlas，否则重置活跃子图ResetActiveMap
若使用imu且上一帧是关键帧时，当前帧设置新的bias
若 mState == NO_IMAGES_YET，则：mState = NOT_INITIALIZED
若使用imu且非创建地图时，imu进行预积分 (两套)
上一状态和创建地图标记赋值
1. mLastProcessedState=mState;
2. mbCreatedMap = false;
若 mState == NOT_INITIALIZED
1. 基于传感器参数进行相应初始化，如MonocularInitialization
2. 绘制地图更新 mpFrameDrawer->Update(this)
3. 若未正常初始化时， mLastFrame = currentFrame ,return
4. 若地图集 == 1时，mnFirstFrameId = mCurrentFrame.mnId;
否则 mState 已经初始化：
1. 若不是仅跟踪模式：
  1. 若 mState == ok时：
    1. 检查上一帧中被替换的地图点，将地图点替换成新的地图点CheckReplacedInLastFrame
    2. 状态跟踪
      1. 若运动模式是空且imu未初始化或刚刚完成初始化时，跟踪参考关键帧TrackReferenceKeyFrame
      2. 否则，跟踪运动模式TrackWithMotionModel，若运动模式跟踪失败，则跟踪参考帧
      3. 若跟踪失败时，
      4. 若刚刚进行了重定位且imu模式，则直接将mState置为lost
      5. 否则，如果地图中关键帧数量大于10，则 RECENTLY_LOST
      6. 否则 mState = LOST;
  2. 若状态不是ok时：
    1. 如果是最近丢失时 :mState == RECENTLY_LOST
      1. bOK = true;
      2. 如果使用imu时，
        若：imu初始化成功，则用imu跟踪 PredictStateIMU
        
        否则初始化失败，bOK = false;
        3. 若与前一次跟踪相隔5s,记为lost
        
        mState = LOST;，OK=false;
      3. 纯视觉模式下进行重定位，重定位失败记为lost
        mState = LOST;，OK=false;
        2. 若状态为 lost: mState == LOST
        
        若地图关键帧数量小于10个时，则重置活跃地图
        
        否则新建地图，删除mLastKeyFrame,从头开始
2. 若是仅跟踪模式：
  1. 若定位状态为lost，定位丢失则重定位 Relocalization
  2. 否则若跟踪正常时：
    1. 恒速模式时使用速度，bOK = TrackWithMotionModel
    2. 否则使用参考帧， bOK = TrackReferenceKeyFrame
  3. 状态不正常时
    1. 恒速模式非空时用恒速模型，bOKMM = TrackWithMotionModel
    2. 重定位 Relocalization
    3. 恒速模型且重定位失败时，使用了恒速预测的数据
      1. mCurrentFrame 设置为恒速模型的数据
      2. 若仍失败，则当前帧地图点 IncreaseFound
    4. 否则若重定位成功时：设置状态正常 mbVO = false;
    5. 跟踪成功：bOKMM || 重定位成功
      3. 若当前帧是关键帧，则更新关键帧
      4. 若不是仅跟踪模式： // slam模式
      1. 若跟踪成功时，则跟综局部地图（寻找更多的匹配，优化当前帧的pose） TrackLocalMap
      2. 若跟踪失败，则打印
      5. 若定位模式时：
      1. 跟踪成功且状态ok(!mbVO)：跟踪localmap（寻找更多的匹配，优化当前帧的pose）TrackLocalMap
      2. mbVO true 表示地图中与 MapPoints 的匹配项很少。我们无法检索本地地图，因此我们不执行 TrackLocalMap()。一旦系统重新定位相机，我们将再次使用本地地图。
      6. 跟踪成功时：bOK //包括了跟踪局部地图的结果
      1. mstate =ok。
      7. 跟踪失败时：bOKfalse
      1. 若前面位姿跟踪成功时：mState == OK
      1. 使用imu，且当前地图未初始化完imu 或 imu做ba2失败时，系统重置活跃子图ResetActiveMap， mState = RECENTLY_LOST;
      2. 不使用imu时： mState = RECENTLY_LOST;
      3. 更新丢失时间 mTimeStampLost
      8. 如果最近重新定位，且当前帧id>上次重置imu，且使用imu，且地图已经初始化imu
      1. 当前帧构建frame，并进行预积分 Preintegrated
      9. 若pCurrentMap已初始化imu成功时：
      1. 若跟踪成功，且未重置imu，则更新mLastBias
      显示设置当前帧位姿
      10. 如果跟踪成功bOK或者 mStateRECENTLY_LOST,
    6. 更新恒速模型，计算上一帧到当前帧的位姿变换
    7. 若使用imu，则更新当前相机pose SetCurrentCameraPose
    8. 清除恒速跟踪中的临时地图点
    9. 清除观测不到的地图点
    10. 判断是否需要插入关键帧，插入关键帧(对双目或rgb-d会产生新的地图点)
      11. 若状态失败 mState==LOST)时：
    11. 若：关键帧小于5个，或使用imu且imu未初始化时：系统重置活跃子图
    12. 否则创建新地图
      12. 若当前帧不是关键帧，则赋值临时帧。赋值mLastFrame
若状态ok 或状态是最近丢失时：
存储帧姿态信息以在之后检索完整的相机轨迹。

void Tracking::Track()
{if (bStepByStep){while(!mbStep)usleep(500);mbStep = false;}// 若 localmap中imu数据有问题，则系统重置活跃地图if(mpLocalMapper->mbBadImu){cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;mpSystem->ResetActiveMap();return;}// 地图集中 取出 当前地图Map* pCurrentMap = mpAtlas->GetCurrentMap();// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态if(mState!=NO_IMAGES_YET)  // 有图片时{// 当前帧时间回跳时，清空imu数据，并创建地图if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp){cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;unique_lock<mutex> lock(mMutexImuQueue);mlQueueImuData.clear();CreateMapInAtlas();return;}else  // 当前帧与上一帧相差了1s以上时，若未使用imu，直接return，若使用imu,则创建地图 or 重置地图if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0){cout << "id last: " << mLastFrame.mnId << "    id curr: " << mCurrentFrame.mnId << endl;if(mpAtlas->isInertial()) // 使用了IMU{if(mpAtlas->isImuInitialized()) // imu已经初始化{cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;// 1. 若imu初始化成功时：若得到imu ba 错误时重置活跃子图，否则创建地图在地图集中if(!pCurrentMap->GetIniertialBA2()){mpSystem->ResetActiveMap();}else{CreateMapInAtlas();}}else{cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;// 重置活跃子图mpSystem->ResetActiveMap();}}return;}}// 若使用imu且上一帧是关键帧时，当前帧设置新的biasif ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame)mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}mLastProcessedState=mState;if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap){#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now();
#endifPreintegrateIMU();
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now();double timePreImu = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPreIMU - time_StartPreIMU).count();vdIMUInteg_ms.push_back(timePreImu);
#endif}mbCreatedMap = false;// Get Map Mutex -> Map cannot be changedunique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);mbMapUpdated = false;int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();int nMapChangeIndex = pCurrentMap->GetLastMapChange();if(nCurMapChangeIndex>nMapChangeIndex){// cout << "Map update detected" << endl;pCurrentMap->SetLastMapChange(nCurMapChangeIndex);mbMapUpdated = true;}if(mState==NOT_INITIALIZED){// 如果没有初始化，则先进行初始化if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO)StereoInitialization();else{MonocularInitialization();}// 地图更新此mpFrameDrawer->Update(this);// 如果正确初始化，mState=OKif(mState!=OK) // If rightly initialized, mState=OK{  // 若状态不是ok，则当前帧赋值last，returnmLastFrame = Frame(mCurrentFrame);return;}if(mpAtlas->GetAllMaps().size() == 1){mnFirstFrameId = mCurrentFrame.mnId;}}else{// System is initialized. Track Frame.bool bOK;#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now();
#endif// Initial camera pose estimation using motion model or relocalization (if tracking is lost)// 使用运动模型或重新定位的初始相机姿态估计（如果跟踪丢失）if(!mbOnlyTracking)  // 不仅仅是跟踪模式{  // State OK// Local Mapping is activated. This is the normal behaviour, unless// you explicitly activate the "only tracking" mode.// 局部地图工作，这是正常行为，除非您明确激活“仅跟踪”模式。if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame// 检查上一帧中被替换的地图点，将地图点替换成新的地图点CheckReplacedInLastFrame();// 若运动模式是空且imu未初始化 或刚刚完成初始化时 跟踪参考关键帧if((mVelocity.empty() && !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);// 跟踪运动模式bOK = TrackWithMotionModel();if(!bOK)bOK = TrackReferenceKeyFrame();}if (!bOK) //如果跟踪失败了{// 如果刚刚进行了重定位且imu模式，则直接将mState置为lostif ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&(mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO)){mState = LOST;} // 否则，如果地图中关键帧数量大于10，则 RECENTLY_LOSTelse if(pCurrentMap->KeyFramesInMap()>10){cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;mState = RECENTLY_LOST;mTimeStampLost = mCurrentFrame.mTimeStamp;//mCurrentFrame.SetPose(mLastFrame.mTcw);}else{mState = LOST;}}}else // 状态不ok时{// 如果是最近丢失时if (mState == RECENTLY_LOST){Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);bOK = true;// 如果是imu模式，则用imu跟踪，最多跟踪5s,否则记为lostif((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)){if(pCurrentMap->isImuInitialized())PredictStateIMU();elsebOK = false;if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost){mState = LOST;Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);bOK=false;}}else { // 纯视觉模式下进行重定位，重定位失败记为lost// TODO fix relocalizationbOK = Relocalization();if(!bOK && mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost_visual){mState = LOST;Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);bOK=false;}}}else if (mState == LOST) // 状态处于lost时{Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);// 若地图关键帧数量小于10个时，则重置活跃地图if (pCurrentMap->KeyFramesInMap()<10){mpSystem->ResetActiveMap();cout << "Reseting current map..." << endl;}else // 否则新建地图，删除mLastKeyFrame,从头开始CreateMapInAtlas();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)if(mState==LOST) //若定位状态为lost，定位丢失则重定位{if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);bOK = Relocalization();}else{if(!mbVO) // 跟踪正常时{// In last frame we tracked enough MapPoints in the map// 恒速模式时使用速度，否则使用参考帧if(!mVelocity.empty()){    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.//在最后一帧中，我们主要跟踪“视觉里程计”点。 我们计算两个相机姿势，一个来自运动模型，另一个进行重新定位。 如果重新定位成功，我们选择该解决方案，否则我们保留“视觉里程计”解决方案。bool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;// 恒速模式非空用恒速模型if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}// 重定位bOKReloc = Relocalization();if(bOKMM && !bOKReloc){mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;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;}}}// 若当前帧是关键帧，则更新关键帧if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;#ifdef REGISTER_TIMESstd::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_TIMESstd::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now();
#endif// If we have an initial estimation of the camera pose and matching. Track the local map.if(!mbOnlyTracking) // 若不是仅跟踪模式：{ // 若跟踪成功时，则跟综局部地图，寻找更多的匹配，优化当前帧的poseif(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.// mbVO true 表示地图中与 MapPoints 的匹配项很少。 我们无法检索本地地图，因此我们不执行 TrackLocalMap()。 一旦系统重新定位相机，我们将再次使用本地地图。if(bOK && !mbVO) //跟踪成功 且 状态ok(!mbVO)：跟踪localmap（寻找更多的匹配，优化当前帧的pose）bOK = TrackLocalMap();}// 局部地图跟踪成功，mstate =ok。局部地图失败但前面位姿跟踪成功，则imu模式置为recent_lost，纯视觉置为lostif(bOK)mState = OK;else if (mState == OK){if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO){Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2()){cout << "IMU is not or recently initialized. Reseting active map..." << endl;mpSystem->ResetActiveMap();}mState = RECENTLY_LOST;}elsemState = RECENTLY_LOST; // visual to lostif(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)
// 如果最近重新定位，且帧id>上次重置imu，且使用imu，且地图已经初始化imu
if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) && ((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO)) && pCurrentMap->isImuInitialized()){// TODO check this situationVerbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL);Frame* pF = new Frame(mCurrentFrame);pF->mpPrevFrame = new Frame(mLastFrame);// Load preintegrationpF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);}if(pCurrentMap->isImuInitialized()){if(bOK){if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU)){cout << "RESETING FRAME!!!" << endl;}else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))mLastBias = mCurrentFrame.mImuBias;}}#ifdef REGISTER_TIMESstd::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 drawermpFrameDrawer->Update(this);if(!mCurrentFrame.mTcw.empty())mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// 如果跟踪成功或者recent_lost,更新恒速模型，计算上一帧到当前帧的位姿变换if(bOK || mState==RECENTLY_LOST){// Update motion modelif(!mLastFrame.mTcw.empty() && !mCurrentFrame.mTcw.empty()){cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));mVelocity = mCurrentFrame.mTcw*LastTwc;}elsemVelocity = cv::Mat();if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matches 清除恒速跟踪中的临时地图点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 清除观测不到的地图点for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;delete pMP;}mlpTemporalPoints.clear();#ifdef REGISTER_TIMESstd::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 插入关键帧(对双目或rgb-d会产生新的地图点)if(bNeedKF && (bOK|| (mState==RECENTLY_LOST && (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO))))CreateNewKeyFrame();#ifdef REGISTER_TIMESstd::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 trackedfor(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//若：关键帧小于5个，或使用imu且imu未初始化时：系统重置活跃子图//否则 创建新地图if(mState==LOST) {if(pCurrentMap->KeyFramesInMap()<=5){mpSystem->ResetActiveMap();return;}if ((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO))if (!pCurrentMap->isImuInitialized()){Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);mpSystem->ResetActiveMap();return;}CreateMapInAtlas();}if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;mLastFrame = Frame(mCurrentFrame);}// 保存当前关键帧相对参考帧的位姿，参考kf、当前帧时间戳，当前帧状态if(mState==OK || mState==RECENTLY_LOST){// Store frame pose information to retrieve the complete camera trajectory afterwards.if(!mCurrentFrame.mTcw.empty()){cv::Mat Tcr = mCurrentFrame.mTcw*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 lostmlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}}
}

ORB-SLAM3 单目惯导ros-system-track相关推荐

PL-VINS:实时基于点线的单目惯导SLAM系统
摘要: 利用线特征来提高基于点特征的视觉惯性SLAM(VINS)的定位精度越来越重要,因为它们提供了结构化场景中规则性的额外约束,然而,实时性能一直没有得到关注.本文介绍了PL-VINS,一种基于实时 ...
十六.激光和惯导LIO-SLAM框架学习之配置自用传感器实时室外跑LIO-SAM框架
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
PSL‑SLAM:PSL‑SLAM: a monocular SLAM system using points and structure lines in Manh 曼哈顿世界中的点线单目SLAM
文章下载地址:https://download.csdn.net/download/weixin_50508111/85811572 一.文章简介 1. 文章类型/时间/作者/所属单位: 期刊论文/2 ...
Ubuntu下使用单目相机运行ORB SLAM2
环境:Ubuntu16.04+ROS Kinetic+USB单目摄像头虽然ORB SLAM2的官方说明中表示没有ROS也可以编译运行,但要实时的跑ORB SLAM2还是需要ROS平台的,所以之前没有 ...
翻译 | ORB-SLAM: a Versatile and Accurate Monocular SLAM System（ORB-SLAM：一种通用的(全能的)精确的单目SLAM系统）
博主github:https://github.com/MichaelBeechan 博主CSDN:https://blog.csdn.net/u011344545 ORB-SLAM:一种通用的(全能 ...
ROS获取串口信息及后续处理（以惯导IMU XW-GI5651为例）
一.问题简介自动驾驶小车的底层的数据相当一部分是通过串口发送的,以惯导为例,惯导的定位信息大概如下所示: $GPFPD,0,1666.330,0.000,0.015,0.129,0.00000000 ...
Ubuntu20.04 ROS读取basler相机图像步骤。MATLAB2021b标定basler单目相机
运行环境:Ubuntu20.04(64-Bit) ROS版本:Noetic 网卡型号:Realtek RTL 8156外置网卡相机型号:acA 1920-25gc (GigE接口) 驱动版本:pyl ...
ros打开笔记本摄像头,单目标定
笔记本摄像头多是usb 镜头下载包或者sudo apt-get install ros-indigo-usb-cam git clone https://github.com/bosch-ros-p ...
ROS USB摄像头配置（单目和双目）
ROS USB摄像头配置(单目和双目) 原文目前使用ROS和PX4固件控制无人机飞行,现在需要在ROS中加入USB摄像头,并且是两个.目前根据其它博客,以及ROS WIKI,对该流程进行记录参考: ...

ORB-SLAM3 单目惯导ros-system-track

文章目录

ROS

ros_mono_interial.cc

Function

主函数

imu抓取

image 抓取

image与imu同步

ORB_SLAM3::System

Function

构造

TrackMonocular 单目跟踪

TrackStereo

TrackRGBD

Shutdown 关闭

SaveTrajectoryTUM 按TUM保存轨迹

其他数据的保存

Tracking

核心函数

构造

GrabImageMonocular

Track 跟踪

相关函数-地图集

CreateMapInAtlas

ResetActiveMap

相关函数-参数读取

ParseCamParamFile 读取相机文件

ParseORBParamFile 读取orb参数

ParseIMUParamFile 读取imu参数

相关函数-Imu相关

PreintegrateIMU IMU预积分

PredictStateIMU 利用IMU计算位姿

ComputeGyroBias 角度零偏

ComputeVelocitiesAccBias 加速度零偏

相关函数-初始化

MonocularInitialization 单目初始化

CreateInitialMapMonocular 创建初始化单目地图

相关函数-跟踪

TrackWithMotionModel 恒速模型跟踪

UpdateLastFrame 更新上一Frame

TrackReferenceKeyFrame 跟踪参考关键帧

TrackLocalMap 跟踪局部地图

相关函数-关键帧

Relocalization 重定位

NeedNewKeyFrame 关键帧选取策略

CreateNewKeyFrame 创建新的关键帧

ORB-SLAM3 单目惯导ros-system-track相关推荐

最新文章

热门文章