github上关于Learn_VIORB的一些问答整理

一.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/76
问：
如何边缘化在LocalBAPRVIDP()中丢弃的关键帧
答：在VI-ORB-SLAM论文中，边缘化仅在优化当前框架时应用(通过预集成IMU测量与前一个关键框架链接，并受当前边缘化先验的约束)。
没有信息真正丢失，因为在优化中包含了以前关键帧的窗口作为固定约束。然而，与滑动窗口/边缘化方法(如VINS-Mono)相比，固定关键帧的不确定性信息丢失，可能导致优化过程中的过度自信，但也保证了更快的收敛速度和较小的漂移。
问： 非常感谢，看源代码发现边缘化只有TrackLocalMapWithIMU();
我可以理解前面已经被边缘化了，所以滑动窗口不需要被边缘化，因为滑动窗口中的框架信息已经完整了。

二。

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/74
问：
我最近在学习你的代码，我有一个关于PoseOptimization函数的问题，我看到在PoseOptimization()函数的参数是正常的。
例如“const float thHuberNavStatePVR = sqrt(21.666);”
但是在LocalBAPRVIDP()和GlobalBundleAdjustmentNavStatePRV()函数中，参数被改变了。
例如“const float thHuberNavStatePRV = sqrt(100*21.666);”
如何确定Huber的值?
答： Huber值取自chi2表，使用测量自由度的数量和内嵌概率(这里21.66的值是9个自由度，来自代表位置、旋转、速度的9D向量，内嵌概率为1%)。

然而，对我来说，在IMU测量上使用一个健壮的成本函数是没有意义的，因为它们确实遵循高斯分布。Huber的使用更适合于处理视觉测量，因为它们中的一些可能来自于完全打破高斯分布假设的异常值，因此，如果不使用鲁棒代价函数，将导致非常糟糕的优化。

对于GlobalBundleAdjustmentNavStatePRV()中使用的奇怪的100因子，我的猜测是，当保持21.666值时，会得到不好的结果，而当添加100因子时，会得到更好的结果。然而，乘以100最有可能消除Huber成本的影响，因为IMU测量误差不太可能超过Huber阈值。
问： 谢谢你的回复，我同意你的观点。当我检查imu图边缘的信息矩阵时，我发现它与视觉的图边缘相比是非常巨大的，这意味着测量误差很容易超过Huber阈值。我猜作者@jingpang更信任imu，所以他使用了奇怪的100因子。

三.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/70
问： optimizer.computeMarginals()的结果，是关于估计姿态的协方差矩阵吗?在LearnVIORB / src / Optimizer.cc的3745行

// bool flag = optimizer.computeMarginals(spinv,optimizer.vertex(0));

答： 函数computeMarginals()返回与状态顶点(0)相关的Hessian矩阵，您必须对它进行反求才能得到协方差。

问：
（1）我试着写这个代码:

g2o::SparseBlockMatrixXd spinv; 
optimizer.computeMarginals(spinv, optimizer.vertex(0)); Eigen::Matrix<double, 6, 6> margCovInv = Eigen::Matrix<double, 6, 6>::Zero(); 
margCovInv.topLeftCorner(6,6) = spinv.block(0, 0)->inverse(); cout<<"Matrix covariance:\n"<<margCovInv<<endl;

