以及相机内参矩阵 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/types/sba/types_sba.h>；
边：重投影误差：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<MapPoint *> &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.
具体：添加Sim3顶点（两个关键帧的位姿变换）
添加特征点顶点
这里注意，vpmatches1是关键帧2中的特征点，vpmappoints1是关键帧1中的特征点
循环 对当前循环，pmp1为关键帧1中特征点，pmp2为关键帧2中特征点，每次循环添加两个顶点
一个顶点是pmp1在相机1坐标系下的坐标，一个顶点是pmp2在相机2坐标系下的坐标
添加边：
可以看到 这里的边是EdgeSim3ProjectXYZ边
这个边的误差是
也就是说 现在，对每对匹配，有三个节点，一个节点是两帧的相对位姿变换，两个节点是特征点在相机坐标系下的位置
有两条边，第一条边连接相对位姿变换和节点2
这里比较难理解的是，v2是特征点的世界坐标，v1是相对位姿变换（而不是之前的v1是当前帧相机位姿）
那
意味着什么呢，如果v1是当前帧相机坐标的时候，这意味着计算特征点在相机坐标系下的位置
现在就意味着，计算特征点在相对位姿变换下的位置？这怎么理解
而且，测量值的确是特征点在图像上的坐标（u,v），那这就肯定要指定是在图像1，还是图像2上的坐标
总不可能是在“图像的相对变换”上的坐标吧
比如
这里的obs1，阅读代码后发现确实是特征点在图像1上的坐标
而且，这里的目的很显然是要算对称重投影误差
只是不明白的是，在节点是表示相对位姿变换的情况下，怎么投影到其中一帧上
网友评论