后端 位姿图 第11章

Posted 2020-10-03 踏路girl

1.前言
以BA为主的图优化，很多路标点，大量特征点，因为有这大量的特征点，所以实时不好，要在更大的场景上用，必须进行优化。这里后端有两种方法，一种位姿图，一种因子图。
2.位姿图
BA带有很多的相机位姿和空间点，时间一长，BA计算效率会下降。但是实际情况中，经过若干次观测之后，收敛的特征点和空间位置肯定会收敛到一个点，而那些发散的外点，也通常看不到了，所以优化几次就可以把它们给固定住，没有必要再优化了。
位姿图其实就是BA中，不管路标了，原来是路标和顶点之间构成边，而这里是顶点和顶点之间构成边，求得是两个相机位姿之间的相对运动。路标点这时候只是对姿态节点的约束。
实时BA，只能处理几万个点，而一个关键帧就几百个点。所以要实时，必须要做一些优化。比如滑动窗口法，丢弃一些历史数据。比如位姿图，舍弃对路标点的优化。
2.1位姿图的优化
先了解一下李代数SO3,SE3.SO3是由三维向量fai组成的，fai.hat是一个3*3的矩阵，exp(fai.hat)就是旋转矩阵。
SE3是由6维向量ep组成的，ep是由p和fai组成，p就是一个三维向量。ep.hat形式为fai.hat,p,0.trans,0组成的一个4*4矩阵，exp(ep.hat)就是变换矩阵。
李代数就更简单，就是SO3就是由旋转矩阵R组成的，R*R.trans=I,det(R)=1.
SE3就是变换矩阵组成的，R属于SO3.变换矩阵是4*4的矩阵。
先进行的ln,再进行的矩阵到向量的 变换。所以误差eij是一个6维的向量，属于SE3.
这里误差的计算公式是，v1=_vertices[0]->estimate,v2=_vertices[1]->estimate,v1,v2都是SE3形式。测量值也是SE3的格式。
_error=(_measurement.inverse()*v1.inverse()*v2).log()
不明白这里的误差为什么还要取个log.
这里求导数，先求J.造了一个JRInv的函数，里面放入的变量是SE3::exp(_error)=e.
这里A11就为SO3::hat(e.so3().log());
A12=SO3::hat(e.translation());A21=0,A22=SO3::hat(e.so3().log());
J=A*0.5+I.
而误差对epi的导数就为-J*(v2.inverse().Adj())
误差对epj的导数就为J*(v2.inverse().Adj())
2.2实践
数据是g2o文件，就是sphere.g2o.转圈上升，t-1时刻与t时刻的边，里程计，层与层之间，回环。
sphere.g2o文件前半部分由节点组成，VERTEX_SE3，默认用四元数和平移向量，就是Id,平移，旋转。qw在最后。前面都是VERTEX_SE3.
后半部分就都是边了，EDGE_SE3:两个id,tx,ty,tz,qx,qy,qz,qw,信息矩阵的右上角（因为信息矩阵为对称阵，只需保存一半就可以）
SE3实际上是四元数而非李代数。
2.2.1简单优化
如果是简单优化的话，就比较好做了，因为有sphere.g2o文件，文件提供了各个节点和边，所以直接读取做优化就可以了。顶点和边可以用g2o原有的形式。
先设fin，ifstream fin(argv[1]),还要做两个判断，argc!=2,!fin,!fin里的输出通常就是文件不存在。
定义顶点数和边数vertexCnt,edgeCnt,初值为0。
用!fin.eof()做一个while循环，在循环中
string name,fin>>name,如果name为节点，那么读取顶点值。这里v用的是g2o::VertexSE3()形式，fin先赋值给index,然后v->setId(index),v->read(fin);v是指针，指针访问从来都是->.图模型添加顶点，顶点数加1.当index为0的时候，v->setFixed(true),因为是初值啊。
如果name为边，那么读边，边形式g2o::EdgeSE3(),定义idx1,idx2,fin>>idx1>>idx2,fin分别赋值给idx1,idx2,这两个id是节点id,边的id设的是edgeCnt++,然后边设置顶点0为optimizer.vertices()[idx1],顶点1为optimizer.vertices()[idx2],然后e从fin里读值，e->read(fin),图模型添加边。

其他都差不多，只不过这里optimizer.optimize(30)之后，optimizer.save("result.g2o"),把优化结果保存到result.g2o中。
2.2.2位姿图优化的实践
这里定义了自己的顶点和边。
先定义求J_R^{-1}的近似函数JRInv,变量是SE3 e,返回值是6*6的矩阵。
这里A11=SE3::hat(e.so3().log()),A12=SE3::hat(e.translation()),
A21=0,A22=A11.
定义了自己的顶点VertexSE3LieAlgebra,顶点为6维向量，类型为SE3.
顶点对read函数进行了操作，用来读tx,ty,tz,qx,qy,qz,qw的，is赋值给data,估计值就可以设为
setEstimate(SE3(Eigen::Quaterniond(data[6],data[3],data[4],data[5]),Eigen::Vector3d(data[0],data[1],data[2])));到时候v直接read fin就可以了。
顶点对write函数也进行了操作，变量os,os输出的有id(), _estimate.translation().transpose()，定义完q=_estimate.unit_quaternion(),输出q.coeffs()[0]到3.
更新值up由Sophus::SO3(update[3],update[4],update[5])和Eigen::vector3d组成，而且更新的方式是乘即_estimate=up*_estimate.
定义了自己的边EdgeSE3LieAlgebra,误差值是6为的SE3,两个顶点就是我们之前定义的顶点。
在read函数，跟顶点一样，把前7个值赋给data,后四个数组成四元数q,q要规范化一下，用q.normalize,这7个数组成了测量值，即setMeasurement(q,Eigen::Vector3d(data[0],data[1],data[2],data[3]).对于0<=i<information().rows(),i<=j<information().cols(),is把剩下的数赋给信息矩阵的(i,j),对于i和j不等的情况，(j,i)=(i,j).
write函数，变量os,把_vertices[0],_vertices[1]赋值给指针v1,v2,输出v1,v2的id.同顶点，输出信息矩阵，输出测量值的四元数的协方差。
计算误差和导数，跟之前一样。
接下来的主函数和2.2.1一样，只是顶点和边的类型不同。
位姿图有带状的（机器人直线前进的时候），球样的稠密的位姿图。
2.3小结
后端和前端可不同时，有时候称为跟踪和建图，这里的建图其实是后端来着。前端实时，后端则可以慢悠悠地优化，只要在优化完成时把结果返回成前端就可以了。

 

 

 

。