但矩阵似乎是不正确的(值太高?)
（2）：这个协方差矩阵只代表最后一个姿态，还是整个轨迹?
答：
1)嗯，Hessian矩阵的值来自于这个表达式:J’W；
J，其中J是优化问题的雅可比矩阵W是信息矩阵(给每一个测量的协方差的倒数)。
由此产生的海森矩阵取决于你的优化问题有定义。事实上，状态顶点(0)值很高意味着您对估计参数有很高的信心(如果您求这个海森矩阵的逆就得到相关的协方差矩阵估计的状态)。
2)我认为在g2o中，computeMarginals()函数只是提供了块与您所需要的状态相关联(但是记得不太清楚了，你应该检查一下)。如果确实是这样做的然后你得到的矩阵对应于你的置信度顶点(0)根据你定义的优化问题。因此,如果你已经执行了一个全局包调整，如优化，则对应的信息矩阵整个轨迹的states vertex(0) 。
问：
（原文:the matrix figure represent covariance not hessian, but is the pose coordinate uncertainty in meters?）
计算出来的矩阵表示协方差不是hessian，但姿势坐标的不确定性在米?
答：
（原文：yes you’re right, my bad I read it too fast. In this case @Andrea485 , the uncertainty values of your covariance matrix indeed seems very high. The uncertainty around the translation parts of the matrix are in squared meters if the transnational parts of the states are expressed in meters (it should be the case with a Visual-Inertial method but not with a monocular SLAM)）
是的，你说得对，是我的错，我读得太快了。在这种情况下，你的协方差矩阵的不确定性值似乎非常高。如果变化部分的状态矩阵以米表示，则矩阵平移部分的不确定性以平方米表示(应该是视觉惯性方法，而不是单目SLAM)
问：
what’s wrong in my code, what can I do?
答：
你用来计算协方差矩阵的代码没有任何错误。但你在做什么?VI-Odometry吗?如果是这样，Bundle Adjustment就像优化给你带来了好的结果吗?你也可以看看你设置的边缘信息。
由于Hessian矩阵是由雅可比矩阵和信息矩阵导出的，高的协方差来自于高的雅可比矩阵值。也许你也应该验证一下雅可比矩阵(你可以测试一下自动微分)

四.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/66
问：
我在一些数据集中成功地运行ORB_SLAM2，然而，当我使用与视觉部分完全相同的参数LearnVIORB时，轨道在我尝试的两个序列中丢失。在ORB_SLAM2中成功实现了跟踪。
有什么建议吗?

答：
尝试从kalibr和imu_utils更新您的D435i Tbc参数。
问： 你对具体的回购有什么建议吗?因为我检查了一些，没有一个声称修复了与实际性能相关的bug，同时也支持ROS。
**其他：**我已经更新Tbc and imu hz, but i can not get good results

五.

地址：
问： 大家好，我试图在自定义数据集上运行LearnVIORB，我得到了负值。有人能给我解释一下可能的原因吗?我的scale.txt有以下缩放值:

1531449498.440038 -2.934031 -2.914245
1531449498.607197 -2.723707 -2.395356
1531449498.773323 -1.735127 -1.396313
1531449498.939945 -1.374596 -0.992191
1531449499.073253 -1.253044 -0.801955
1531449499.240143 -1.300863 -0.900277
1531449499.406934 -1.493881 -0.874370
1531449499.573221 -1.449506 -0.837931
1531449499.706542 -0.842949 0.108764
1531449499.840175 0.289597 1.372092
1531449500.006769 0.472643 1.565625

刻度值在+8到-8之间波动。我的gw.txt文件是这样的：

1531449494.840364 3.107167 4.403760 -8.196860 -2.849978 -7.442929 5.719837
1531449494.973254 -2.447847 -9.476461 0.663958 -2.974932 -7.244322 5.908104
1531449495.106953 -1.316977 -6.593646 -7.143215 -2.894555 -7.337679 5.832334
1531449495.240191 -1.543059 -4.121672 8.767377 -2.853355 -7.265836 5.941557

看来他们俩都错了。任何提示和指示将非常感谢。谢谢你！

没有回答，难过！！！！！！！

六.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/54
**问：**在forster 15’RSS中（原话：forster 15’RSS,According to the "lift-solve-retract），根据“提升-解决-撤回”（原话：“lift-solve-retract”）方法，位置的增量是pi = pi+Ri*dpi等等，但在你的代码中是pi = pi+dpi
是我误解了你的论文，还是你的代码和公式不一样?
答： 用不同的残差项来计算雅可比矩阵。雅可比矩阵是不同的，更新使用相同的不同项。两者都是对的

