ORB_SLAM中Optimizer的优化分析及g2o点和边在ORB中的使用

Optimizer::PoseOptimization

用于Tracking中匀速运动模型跟踪等

EdgeSE3ProjectXYZOnlyPose

1
class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

测量值是2维的Vector2d数据,即像素坐标,这条边连接着pose节点,是一元Unary边,只优化SE3的顶点变量,地图点XYZ固定,世界坐标系下的地图点是成员变量Xw,在Optimizer::PoseOptimization(Frame *pFrame)中遍历当前帧可视的地图点调用,一个顶点N条边。
(一元边里面只有_jacobianOplusXi

1
2
3
e = (u,v) - project(Tcw * Pw)  

Tcw = CameraPose

二元边BaseBinaryEdge有_jacobianOplusXi_jacobianOplusXj,这个比较好理解)
观测二维,所以残差也是二维,即当前帧的像素观测和地图点在当前帧的投影位置的残差,从而优化位姿。残差对位姿的求导所以雅克比维度是2X6。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
Vector3d xyz_trans = vi->estimate().map(Xw);

double x = xyz_trans[0];
double y = xyz_trans[1];
double invz = 1.0/xyz_trans[2];
double invz_2 = invz*invz;

_jacobianOplusXi(0,0) = x*y*invz_2 *fx;
_jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
_jacobianOplusXi(0,2) = y*invz *fx;
_jacobianOplusXi(0,3) = -invz *fx;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = x*invz_2 *fx;

_jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
_jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
_jacobianOplusXi(1,2) = -x*invz *fy;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -invz *fy;
_jacobianOplusXi(1,5) = y*invz_2 *fy;
}

加完边后进行4次迭代,每次迭代优化10次,每次迭代完会有一次野值的筛选(根据卡方分布),TODO:野值边setLevel(1),内点setLevle(0),why,好像1代表不优化。

Optimizer::BundleAdjustment

3D-2D BA,在GlobalBundleAdjustemnt中调用,计算量比较大

EdgeSE3ProjectXYZ

1
2
EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {
}

这条边连着3D地图点和位姿,测量和误差函数和EdgeSE3ProjectXYZOnlyPose一样,分别是像素点和

1
2
3
e = (u,v) - project(Tcw * Pw)  

Tcw = CameraPose

但是上面是一元边,而这是二元边,即同时优化pose和地图点。所以线性化中要写两个雅可比,分别是二维的残差对3D地图点求导2X3,和对xi求导,2X6:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void EdgeSE3ProjectXYZ::linearizeOplus() {
……

// 2*3
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();

// 2*6 the same as EdgeSE3ProjectXYZOnlyPose::linearizeOplus()
_jacobianOplusXj(0,0) = x*y/z_2 *fx;
_jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
_jacobianOplusXj(0,2) = y/z *fx;
_jacobianOplusXj(0,3) = -1./z *fx;
_jacobianOplusXj(0,4) = 0;
_jacobianOplusXj(0,5) = x/z_2 *fx;

_jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
_jacobianOplusXj(1,1) = -x*y/z_2 *fy;
_jacobianOplusXj(1,2) = -x/z *fy;
_jacobianOplusXj(1,3) = 0;
_jacobianOplusXj(1,4) = -1./z *fy;
_jacobianOplusXj(1,5) = y/z_2 *fy;
}

然後迭代一次,优化10次。然后setpose和setWorldPos成优化后的结果


Optimizer::LocalBundleAdjustment

得到当前帧连接的KF和这些KF中的地图点后(lLocalKeyFrames和lLocalMapPoints),和能观测到这些地图点但没有和当前帧相连的KF(lFixedCameras)。
在加Pose顶点时(其中lFixedCameras的vSE3->setFixed(true),而lLocalKeyFrames只用第一帧fix就可以)。

边也是EdgeSE3ProjectXYZ。和BA一样,只不过这里的数据来源是Local的,比较快一些。

开始优化,迭代一次优化5次。然后根据卡方分布筛选阈值(这个阈值见多视图几何),去除野值后再优化。然后去除误差比较大的KF和地图点的互相观测。最后更新优化后的位姿和地图点。


Optimizer::OptimizeEssentialGraph

闭环检测后的优化
首先拿到地图中所有KF和地图点,声明vScw和vCorrectedSwc,分别代表未sim3优化前的sim3位姿和优化后的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
for(vKFs)
{
g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
如果是sim3调整过的KF,VSim3->setEstimate(it->second);用调整过的位姿
否则用当前pose。VSim3->setEstimate(Siw);
如果是当前帧对应的闭环帧,fixed
optimizer.addVertex(VSim3);

并给vScw赋值供后面使用
}

// Set Loop edges
// Set normal edges
这边加的各种边类型都是EdgeSim3,区别只是不同的来源

EdgeSim3

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
class G2O_CORE_API EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSim3();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError()
{
const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);
const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);

Sim3 C(_measurement);
Sim3 error_=C*v1->estimate()*v2->estimate().inverse();
_error = error_.log();
}

virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
{
VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]);
VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]);
if (from.count(v1) > 0)
v2->setEstimate(measurement()*v1->estimate());
else
v1->setEstimate(measurement().inverse()*v2->estimate());
}
};

可以看到是连接两个Sim3节点的二元边,顶点0的位姿是Tiw,1的位姿是Tjw,那么边的测量是Tji,即from i to j。

1
e = log( Measurement * Tiw * Tjw^{-1}  )

没有重载::linearizeOplus()方法,似乎是用g2o的自动求导,即差分。会不会慢一些?
优化后,
1.SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1],重新setpose,即取R,t/S。
2.Correct points. Transform to “non-optimized” reference keyframe pose and transform back with optimized pose。把世界系下的地图点投到未校正的相机系下再用矫正后的pose投到世界系,得到心得世界系坐标。

1
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));


Optimizer::OptimizeSim3

是在筛选闭环候选帧的时候用的。

1
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

输入时当前帧、闭环候选帧和候选帧对应的地图点、优化前的sim3(当前帧和闭环候选帧之间Scm,m-候选帧)。输出时优化后的sim3位姿。

EdgeSim3ProjectXYZ

和前面提到的EdgeSE3ProjectXYZ有些不同,从SE3变成了SIM3,7自由度。ORB也用了g2o自动求导,把线性化函数注释掉了。观测一样,是像素坐标。如果自己写的话,应该是一个2X3的对点的雅可比,一个2X7的对sim3的雅可比。


ORBLoopClosing

1
2
3
4
5
6
7
8
9
10
11
12
13
14
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3()) //用到Optimizer::OptimizeSim3筛选候选帧
{
// Perform loop fusion and pose graph optimization
CorrectLoop(); //如果找到了,用到Optimizer::OptimizeEssentialGraph 优化。
}
}
}

Sim3

1
2
3
4
5
6
7
8
9
T  =  \begin{bmatrix}
sR & t \\
0 & 1
\end{bmatrix}

T^{-1} = \begin{bmatrix}
R/s & -R^{T}t/s \\
0 & 1
\end{bmatrix}

在ORB实现的sim3节点中,逆的表示为

1
q' = q*; t'=q*(-t/s);s'=1/s