1. OptimizeSim3
(1)顶点与边
顶点
Sim(3)位姿优化发生在闭环检测过程中,仅对形成闭环的闭环候选关键帧到当前帧的Sim(3)位姿进行优化,不优化地图点。但由于地图点与二者产生约束,因此顶点包括待优化的闭环候选关键帧到当前帧的Sim(3)位姿和地图点(固定住不优化)。
顶点中Sim(3)位姿的类型为g2o::VertexSim3Expmap,地图点的类型为g2o::VertexSBAPointXYZ。
边
边在Sim(3)优化中是指地图点和Sim(3)位姿的投影关系,由于地图点和Sim(3)位姿都参与优化,因此是二元边。另外,根据投影关系可分为两种类型边。第一种为正向投影,即从闭环候选关键帧的地图点用g2oS12投影到当前关键帧上的边,类型是g2o::EdgeSim3ProjectXYZ;第二种是反向投影,即从当前关键帧的地图点用g2oS21投影到闭环候选关键帧的边上,类型是g2o::EdgeInverseSim3ProjectXYZ。
(2)误差
正向投影
// 正向投影
class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSim3ProjectXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
// 顶点1: 关键帧的Sim(3)变换 g2oS12
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
// 顶点0: 地图点
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector2d obs(_measurement); // 二维观测
// 误差 = 观测 - 重投影坐标 候选关键帧投影到当前关键帧
_error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
}
// virtual void linearizeOplus();
};
// 反向投影
class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeInverseSim3ProjectXYZ();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
// 顶点1: 关键帧的Sim(3)变换 g2oS21
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
// 顶点0: 地图点
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
Vector2d obs(_measurement); // 二维观测
// 误差 = 观测 - 重投影坐标 当前关键帧投影到候选关键帧
_error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())));
}
// virtual void linearizeOplus();
};
v2->estimate()表示估计的相机2坐标系下的三维点坐标,
v1->estimate()表示估计的位姿g2oS12。
v1->estimate().inverse()表示估计的位姿g2oS21。在优化过程中不断更新,参与到重投影误差的计算当中。
正向投影:cam_map1(project(v1->estimate().map(v2->estimate())))表示用v1估计的位姿g2oS12把v2估计的相机2坐标系下的三维点转换到相机1坐标系下,然后通过project函数归一化三维点,再通过cam_map1函数用内参将其转化为像素坐标。
(3)OptimizeSim3整体流程
- 初始化g2o优化器。
- 设置待优化的Sim(3)位姿作为顶点。如果是单目相机模式,则不固定尺度;如果是双目/RGB-D相机模式,则在优化时固定尺度。
- 设置匹配的地图点作为顶点,并且设置地图点不优化。
- 设置地图点投影关系作为边。包括正向投影:从闭环候选关键帧的地图点投影到当前关键帧;反向投影:从当前关键帧投影到闭环候选关键帧。
- 用g2o开始优化,迭代5次。
- 用卡方检验剔除优化后误差大的边。
- 再次用g2o优化剩下的边。如果在上一步中有误差较大的边被剔除,说明闭环质量不是很好,则本次迭代10次;否则,本次迭代5次。
- 用优化后的结果更新Sim(3)位姿。
(4)OptimizeSim3代码实现
/**
* @brief 形成闭环时固定(不优化)地图点进行Sim3位姿优化
* 1. Vertex:
* - g2o::VertexSim3Expmap(),两个关键帧的位姿
* - g2o::VertexSBAPointXYZ(),两个关键帧共有的MapPoints
* 2. 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(与特征点所在的尺度有关)
*
* @param[in] pKF1 当前帧
* @param[in] pKF2 闭环候选帧
* @param[in] vpMatches1 两个关键帧之间的匹配关系
* @param[in] g2oS12 两个关键帧间的Sim3变换,方向是从2到1,即闭环候选关键帧到当前帧
* @param[in] th2 卡方检验是否为误差边用到的阈值
* @param[in] bFixScale 是否优化尺度,单目进行尺度优化,双目/RGB-D不进行尺度优化
* @return int 优化之后匹配点中内点的个数
*/
int Optimizer::OptimizeSim3(KeyFra