图优化

1.图优化的流程

1. 选择你想要的图里的节点与边的类型，确定它们的参数化形式；
2. 往图里加入实际的节点和边；
3. 选择初值，开始迭代；
4. 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
5. 求解稀疏线性方程 H * detaX = -b，得到梯度方向；
6. 继续用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

1. 定义顶点
2. 定义边
3. 配置BlockSolver(LinerSolverType)
4. 配置OptimizationAlgorithm
5. 配置Optimizer
6. 添加顶点
7. 添加边
8. 启动优化

 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