图优化
1.图优化的流程
- 选择你想要的图里的节点与边的类型，确定它们的参数化形式；
- 往图里加入实际的节点和边；
- 选择初值，开始迭代；
- 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
- 求解稀疏线性方程 H * detaX = -b，得到梯度方向；
- 继续用GN或LM进行迭代。如果迭代结束，返回优化值。
实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。
2.顶点和边
- 在图中，以顶点表示优化变量，以边表示观测方程。
| 两个顶点 | 一条边边 | |
| 机器人两个Pose之间的变换 | 两个pose |
变换关系： detaT = T1 * inv(T2) |
| 机器人在某个Pose处用激光测量到了某个空间点 |
一个2D Pose[x,y,theta]; 一个空间点(lamda_x，lamda_y) |
观测方程：
|
| 机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标 |
一个像素坐标 Pose = [u; v] 一个空间点x = (x,y,z) |
z = [u; v] = C (R * x + t)， C为相机内参 |
- 优化目标
e(x,k) 是x符合z的程度的一个度量，越小越符合，反之越不符合。对式进行求导和一阶泰勒展开，得到其极值为0的方程：
其中，deta X即为梯度。
- 可解性解释
只有和xk顶点相连的边，出现了非零值。相应的二阶导矩阵H中，大部分也是零元素。这种稀疏性能很好地帮助我们快速求解上面的线性方程。稀疏矩阵代数库：SBA、PCG、CSparse、Cholmod等等
- 引入核函数的原因，是因为SLAM中可能给出错误的边。如cauchy核，huber核等等。
- 问题： 四维变换矩阵T或者三维旋转矩R无法进行加减和求导：李群和李代数变换后可以，再变换回来。
2.g2o库
3.应用
参考：https://www.cnblogs.com/gaoxiang12/p/5304272.html
- 定义顶点
- 定义边
- 配置BlockSolver(LinerSolverType)
- 配置OptimizationAlgorithm
- 配置Optimizer
- 添加顶点
- 添加边
- 启动优化
4.解释
4.1 顶点（Vertex)
VertexSE2 : publicBaseVertex< 3, SE2> //2D pose Vertex, (x,y,theta) VertexSE3 : publicBaseVertex< 6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion) VertexPointXY : publicBaseVertex< 2, Vector2> VertexPointXYZ : publicBaseVertex< 3, Vector3> VertexSBAPointXYZ : publicBaseVertex< 3, Vector3> // SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map//指数映射关系 VertexSE3Expmap : publicBaseVertex< 6, SE3Quat>//类型内部使用的其实是四元数，不是李代数//g2o定义的相机位姿类型：SE3Quat // SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. // qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation VertexCam : publicBaseVertex< 6, SBACam> // Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. VertexSim3Expmap : publicBaseVertex< 7, Sim3>
重新定义顶点一般需要考虑重写如下函数：
virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; //分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以 //顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方 //程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。 virtual void oplusImpl (const number_t* update); //顶点重置函数，设定被优化变量的原始值。 virtual void setToOriginImpl();
4.2 边（Edge)
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。一条边只连接一个顶点，两元边理解为一条边连接两个顶点；多元边的一条边可以连接多个（3个以上）顶点。
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
D, //测量值的维度 E, //测量值的数据类型 VertexXi, //顶点的类型 VertexXj //顶点的类型
自定义边
virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; //分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以 virtual void computeError(); //非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差 virtual void linearizeOplus(); //非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian _measurement： //存储观测值 _error： //存储computeError() 函数计算的误差 _vertices[]： //存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) //是设定的int 有关（0 或1） setId(int)： //来定义边的编号（决定了在H矩阵中的位置） setMeasurement(type) //函数来定义观测值 setVertex(int, vertex) //来定义顶点 setInformation() //来定义协方差矩阵的逆
总结：
重要参考：https://www.cnblogs.com/gaoxiang12/p/5244828.html
https://www.aiimooc.com/mall/preshow-htm-itemid-382.html
https://blog.csdn.net/hzwwpgmwy/article/details/79884070
