From b9823b78927e1f2349be4b8d9ed91618ec7a2612 Mon Sep 17 00:00:00 2001 From: ZhouJiafen Date: Mon, 28 Dec 2020 19:03:58 -0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0IMU=E6=B3=A8=E9=87=8A?= =?UTF-8?q?=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 10 +-- .../mono_inertial_tum_vi.cc | 14 ++-- build.sh | 4 +- include/Map.h | 2 +- include/System.h | 11 ++++ src/Atlas.cc | 1 + src/ImuTypes.cc | 47 +++++++++++-- src/Map.cc | 1 + src/System.cc | 37 +++++++---- src/Tracking.cc | 66 ++++++++++++++++--- 10 files changed, 156 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f7c65b..67c356c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,16 +4,16 @@ project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) ENDIF() - +# set(CMAKE_BUILD_TYPE "Debug") MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") -set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") +# set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") +# set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native") # Check C++11 or C++0x support include(CheckCXXCompilerFlag) diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc index 0a82406..3193c12 100644 --- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc +++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc @@ -103,8 +103,10 @@ int main(int argc, char **argv) // Find first imu to be considered, supposing imu measurements start first // Step 3 默认IMU数据早于图像数据记录，找到和第一帧图像时间戳最接近的imu时间戳索引，记录在first_imu[seq]中 - while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0]) + while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0]){ first_imu[seq]++; + cout << "first_imu[seq] = " << first_imu[seq] << endl; + } // 因为上面退出while循环时IMU时间戳刚刚超过图像时间戳，所以这里需要再减一个索引 first_imu[seq]--; // first imu measurement to be considered @@ -125,22 +127,26 @@ int main(int argc, char **argv) // Step 4 SLAM系统的初始化，包括读取配置文件、字典，创建跟踪、局部建图、闭环、显示线程 ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name); + //遍历所有数据 int proccIm = 0; for (seq = 0; seq vImuMeas; proccIm = 0; + //直方图均衡化,直方图均衡化的思想就是这样的: + //假设我有灰度级255的图像，但是都是属于［100，110］的灰度，图像对比度就很低，我应该尽可能拉到整个［0，255］ cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); for(int ni=0; niapply(im,im); @@ -163,7 +169,7 @@ int main(int argc, char **argv) { // cout << "t_cam " << tframe << endl; // Step 6 把上一图像帧和当前图像帧之间的imu信息存储在vImuMeas里 - // 注意第一个图像帧没有对应的imu数据 + // 注意第一个图像帧没有对应的imu数据 //?是否存在一帧,因为之前是从最接近图像第一帧的imu算起,可能无效 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) { vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z, diff --git a/build.sh b/build.sh index 9aadf59..a70a7f1 100644 --- a/build.sh +++ b/build.sh @@ -27,5 +27,5 @@ echo "Configuring and building ORB_SLAM3 ..." mkdir build cd build -cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +cmake .. -DCMAKE_BUILD_TYPE=Debug +make diff --git a/include/Map.h b/include/Map.h index 00396ba..4c4314d 100644 --- a/include/Map.h +++ b/include/Map.h @@ -193,7 +193,7 @@ protected: bool mHasTumbnail; bool mbBad = false; - bool mbIsInertial; + bool mbIsInertial; //标记是否有imu bool mbIMU_BA1; bool mbIMU_BA2; diff --git a/include/System.h b/include/System.h index a4becb6..41a150b 100644 --- a/include/System.h +++ b/include/System.h @@ -99,6 +99,17 @@ public: public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. + /** + * @brief Construct a new System object + * + * @param[in] strVocFile //词袋文件所在路径 + * @param[in] strSettingsFile //配置文件所在路径 + * @param[in] sensor //传感器类型 + * @param[in] bUseViewer //指定是否使用可视化界面 + * @param[in] initFr //initFr表示初始化帧的id,开始设置为0 + * @param[out] strSequence //序列名,在跟踪线程和局部建图线程用得到 + * @param[in] strLoadingFile //看起来作者貌似想加地图重载功能的一个参数 + */ System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string(), const string &strLoadingFile = std::string()); // Proccess the given stereo frame. Images must be synchronized and rectified. diff --git a/src/Atlas.cc b/src/Atlas.cc index a9c6334..6b131af 100644 --- a/src/Atlas.cc +++ b/src/Atlas.cc @@ -244,6 +244,7 @@ bool Atlas::isInertial() void Atlas::SetInertialSensor() { unique_lock lock(mMutexAtlas); + //接着又调用Map类的SetInertialSensor成员函数,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关 mpCurrentMap->SetInertialSensor(); } diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index 9e7bf16..84336ac 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -149,30 +149,44 @@ cv::Mat InverseRightJacobianSO3(const cv::Mat &v) return InverseRightJacobianSO3(v.at(0),v.at(1),v.at(2)); } - +/** + * @brief 计算旋转角度积分量 + * + * @param[in] angVel 陀螺仪数据 + * @param[in] imuBias 陀螺仪偏置 + * @param[in] time 两帧间的时间差 + */ IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time): deltaT(time) { + //得到考虑偏置后的角度旋转 const float x = (angVel.x-imuBias.bwx)*time; const float y = (angVel.y-imuBias.bwy)*time; const float z = (angVel.z-imuBias.bwz)*time; cv::Mat I = cv::Mat::eye(3,3,CV_32F); + //计算旋转矩阵的模值，后面用罗德里格公式计算旋转矩阵时会用到 const float d2 = x*x+y*y+z*z; const float d = sqrt(d2); + //角度转成叉积的矩阵形式 cv::Mat W = (cv::Mat_(3,3) << 0, -z, y, z, 0, -x, -y, x, 0); + // eps = 1e-4 是一个小量，根据罗德里格斯公式求极限，后面的高阶小量忽略掉得到此式 if(d(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz); cv::Mat accW = (cv::Mat_(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz); + // 记录平均加速度和角速度 avgA = (dT*avgA + dR*acc*dt)/(dT+dt); avgW = (dT*avgW + accW*dt)/(dT+dt); - + // Update delta position dP and velocity dV (rely on no-updated delta rotation) + // 根据没有更新的dR来更新dP与dV eq.(38) dP = dP + dV*dt + 0.5f*dR*acc*dt*dt; dV = dV + dR*acc*dt; // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) + // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新 cv::Mat Wacc = (cv::Mat_(3,3) << 0, -acc.at(2), acc.at(1), acc.at(2), 0, -acc.at(0), -acc.at(1), acc.at(0), 0); @@ -285,27 +315,36 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt; // Update position and velocity jacobians wrt bias correction + // ? 更新bias雅克比,计算偏置的雅克比矩阵，pv 分别对ba与bg的偏导数,论文中没推这个值，邱笑晨那边有推导 + // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同 JPa = JPa + JVa*dt -0.5f*dR*dt*dt; JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg; JVa = JVa - dR*dt; JVg = JVg - dR*dt*Wacc*JRg; // Update delta rotation + // Step 2. 构造函数，会根据更新后的bias进行角度积分 IntegratedRotation dRi(angVel,b,dt); + // 强行归一化使其符合旋转矩阵的格式 dR = NormalizeRotation(dR*dRi.deltaR); // Compute rotation parts of matrices A and B + // 补充AB矩阵 A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t(); B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt; - + // 小量delta初始为0，更新后通常也为0，故省略了小量的更新 // Update covariance + // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响 C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t(); + // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵 C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk; // Update rotation jacobian wrt bias correction + // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt; // Total integrated time + // 更新总时间 dT += dt; } diff --git a/src/Map.cc b/src/Map.cc index c542d48..7d3738b 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -342,6 +342,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale void Map::SetInertialSensor() { unique_lock lock(mMutexMap); + //将mbIsInertial设置为true,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关 mbIsInertial = true; } diff --git a/src/System.cc b/src/System.cc index d3f7b61..8cac461 100644 --- a/src/System.cc +++ b/src/System.cc @@ -37,11 +37,20 @@ namespace ORB_SLAM3 { Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL; - -System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, - const bool bUseViewer, const int initFr, const string &strSequence, const string &strLoadingFile): - mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbResetActiveMap(false), - mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) +// 系统的构造函数，将会启动其他的线程 +System::System(const string &strVocFile, //词袋文件所在路径 + const string &strSettingsFile, //配置文件所在路径 + const eSensor sensor, //传感器类型 + const bool bUseViewer, //是否使用可视化界面 + const int initFr, //initFr表示初始化帧的id,开始设置为0 + const string &strSequence, //序列名,在跟踪线程和局部建图线程用得到 + const string &strLoadingFile //看起来作者貌似想加地图重载功能的一个参数 + ): + mSensor(sensor), //初始化传感器类型 + mpViewer(static_cast(NULL)), // ?空。。。对象指针？ TODO + mbReset(false), mbResetActiveMap(false),// ?重新设置ActiveMap + mbActivateLocalizationMode(false), // ?是否开启局部定位功能开关 + mbDeactivateLocalizationMode(false) // ?没有这个模式转换标志 { // Output welcome message cout << endl << @@ -54,15 +63,15 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cout << "Input sensor was set to: "; // Step 1 输出当前传感器类型 if(mSensor==MONOCULAR) - cout << "Monocular" << endl; + cout << "Monocular" << endl; //单目 else if(mSensor==STEREO) - cout << "Stereo" << endl; + cout << "Stereo" << endl; //双目 else if(mSensor==RGBD) - cout << "RGB-D" << endl; + cout << "RGB-D" << endl; //RGBD相机 else if(mSensor==IMU_MONOCULAR) - cout << "Monocular-Inertial" << endl; + cout << "Monocular-Inertial" << endl; //单目 + imu else if(mSensor==IMU_STEREO) - cout << "Stereo-Inertial" << endl; + cout << "Stereo-Inertial" << endl; //双目 + imu //Check settings file // Step 2 读取配置文件 @@ -75,6 +84,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS exit(-1); } + // ?ORBSLAM3新加的多地图管理功能，这里好像是加载Atlas标识符 bool loadedAtlas = false; //---- @@ -172,6 +182,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS // 设置Atlas中的传感器类型 if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR) + // ? 如果是有imu的传感器类型，将mbIsInertial设置为imu属性,以后的跟踪和预积分将和这个标志有关 mpAtlas->SetInertialSensor(); // Step 6 依次创建跟踪、局部建图、闭环、显示线程 @@ -182,7 +193,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) - // 创建跟踪线程（主线程） + // 创建跟踪线程（主线程）,不会立刻开启,会在对图像和imu预处理后在main主线程种执行 cout << "Seq. Name: " << strSequence << endl; mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence); @@ -196,6 +207,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpLocalMapper->mInitFr = initFr; //设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃 mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"]; + // ? 这里有个疑问,C++中浮点型跟0比较是否用精确? if(mpLocalMapper->mThFarPoints!=0) { cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl; @@ -381,6 +393,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp, const // Check mode change { + // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱 unique_lock lock(mMutexMode); // mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式 if(mbActivateLocalizationMode) @@ -394,6 +407,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp, const } // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新 mpTracker->InformOnlyTracking(true); + // 关闭线程可以使得别的线程得到更多的资源 mbActivateLocalizationMode = false; } if(mbDeactivateLocalizationMode) @@ -413,6 +427,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp, const mbReset = false; mbResetActiveMap = false; } + //如果检测到重置活动地图,讲重置地图设置 else if(mbResetActiveMap) { cout << "SYSTEM-> Reseting active map in monocular case" << endl; diff --git a/src/Tracking.cc b/src/Tracking.cc index bf0abc5..ee0dc2a 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -1073,7 +1073,8 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, } else if(mSensor == System::IMU_MONOCULAR) { - if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) + //判断该帧是不是初始化 + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET { mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib); } @@ -1104,6 +1105,7 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, f_track_stats << mvpLocalMapPoints.size() << ","; f_track_stats << setprecision(6) << t_track << endl;*/ +//开启保存数据模式 #ifdef SAVE_TIMES f_track_times << mCurrentFrame.mTimeORB_Ext << ","; f_track_times << mCurrentFrame.mTimeStereoMatch << ","; @@ -1118,7 +1120,7 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, return mCurrentFrame.mTcw.clone(); } - +//将imu数据存放在mlQueueImuData的list链表里 void Tracking::GrabImuData(const IMU::Point &imuMeasurement) { unique_lock lock(mMutexImuQueue); @@ -1127,11 +1129,13 @@ void Tracking::GrabImuData(const IMU::Point &imuMeasurement) void Tracking::PreintegrateIMU() { - //cout << "start preintegration" << endl; - + // Step 1.拿到两两帧之间待处理的预积分数据，组成一个集合 + // cout << "start preintegration" << endl; + // 上一帧不存在,说明两帧之间没有imu数据，不进行预积分 if(!mCurrentFrame.mpPrevFrame) { Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL); + // ? 当前帧是否进行预积分标志 mCurrentFrame.setIntegrated(); return; } @@ -1140,6 +1144,7 @@ void Tracking::PreintegrateIMU() mvImuFromLastFrame.clear(); mvImuFromLastFrame.reserve(mlQueueImuData.size()); + // 没有imu数据,不进行预积分 if(mlQueueImuData.size() == 0) { Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL); @@ -1149,17 +1154,22 @@ void Tracking::PreintegrateIMU() while(true) { + // 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待 bool bSleep = false; { unique_lock lock(mMutexImuQueue); if(!mlQueueImuData.empty()) { + // 拿到第一个imu数据作为起始数据 IMU::Point* m = &mlQueueImuData.front(); cout.precision(17); + // imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据 if(m->tmTimeStamp-0.001l) { mlQueueImuData.pop_front(); } + // 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001 + // ? (时间戳本身的差值,单位是s秒,tum数据集里面是19位时间戳,单位为ns,这里可能只保留了10位整数和9位小数) else if(m->tmTimeStamp; + // ? 这里采用离散中值积分进行预积分,获取当前imu到上一帧的时间间隔 + // 设当前时刻imu的加速度a0，下一时刻加速度a1，时间间隔tab 为t10，tini t0p + // 正常情况下时为了求上一帧到当前时刻imu的一个平均加速度，但是imu时间不会正好落在上一帧图像的时刻，需要做补偿，要求得a0时刻到上一帧这段时间加速度的改变量 + // 有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)*(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值 + // 其中tstep表示a1到上一帧的时间间隔，a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后，(a1-a0)也是一样，多种情况下这个式子依然成立 acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a- (mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tini/tab))*0.5f; angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w- (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tini/tab))*0.5f; + // 上一帧图像到下一帧imu（也就是第二个imu数据）的时间差 tstep = mvImuFromLastFrame[i+1].t-mCurrentFrame.mpPrevFrame->mTimeStamp; } + //中间帧直接求中值 else if(i<(n-1)) { acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a)*0.5f; angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w)*0.5f; tstep = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t; } + //最后一帧,但不是第一帧 else if((i>0) && (i==(n-1))) { + // 直到倒数第二个imu时刻时，计算过程跟第一时刻类似，都需要考虑帧与imu时刻的关系 float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t; float tend = mvImuFromLastFrame[i+1].t-mCurrentFrame.mTimeStamp; acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a- @@ -1215,23 +1249,28 @@ void Tracking::PreintegrateIMU() (mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tend/tab))*0.5f; tstep = mCurrentFrame.mTimeStamp-mvImuFromLastFrame[i].t; } + //只有两帧的情况 else if((i==0) && (i==(n-1))) { + // ? 就两个数据时使用第一个时刻的，之前size减一了，最后一帧imu好像没遍历，这时候应该还有一帧在后面，但这里直接没有算中值积分？ acc = mvImuFromLastFrame[i].a; angVel = mvImuFromLastFrame[i].w; tstep = mCurrentFrame.mTimeStamp-mCurrentFrame.mpPrevFrame->mTimeStamp; } - + // Step 3.依次进行预积分计算 + // 应该是必存在的吧，一个是相对上一关键帧，一个是相对上一帧 if (!mpImuPreintegratedFromLastKF) cout << "mpImuPreintegratedFromLastKF does not exist" << endl; mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep); pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep); } + // 记录当前预积分的图像帧 mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame; mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame; + // ? 设置为已预积分状态 mCurrentFrame.setIntegrated(); Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG); @@ -1399,7 +1438,11 @@ void Tracking::ResetFrameIMU() /** * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪 + * track包含两部分：估计运动、跟踪局部地图 * + * Step 1：初始化 + * Step 2：跟踪 + * Step 3：记录位姿信息，用于轨迹复现 */ void Tracking::Track() { @@ -1409,7 +1452,7 @@ void Tracking::Track() mTime_LocalMapTrack = 0; mTime_NewKF_Dec = 0; #endif - + // ? 还不清楚这个标识符,跟绘图相关? if (bStepByStep) { while(!mbStep) @@ -1445,12 +1488,14 @@ void Tracking::Track() { // 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回 cout << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl; + //根据是否是imu模式,进行imu的补偿 if(mpAtlas->isInertial()) { // 如果当前地图imu成功初始化 if(mpAtlas->isImuInitialized()) { cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl; + // ? BA2标志代表什么?,BA优化成功? if(!pCurrentMap->GetIniertialBA2()) { // 如果当前子图中imu没有经过BA2，重置active地图 @@ -1474,8 +1519,9 @@ void Tracking::Track() } } - // Step 3 IMU模式下设置IMU的Bias参数 + // Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在非空 if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame) + //认为bias在两帧间不变 mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias()); if(mState==NO_IMAGES_YET) @@ -1484,7 +1530,7 @@ void Tracking::Track() } mLastProcessedState=mState; - // Step 4 IMU模式下对IMU数据进行预积分 + // Step 4 IMU模式下对IMU数据进行预积分 -> // ? 没有创建地图的情况下 if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap) { #ifdef SAVE_TIMES