SLAM中的图优化问题：从两帧位姿开始

以及相机内参矩阵 C，求解两个图中的相机运动R,t。

总之，假设相机1的位姿为单位矩阵，对于任意一个特征点，它在三维空间的真实坐标位于 Xj，而在两个相机坐标系上看来是 zj1,zj2。根据投影关系，我们有：

它叫做最小化重投影误差问题（Minimization of Reprojection error）。当然，它很遗憾地，是个非线性，非凸的优化问题，这意味着我们不一定能求解它，也不一定能找到全局最优的解。在实际操作中，我们实际上是在调整每个Xj，使得它们更符合每一次观测zj，也就是使每个误差项都尽量的小。由于这个原因，它也叫做捆集调整（Bundle Adjustment）。

注意这个边，实际上不是那么直观的“边”也即从相机位姿到特征点位置的连线，而是空间点Xj到其在相机上的像素坐标的投影关系

也就是说边是重投影误差

下面我们来用g2o实现一下BA。选取的结点和边如下：

结点1：相机位姿结点：g2o::VertexSE3Expmap，来自

结点2：特征点空间坐标结点：g2o::VertexSBAPointXYZ，来自；

边：重投影误差：g2o::EdgeProjectXYZ2UV

　这个是 EdgeProjectXYZ2UV 边的定义。它是一个Binary Edge，后面的模板参数表示，它的数据是2维的，来自Eigen::Vector2D，它连接的两个顶点必须是 VertexSBAPointXYZ， VertexSE3Expmap。我们还能看到它的 computeError 定义，和前面给出的公式是一致的。注意到计算Error时，它调用了 g2o::CameraParameters 作为参数，所以我们在设置这条边时也需要给定一个相机参数。

而误差计算是什么意思呢：

从简单的入手：（这个边是edgesim3ProjectXYZonlePose）

这里v1为相机位姿（se3）

v1.estimate.map(Xw) 中：Xw为特征点在世界坐标系中的位置（3d）

从而这个的计算结果可以视为xyz_trans，其为特征点在相机坐标系中的位置(3d)

对这个结果xyz_trans进行cam_project，也就是得到特征点在相机坐标系中的坐标(2d) 也即u,v

其和观测值obs相减，即为误差项。

而到这里：

v1为相机1位姿（se3），v2为特征点位置（pointXYZ）（两个节点不是同一种节点）

所以 v1.estimete.map(v2.estimate)，计算得到的是特征点在相机1坐标中的位置（3d）

对这个结果进行cam.cam_map，计算得到的是特征点在相机坐标系中的坐标（2d）也即u，v

这个值和观测值obs相减，得到误差项

总结，边实际上就是提供了一种误差项的计算方式，以及一种使用节点数据的方式

对于我定义的节点类型，以及我定义的误差项计算方式，找到这样的边的类型，或者自己自定义一个

在使用的时候，把边和节点连接起来，然后给边一个观测值，这个值就是用来计算误差项之中的obs

从而优化就是不断调整节点的值，使得误差项的和最小

特别篇：

刚才的节点实际上都是相机的位姿，或者说单帧位姿，但实际上，节点也可以是相对位姿，如下：

闭环检测时两帧之间相对位姿计算：OptimizeSim3

a. 参数列表

OptimizeSim3(

KeyFrame *pKF1, //匹配的两帧中的一帧

KeyFrame *pKF2, //匹配的两帧中的另一帧

vector &vpMatches1, //共视的地图点

g2o::Sim3 &g2oS12, //两个关键帧间的Sim3变换

const float th2, //核函数阈值

const bool bFixScale)//单目进行尺度优化，双目不进行尺度优化

b. 图的结构

Vertex:

- g2o::VertexSim3Expmap()，两个关键帧的位姿

- g2o::VertexSBAPointXYZ()，两个关键帧共有的地图点

Edge:

- g2o::EdgeSim3ProjectXYZ()，BaseBinaryEdge

+ Vertex：关键帧的Sim3，MapPoint的Pw

+ measurement：MapPoint在关键帧中的二维位置(u,v)

+ InfoMatrix: invSigma2(与特征点所在的尺度有关)

- g2o::EdgeInverseSim3ProjectXYZ()，BaseBinaryEdge

+ Vertex：关键帧的Sim3，MapPoint的Pw

+ measurement：MapPoint在关键帧中的二维位置(u,v)

+ InfoMatrix: invSigma2(与特征点所在的尺度有关)

c. 具体流程

1) 把输入的KF1到KF2的位姿变换SIM3加入图中作为节点0.

2) 找到KF1中对应的所有map点, 放在vpMapPoints1中. vpMatches1为输入的匹配地图点, 是在KF2中匹配上map点的对应集合.

3) Point1是KF1的Rotation matrix*map point1的世界坐标+KF1的Translation matrix.（也即，Point1是map point 1在关键帧1的相机坐标系下的坐标（3D））

Point2是KF2的Rotation matrix*map point2的世界坐标+KF2的Translation matrix. （也即，Point2是map point 2在关键帧2的相机坐标系下的坐标（3D））

把Point1 Point2作为节点加入图中。

4) 在节点0与Point1, Point2之间都建立边连接. 测量值分别是地图点反投影在图像上的二维坐标, 信息矩阵为反投影的误差.

从而，边有两类，一类是KF2对应的特征点到KF1的投影，一类是KF1对应的特征点到KF2的投影

5) 图构建完成, 进行优化!

6) 更新两帧间转换的Sim3.