12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

添加IMU注释内容

master
ZhouJiafen 2020-12-28 19:03:58 -08:00
parent 62da5259f8
commit b9823b7892
10 changed files with 156 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@ -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)

14
Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
View File

@ -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<num_seq; seq++)
{
// Main loop
cv::Mat im;
//存放imu数据容器,包含该加速度,角速度,时间戳
vector<ORB_SLAM3::IMU::Point> vImuMeas;
proccIm = 0;
//直方图均衡化,直方图均衡化的思想就是这样的:
//假设我有灰度级255的图像，但是都是属于［100，110］的灰度，图像对比度就很低，我应该尽可能拉到整个［0，255］
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
{
// Read image from file
// Step 5 读取每一帧图像并转换为灰度图存储在im
// Step 5 读取每一帧图像并转换为灰度图存储在im,seq表示第几个数据集,ni表示这个数据集的第几个数据
im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_GRAYSCALE);
// clahe
// ?这里是什么作用？
//直方图均衡化
clahe->apply(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,

4
build.sh
View File

@ -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

2
include/Map.h
View File

@ -193,7 +193,7 @@ protected:
bool mHasTumbnail;
bool mbBad = false;
bool mbIsInertial;
bool mbIsInertial; //标记是否有imu
bool mbIMU_BA1;
bool mbIMU_BA2;

11
include/System.h
View File

@ -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.

1
src/Atlas.cc
View File

@ -244,6 +244,7 @@ bool Atlas::isInertial()
void Atlas::SetInertialSensor()
{
unique_lock<mutex> lock(mMutexAtlas);
//接着又调用Map类的SetInertialSensor成员函数,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
mpCurrentMap->SetInertialSensor();
}

47
src/ImuTypes.cc
View File

@ -149,30 +149,44 @@ cv::Mat InverseRightJacobianSO3(const cv::Mat &v)
return InverseRightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(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_<float>(3,3) << 0, -z, y,
z, 0, -x,
-y, x, 0);
// eps = 1e-4 是一个小量，根据罗德里格斯公式求极限，后面的高阶小量忽略掉得到此式
if(d<eps)
{
//forster 经典预积分论文公式（4）
deltaR = I + W;
//小量时，右扰动 Jr = I
rightJ = cv::Mat::eye(3,3,CV_32F);
}
else
{
//forster 经典预积分论文公式（3）
deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
//forster 经典预积分论文公式（8）
rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
}
}
@ -252,8 +266,16 @@ void Preintegrated::Reintegrate()
IntegrateNewMeasurement(aux[i].a,aux[i].w,aux[i].t);
}
/**
* @brief 预积分计算，更新noise
*
* @param[in] acceleration 加速度计数据
* @param[in] angVel 陀螺仪数据
* @param[in] dt 两帧之间时间差
*/
void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt)
{
// 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中
mvMeasurements.push_back(integrable(acceleration,angVel,dt));
// Position is updated firstly, as it depends on previously computed velocity and rotation.
@ -261,20 +283,28 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con
// Rotation is the last to be updated.
//Matrices to compute covariance
// Step 1.构造协方差矩阵 参考Forster论文公式（62），邱笑晨的《预积分总结与公式推导》的P12页也有详细推导:η_ij = A * η_i,j-1 + B_j-1 * η_j-1
// ? 位姿第一个被更新，速度第二（因为这两个只依赖前一帧计算的旋转矩阵和速度），后面再更新旋转角度
// 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差
cv::Mat A = cv::Mat::eye(9,9,CV_32F);
// 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用
cv::Mat B = cv::Mat::zeros(9,6,CV_32F);
// 考虑偏置后的加速度、角速度
cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
cv::Mat accW = (cv::Mat_<float>(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_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
acc.at<float>(2), 0, -acc.at<float>(0),
-acc.at<float>(1), acc.at<float>(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;
}

1
src/Map.cc
View File

@ -342,6 +342,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale
void Map::SetInertialSensor()
{
unique_lock<mutex> lock(mMutexMap);
//将mbIsInertial设置为true,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
mbIsInertial = true;
}

37
src/System.cc
View File

@ -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<Viewer*>(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<Viewer*>(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 &timestamp, const
// Check mode change
{
// 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
unique_lock<mutex> lock(mMutexMode);
// mbActivateLocalizationMode为true会关闭局部地图线程，仅跟踪模式
if(mbActivateLocalizationMode)
@ -394,6 +407,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
}
// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
mpTracker->InformOnlyTracking(true);
// 关闭线程可以使得别的线程得到更多的资源
mbActivateLocalizationMode = false;
}
if(mbDeactivateLocalizationMode)
@ -413,6 +427,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const
mbReset = false;
mbResetActiveMap = false;
}
//如果检测到重置活动地图,讲重置地图设置
else if(mbResetActiveMap)
{
cout << "SYSTEM-> Reseting active map in monocular case" << endl;

66
src/Tracking.cc
View File

@ -1073,7 +1073,8 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp,
}
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 &timestamp,
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 &timestamp,
return mCurrentFrame.mTcw.clone();
}
//将imu数据存放在mlQueueImuData的list链表里
void Tracking::GrabImuData(const IMU::Point &imuMeasurement)
{
unique_lock<mutex> 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<mutex> lock(mMutexImuQueue);
if(!mlQueueImuData.empty())
{
// 拿到第一个imu数据作为起始数据
IMU::Point* m = &mlQueueImuData.front();
cout.precision(17);
// imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据
if(m->t<mCurrentFrame.mpPrevFrame->mTimeStamp-0.001l)
{
mlQueueImuData.pop_front();
}
// 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001
// ? (时间戳本身的差值,单位是s秒,tum数据集里面是19位时间戳,单位为ns,这里可能只保留了10位整数和9位小数)
else if(m->t<mCurrentFrame.mTimeStamp-0.001l)
{
mvImuFromLastFrame.push_back(*m);
@ -1167,6 +1177,7 @@ void Tracking::PreintegrateIMU()
}
else
{
// 得到两帧间的imu数据放入mvImuFromLastFrame中,得到后面预积分的处理数据
mvImuFromLastFrame.push_back(*m);
break;
}
@ -1181,32 +1192,55 @@ void Tracking::PreintegrateIMU()
usleep(500);
}
// Step 2.对两帧之间进行中值积分处理
// m个imu组数据会有m-1个预积分量
const int n = mvImuFromLastFrame.size()-1;
// 构造imu预处理器,并初始化标定数据
IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
// 针对预积分位置的不同做不同中值积分的处理
/**
* 根据上面imu帧的筛选，IMU与图像帧的时序如下：
* Frame---IMU0---IMU1---IMU2---IMU3---IMU4---------------IMUx---Frame---IMUx+1
* T_------T0-----T1-----T2-----T3-----T4-----------------Tx-----_T------Tx+1
* A_------A0-----A1-----A2-----A3-----A4-----------------Ax-----_T------Ax+1
* W_------W0-----W1-----W2-----W3-----W4-----------------Wx-----_T------Wx+1
* T_和_T分别表示上一图像帧和当前图像帧的时间戳，A(加速度数据)，W(陀螺仪数据)，同理
*/
for(int i=0; i<n; i++)
{
float tstep;
cv::Point3f acc, angVel;
// 第一帧数据但不是最后两帧,imu总帧数大于2
if((i==0) && (i<(n-1)))
{
// 时间差作为积分量
float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;
// 根据上面分析的IMU与图像帧的时序分析，获取当前imu到上一帧的时间间隔
// 在加加速度不变的假设上算出第一帧图像到第二个imu数据的中值积分
float tini = mvImuFromLastFrame[i].t-mCurrentFrame.mpPrevFrame->mTimeStamp;
// ? 这里采用离散中值积分进行预积分,获取当前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