七.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/47
问：
我在EuRoC MH_01_easy数据集上运行代码，得到了最终的值（原话：final scale of value）:5.899714。这是MH_01_easy数据集的真实规模吗?然后我想用一些cout语句跟踪主方法的程序流，看起来代码永远不会走出初始化状态到trackingwithimu阶段。下面是初始和最终输出:

Load Image: 3339
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Assigning features to grid
Inside Tracking::Track()
mState: 2
System initialized
Inside !mbOnlyTracking
Inside mState=ok and TRACK_WITH_IMU
!(Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
Inside Tracking::TrackWithMotionModel
Inlier: 382, vDepth: 382, vFeat: 382, matches: 382
This frame has matches: 252
Inside Optimizer::PoseOptimization(Frame *pFrame)
TrackWithMotionModel nmatchesMap : 197
bOK at 6E: 1
Local Map tracking
Inside Tracking::TrackLocalMap
dist = 0.963451
Trying to init VIO
Inside LocalMapping::TryInitVIO
dt = 0.000, this vs next time: 1403636580.264 vs 1403636580.264
dt = 0.000, this vs next time: 1403636580.714 vs 1403636580.714
dt = 0.000, this vs next time: 1403636581.164 vs 1403636581.164

因此，即使在加载了3339张图像之后，系统仍然处于试图初始化vio模式。但是gw的价值似乎是9.7，看起来不错。我还能得到陀螺仪和acc偏差。这是否意味着代码没有IMU初始化的终止条件?谢谢你的回复。
答： 5.899714是正常的。我在EuRoC MH_01_easy数据集上运行了这个项目，得到了大约1到6的比例因子。轨迹结果与数据集的地面真实值比较表明，我们得到的结果误差在4-7 cm左右。
比较了168对绝对姿势计算平移部分构成关系的APE…
平移部分(m)（原话：APE w.r.t. translation part (m)）

  max	0.144626
  mean	0.067789
median	0.074708
   min	0.000399
  rmse	0.076236
   sse	0.976393
   std	0.034878

result of tested 4 times on MH_01_easy dataset
Time: 15.4, sstar: 1.13853, s: 1.14564 -> rmse 0.076236
Time: 15.2, sstar: 4.59188, s: 4.60311 -> rmse 0.060084
Time: 15.4, sstar: 5.18981, s: 5.21842 -> rmse 0.047713
Time: 15.15, sstar: 6.36775, s: 6.40302 -> rmse 0.070640

望这能回答你的一些问题。

八.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/45
问： scale.txt file as follows:

cout<<"Time: "<<mpCurrentKeyFrame->mTimeStamp - mnStartTime<<", 
sstar: "<<sstar<<", s: "<<s_<<endl;
1403636580.963556 5.404152 5.401110
1403636581.113555 5.342806 5.317275
1403636581.263556 5.383121 5.398332
1403636581.413555 5.439227 5.319515
1403636581.563555 5.535811 5.424372

我想第一个是关键帧的时间戳，之后的两个值是什么。我从代码中发现第一个值是s_，第二个值是sstar，但是无法理解它们之间的含义和区别。比例是否随每个关键帧而改变，或者在我们从某些关键帧初始化比例后它是不变的。任何帮助将不胜感激。谢谢你！
答： 我认为sstar代表最初计算的尺度没有考虑重力大小作为先验;s_代表优化尺度，重力大小作为先验约束。
问： 我总共得到了574个比例值，这意味着不同的关键帧比例是不同的吗?如果是这样的话，我怎么把这个比例应用到最终的地图上呢?我的意思是，我应该迭代时间戳，并将translation_x, translation_y和translation_z与最终地图的对应比例值相乘，还是比例是一个单独的值，我可以乘以整个地图。
答：
就我目前所知，是的，缩放是在地图更新之前对每个关键帧进行更新的，并且只使用最后一个关键帧来更新地图(整个过程中只更新一次)。猜测您将VINSInitTime值固定为无穷大的数字，如99999999，以强制进程出于某种原因不更新映射。要将比例应用到最终的映射，可以尝试在局部映射的Tryinitvio函数的最后调用if(bVIOInited)部分（原话：To apply the scale to final map you can try calling section under if(bVIOInited) in function TryInitVIO in localmapping.cc at the end.）

九.关于一个报错

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/43
问：

答： open Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h，change

typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic,SparseMatrix::Index> PermutationMatrix;

to

typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic,int> PermutationMatrix;

十.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/37
问：
我自己的设备刚开始运行时动作很慢，VIORB总是死机。
在LocalMapping线程里，函数**AddToLocalWindow(KeyFrame* pKF)*在往mlLocalKeyFrames里添加新的关键帧时候，有种情况没有考虑到，会导致系统直接报错退出。就是在系统刚运行的时候，由于相机移动缓慢且视觉变化不大，在LocalMapping线程KeyFrameCulling()中会剔除冗余的关键帧，那么伴随着函数DeleteBadInLocalWindow()，mlLocalKeyFrames里的关键帧也会被剔除，这样会有很大的几率导致mlLocalKeyFrames里面的关键帧从一个接近窗口大小或等于窗口大小的数量在一次关键帧剔除后减少到只剩几帧，于是在新的一次AddToLocalWindow(KeyFrame pKF)时，会从LocalMapping.cc的864行开始执行，也就是循环地向mlLocalKeyFrames还剩的关键帧的最前的关键帧那帧的前面，插入它前一帧关键帧(有点绕…)，直到数量超过窗口阈值或者是前一帧是空的(NULL)，注意，这个条件是允许把系统最最开始的第一帧插进去的，在这种情况下，mlLocalKeyFrames的最前一帧关键帧是整个系统的第一帧关键帧。那么，问题来了，在局部优化的时候，是需要取mlLocalKeyFrames最前一帧关键帧的前一关键帧作为顶点添加IMU相关约束进行优化的，如果出现以上情况，mlLocalKeyFrames最前一帧关键帧的前一帧是空的，那么就会出问题。具体体现在Optimizer.cc的247行或者是1094行开始向g2o里添加顶点时，系统直接就退出了。
发现这里的错误，通常是由于插入的第一个关键帧没有prevkeyframe造成的。

So in AddToLocalWindow function:

            pKF0 = pKF0->GetPrevKeyFrame();
            mlLocalKeyFrames.push_front(pKF0);

答（还是同一个人）： 我最终发现了问题，并在代码中解决了它。解决办法很简单，就是在**AddToLocalWindow(KeyFrame* pKF)**函数最后几行里增加一个条件，避免把系统最开始的那帧关键帧也插入mlLocalKeyFrames中去。

while(mlLocalKeyFrames.size() < mnLocalWindowSize && pKF0->GetPrevKeyFrame()!=NULL)
{
pKF0 = pKF0->GetPrevKeyFrame();
if(pKF0->mnId != 0) //新增加的语句
mlLocalKeyFrames.push_front(pKF0);
}

最后还想说一下，因为自己用的设备是廉价的IMU，实际跑了一下VIORB，发现会出现尺度漂移的现象，所以闭环检测时，计算的还是相似变换，这样就能闭上了，否则几何校验这块过不去，闭环没法闭…

十一.

地址：
问： 有两个问题困扰着我，请求你的帮助!
（1）关于IMU噪声模型

Matrix3d IMUData::_gyrMeasCov = Matrix3d::Identity()1.7e-41.7e-4/0.0/*100*/;
Matrix3d IMUData::_accMeasCov = Matrix3d::Identity()2.0 -32.0 -3/0.005/*100*/;

你上次为什么乘100 ?

在函数中

 void EdgeNavStatePVRPointXYZOnlyPose::linearizeOplus()

Vector3d Paux = RcbRwb.transpose()(Pw-Pwb);
Matrix<double,2,3> JdRwb = - Jpi * (Sophus::SO3::hat(Paux) * Rcb);

矩阵 JdRwb = - Jpi * (Sophus::SO3::hat(Paux) * Rcb);

这是Rwb的转置，如何得到雅可比矩阵W.R.T. Rwb ?你能给我一些推荐吗?

答： 我想也许王京找到了加速协方差10倍使结果更好。您可以尝试其他参数，并将RMSE与基础真理进行比较。

我认为他使用了一个右(Rwb，左为Rbw)扰动模型:

Rwb->RwbExp(dphi_wb)或Rbw->Exp(-dphi_wb)Rbw

我想你知道链式法则:

JdRwb=J_error_PcJ_Pc_dRwb=-Jpi J_Pc_dRwb

我更喜欢用twb来表示Pwb:

Pc=RcbRbw(pwb)+tcb

，注意tcb是常量

因此

J_Pc_dRwb=partial(RcbRbw(Pw-twb))/partial(phi_wb)

这里phi_wb=Log(Rwb)

=部分(RcbExp(-phi_wb)(Pw-twb))/部分(phi_wb)，这里我使用Exp(phi_bw)=Exp(-phi_wb)

左(对于Rbw)扰动模型=[RcbExp(-dphi_wb)Exp(-phi_wb)(Pw-twb)-RcbExp(-phi_wb)(Pw-twb)]/dphi_wb=Rcb(-dphi_wb^{)Exp(-phi_wb)(Pw-twb)/dphi_wb，这里使用Exp(dphi)=I+dphi}

=Rcb*[Exp(-phi_wb)(Pw-twb)]^{dphi_wb/dphi_wb，这里使用交叉规则:aXb=-bXa或a}b=-b^a

= Rcb (Rbw * (Pw-twb)) ^ = [RcbRbw (Pw-twb)) ^ Rcb。这里使用伴随性质:Rp^=(Rp)R

证明了。

证明部分原文：
phi_wb=Log(Rwb)
=partial(RcbExp(-phi_wb)(Pw-twb))/partial(phi_wb), here I use Exp(phi_bw)=Exp(-phi_wb)
left(for Rbw) disturbance model=[RcbExp(-dphi_wb)Exp(-phi_wb)(Pw-twb)-RcbExp(-phi_wb)(Pw-twb)]/dphi_wb=Rcb(-dphi_wb^)Exp(-phi_wb)(Pw-twb)/dphi_wb, here use Exp(dphi)=I+dphi^
=Rcb*[Exp(-phi_wb)(Pw-twb)]^dphi_wb/dphi_wb, here use cross rule:aXb=-bXa or a^b=-b^a
=Rcb[Rbw*(Pw-twb)]^=[RcbRbw(Pw-twb)]^Rcb. here use Adjoint property:Rp^=(Rp)^R

十二.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/34
问：
我有一些关于VIORB SLAM在输入和输出中的坐标的问题。
（1）[IMUData]

https://github.com/jingpang/LearnVIORB
/blob/RT/Examples/ROS/ORB_VIO/src/ros_vio.cc#L136

是body框架坐标，不是吗?(x点指向传感器前方)
（2）输出mTcw坐标是摄像机坐标，z指向摄像机的前面(右手规则)，不是吗?

问： 你有没有发现你关于两个坐标系的假设是否正确?

在我的应用程序中，我使用RTIMULib设置(默认)指向Imu的x轴、向上的z轴和向右的y轴。

十三.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/30
问：
我想知道优化结果是否受到set边缘化(true)的影响。或者这仅仅与computeCovariance()有关?我试着把它设置成不同的，看看时间成本。但不管结果是真还是假，结果似乎都是一样的。我不太明白计算g2o中边际方差的算法。我要做的是得到相机姿态协方差部分。我是这样做的:
(1)添加当前相机姿势作为一个顶点setId (0);setEstimate(初始值);setMarginalize(假);setFixed(假);
(2)添加点
setId (i + 1);setEstimate(初始值);setMarginalize(真正的);setFixed(真正的);
(3)增加边缘:
setMeasurements(位置在图像);setInformation(身份);setRobustKernel (Huber); setDelta (sqrt (5.991));设置相机参数(fx、fy，cx, cy)
(4)通过以下代码获取优化后的协方差:

g2o: SparseBlockMatrixXd spinv;
optimizer.computeMarginals (spinv optimizer.vertex (0));

姿态协方差为spinvv .block(0,0)->eval()。
我不太了解边缘化。我不确定setMarginalize为真是否会影响优化的结果。
好像setMarginalize(false)与pose相关，setMarginalize(true)与landmark相关。我很困惑。
答： 实际上这是G2O所需要的。如果你检查G2O的源代码，你会发现G2O认为TRUE是landmark, FALSE是pose
问： 是的。我在G2o源代码中发现了这个，这是否意味着我们必须setMarginalize为假，如果它是一个姿态，否则设置为真?然而，我发现评论说，“当它设置为true时，这个节点应该在优化期间被边缘化”。如果我仍然希望地标被优化，我可以设置为false吗?看起来，如果为假，结构将改变为姿态(6dof)。这让我很困惑。

还有，你知道G2o中的边际协方差算法吗?哪篇论文提出了这个问题?
答：
setMarginalize()选项允许您使用舒尔补语技巧来加速Bundle Adjustment的优化。通过分离相机姿势和地标在你的优化问题，你可以利用问题的矩阵结构。通常情况下，在Bundle Adjustment问题中，你有比相机姿态更多的地标，因此，通过将每个地标设置为setMarginalize(true)， g2o将首先在向前传递中估计相机姿态，然后在向后传递中使用生成的姿势来优化地标。Schur补体利用问题的稀疏性来加速计算，并隐式地计算近似的Hessian来求解优化问题。这个近似的Hessian是由computeMarginals()给出并计算的。

所以，关于你的第一个问题，如果你设置你的地标固定，它不会在速度方面做出改变，以设置它们为setMarginalize(true)。然而，这确实会影响computeMarginals()函数，因为这个函数取决于问题的结构，而g2o会通过将设置为setMarginalize(true)的状态和设置为setMarginalize(false)的状态分开来设置问题。

如果你看一下“舒尔补语”，看看它的形式，应该会更清楚。

希望它可以帮助!

十四.

地址：
问： Tic什么意思

答： Tic表示第i个摄像机帧的位姿矩阵，Pc表示映射点在该摄像机帧中的坐标。相机的名字。Tbc是指从IMU-camera标定得到的IMU-camera从本体到camera的外部跃迁矩阵。如果您问了这个问题，那么这意味着您基本上从未阅读过相关的论文，而是直接接触了代码。名称约定基本上是正确的。我会给你们一个正确的方向，阅读Raul mural关于ORB视觉SLAM的相关论文和Forster关于IMU预整合的论文，然后阅读代码，否则你们会一直问这类问题。

问：

参考Forster论文“On-Manifold Preintegration for Real-Time Visual——Inertial Odometry”中的图像，Tbc是IMU从相机到物体的外部过渡矩阵吗?

十五.

地址：https://github.t-dt.cn/jingpang/LearnVIORB/issues/6
问： Tic什么意思

答：
Hi, dont know whether you fixed this problem and I already fixed it by add EIGEN_MAKE_ALIGNED_OPERATOR_NEW inside the class 在Tracking.h文件中 and also change the type of mv20FramesReloc from vector to vector<Frame,Eigen::aligned_allocator>

you can have a try to see if it works

十六.

地址：https://github.com/raulmur/ORB_SLAM2/pull/585 提示修改

答：
将LoopClosing.h文件中第60行，class LoopClosing 内的Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose; //原来版本
修改成：
Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose; //根据