课程Gitbug地址:https://github.com/wrk666/VSLAM-Course/tree/master
这讲来讲后端
之前的最小二乘的方法属于批量方法,较为简单,另一种方法是递归方法。
前端是实时的估计,抓取关键帧和特征点匹配,后端计算量较大,不要求实时,但是计算量也需要限定在一定的程度内。
保持当前状态,来新信息之后就更新,
运动方程对自己的位姿有一个大概的估计,但是存在不确定性,随着时间推移,不确定性会累积,而观测方程观测到路标点,对之前的估计进行矫正,不确定性会减小很多。
假设一阶马尔科夫性则会得到EKF等滤波器方法
不假设的话会得到非线性优化的方法。
对稀疏矩阵做一个简单的维数验证:
雅可比:88矩阵块
雅可比的转置 J T \bm J^T JT,同样也是88矩阵块,每个块也要转置
H = J T J H=\bm J^T\bm J H=JTJ
和下面的临接矩阵除了对角线外,结构一样
高斯消元,H第二行左乘 − E C − 1 -EC^{-1} −EC−1加到第一行来消掉右上角的E。
这里核心意思是把计算量转移到了求解 Δ x c \Delta x_c Δxc上,最主要的就是就是对C进行求逆,
新的关键帧来到时,要把老的关键帧Marg掉,老的关键帧会影响先验分布
问题是:如果回环之后,老的信息不太容易用起来。
照着例程运行,安装meshlab
sudo apt-get install meshlab
跑代码和4.1差不多。
编译时有个小bug:
./pose_graph_g2o_lie ../sphere.g2o
Sophus ensure failed in function 'void Sophus::SO3Base::normalize() [with Derived = Sophus::SO3]' , file '/usr/local/include/sophus/so3.hpp', line 273.
Quaternion ( 0.706662 4.32706e-17 0.707551 -4.3325e-17) should not be close to zero!
查了一下,原因是:
顶点类VertexSE3LieAlgebra的读入函数virtual bool read(istream& is)缺少返回值,会报以上错误。
解决方法:在函数virtual bool read(istream& is)中加入return true;
virtual bool read(istream &is) override {
double data[7];
for (int i = 0; i < 7; i++)
is >> data[i];
Quaterniond q(data[6], data[3], data[4], data[5]);
q.normalize();
setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
for (int i = 0; i < information().rows() && is.good(); i++)
for (int j = i; j < information().cols() && is.good(); j++) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
用单位阵代替 J r − 1 J_r^{-1} Jr−1结果差不多:
// Matrix6d J = JRInv(SE3d::exp(_error));
Matrix6d J = MatrixXd::Identity(6, 6);
// 尝试把J近似为I?
做作业!