《视觉SLAM十四讲》ch13.3:三角化公式推导及代码详解

前言

《视觉SLAM十四讲–ch13.3》单目稠密重建代码中的三角化过程,不像第7章直接调用OpenCV中的triangulation()函数求解,而是自己根据公式计算、并使用克莱姆法则对方程组进行求解,得出深度信息。

但代码中对有关公式的推导及与公式的对应关系讲解的不甚清楚,这里对该三角化过程进行公式推导,并与实现代码对应起来,方便大家理解。

公式推导也可参考:三角化公式推导&手撕代码进行补充。

公式推导

当通过极线搜索和块匹配确定参考帧与当前帧的匹配像素点以后,接下来就是通过三角化计算像素点对应的深度值。
按照对极几何中的定义,设 x 1 , x 2 x_1,x_2 x1,x2为两个匹配像素点的归一化坐标,则他们满足:

s 1 x 1 = s 2 R x 2 + t s_1x_1=s_2Rx_2+t s1x1=s2Rx2+t

即:

(1) s 1 x 1 − s 2 R x 2 = t s_1x_1-s_2Rx_2=t\tag 1 s1x1s2Rx2=t(1)

对式(1)分别左乘 x 1 T x_1^T x1T ( R x 2 ) T (Rx_2)^T (Rx2)T,得到式(2):
(2) { s 1 x 1 T x 1 − s 2 x 1 T R x 2 = x 1 T t s 1 ( R x 2 ) T x 1 − s 2 x 2 T x 2 = ( R x 2 ) T t \left\{ \begin{array}{c} s_1x_1^Tx_1-s_2x_1^TRx_2=x_1^Tt \tag 2\\ \\ s_1(Rx_2)^Tx_1-s_2x_2^Tx_2=(Rx_2)^Tt \end{array} \right. s1x1Tx1s2x1TRx2=x1Tts1(Rx2)Tx1s2x2Tx2=(Rx2)Tt(2)

令: x 1 = f r e f 、 s 1 = d r e f 、 x 2 = f c u r r 、 s 2 = d c u r r 、 f 2 = R R C ∗ f c u r r x_1=f_{ref}、s_1=d_{ref}、x_2=f_{curr}、s_2=d_{curr}、f_2=R_{RC}*f_{curr} x1=frefs1=drefx2=fcurrs2=dcurrf2=RRCfcurr,并带入式(2),则有式(3):
(3) { d r e f ∗ f r e f T ∗ f r e f − d c u r r ∗ f r e f T ∗ f 2 = f r e f T ∗ t d r e f ∗ f 2 T ∗ f r e f − d c u r r ∗ f c u r r T ∗ f c u r r = f 2 T ∗ t \left\{ \begin{array}{c} d_{ref}*f_{ref}^T*f_{ref}-d_{curr}*f_{ref}^T*f_2=f_{ref}^T*t \tag 3\\ \\ d_{ref}*f_2^T*f_{ref}-d_{curr}*f_{curr}^T*f_{curr}=f_2^T*t \end{array} \right. dreffrefTfrefdcurrfrefTf2=frefTtdreff2TfrefdcurrfcurrTfcurr=f2Tt(3)

将式(3)写成矩阵形式,如下:
(4) [ f r e f T ∗ f r e f − f r e f T ∗ f 2 f 2 T ∗ f r e f − f c u r r T ∗ f c u r r ] [ d r e f d c u r r ] = [ f r e f T ∗ t f 2 T ∗ t ] \begin{bmatrix} f_{ref}^T*f_{ref} & -f_{ref}^T*f_2 \\ \\ f_2^T*f_{ref} & -f_{curr}^T*f_{curr} \\ \tag 4 \end{bmatrix} \begin{bmatrix} d_{ref} \\ \\d_{curr} \\ \end{bmatrix} = \begin{bmatrix} f_{ref}^T*t \\ \\f_2^T*t \\ \end{bmatrix} frefTfreff2TfreffrefTf2fcurrTfcurrdrefdcurr=frefTtf2Tt(4)

根据克莱姆法则(Cramer’s Rule),系数矩阵的行列式 D D D b b b为:
(5) D = ∣ f r e f T ∗ f r e f − f r e f T ∗ f 2 f 2 T ∗ f r e f − f c u r r T ∗ f c u r r ∣ = ∣ A 0 A 1 A 2 A 3 ∣ , b = [ f r e f T ∗ t f 2 T ∗ t ] = [ b 00 b 10 ] D =\begin{vmatrix} f_{ref}^T*f_{ref} & -f_{ref}^T*f_2 \\ \\ f_2^T*f_{ref} & -f_{curr}^T*f_{curr} \\ \tag 5 \end{vmatrix} =\begin {vmatrix} A_0 & A_1 \\ A_2 & A_3 \\ \end{vmatrix} , b =\begin{bmatrix} f_{ref}^T*t \\ \\f_2^T*t \\ \end{bmatrix} =\begin{bmatrix} b_{00} \\ \\b_{10} \\ \end{bmatrix} D=frefTfreff2TfreffrefTf2fcurrTfcurr=A0A2A1A3,b=frefTtf2Tt=b00b10(5)

注意,由于 f r e f f_{ref} fref f 2 f_2 f2均为 V e c t o r 3 d Vector3d Vector3d,即 3 × 1 3×1 3×1,则 f r e f T ∗ f 2 = f 2 T ∗ f r e f = 常 数 f_{ref}^T*f_2=f_2^T*f_{ref}=常数 frefTf2=f2Tfref=,因此 A 1 = − A 2 A_1=-A_2 A1=A2

则:
(6) D 1 = ∣ b 00 A 1 b 10 A 3 ∣ , D 2 = ∣ A 0 b 00 A 2 b 10 ∣ D_1=\begin {vmatrix} b_{00} & A_1 \\ b_{10} & A_3 \\ \tag 6 \end{vmatrix} ,D_2=\begin {vmatrix} A_0 & b_{00} \\ A_2 & b_{10} \\ \end{vmatrix} D1=b00b10A1A3,D2=A0A2b00b10(6)

则,可估计出两个深度值 d r e f , d c u r r d_{ref}, d_{curr} dref,dcurr (7) d r e f = D 1 / D , d c u r r = D 2 / D d_{ref}=D_1/D,d_{curr}=D_2/D \tag 7 dref=D1/D,dcurr=D2/D(7)

至此,三角化有关公式推导完毕。

代码

注意:

  • a . d o t ( b ) a.dot(b) a.dot(b)会自动调整 a a a b b b矩阵的行列数,使其满足矩阵乘法规律,因此不用转置;
  • 此处代码中用到的矩阵乘积结果均为常数,因此 a . d o t ( b ) = b . d o t ( a ) a.dot(b)=b.dot(a) a.dot(b)=b.dot(a),不用在意顺序;
  • 源码中 A [ 3 ] A[3] A[3]的表达式是错的,与公式不符,正确的表达式见下注释。
// dense_monocular/dense_mapping.cpp片段

bool updateDepthFilter(		// 更新深度滤波器
    const Vector2d& pt_ref, 
    const Vector2d& pt_curr, 
    const SE3& T_C_R,
    Mat& depth, 
    Mat& depth_cov
)
{
    // 两帧图像对应的匹配点:pt_ref和pt_curr
    SE3 T_R_C = T_C_R.inverse(); 
    Vector3d f_ref = px2cam( pt_ref ); 	// pt_ref 转到归一化相机坐标系   
    f_ref.normalize();					// 模长变为1
    
    Vector3d f_curr = px2cam( pt_curr );// pt_curr 转到归一化相机坐标系
    f_curr.normalize();					// 模长变为1
    
    // 克莱默法则是先算系数矩阵行列式的值D,然后用常数项b依次替换掉D中的第i列,得到行列式Di,Di/D即为xi的值
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.rotation_matrix() * f_curr; 
 
    Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );	// dot()会自动转换t与f_ref,保证t的列与f_ref的行相等,因此不用转置
    double A[4];	// A即上述的A0-A3
    A[0] = f_ref.dot ( f_ref );
    A[2] = f_ref.dot ( f2 );
    A[1] = -A[2];
    A[3] = - f2.dot ( f2 );		// 此处应该是A[3] = - f_curr.dot ( f_curr );	
    // d 即系数矩阵D的行列式的值
    double d = A[0]*A[3]-A[1]*A[2];
    
    // 线性方程组求出的两个深度值:d_ref、d_curr
    Vector2d lambdavec = 
        Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
                    -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;

	// 计算相机坐标系下的3D坐标
    Vector3d xm = lambdavec ( 0,0 ) * f_ref;    // xm = s1*x1,即归一化相机坐标系转相机坐标系,s1即d_ref    
    Vector3d xn = t + lambdavec ( 1,0 ) * f2;	// xn = s2*R*x2 + t,xn为x2由当前坐标系转到参考坐标系下对应的3d坐标,s2即d_curr
    
    // 实质都是计算参考坐标系下的xm,此处是通过计算xm和xn平均值,减小误差
    Vector3d d_esti = ( xm+xn ) / 2.0;  // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();   // 深度值
    
    /*************************************************/
    // 计算不确定性(以一个像素为误差)
    Vector3d p = f_ref*depth_estimation;	// 由归一化相机坐标系转到相机坐标系
    
    Vector3d a = p - t; 	// 公式13.7
    
    // 根据夹角公式,计算alpha:cos(alpha) = A*B/(|A||B|)
    double t_norm = t.norm();
    double a_norm = a.norm();
    // 由公式13.7可知,alpha = acos(p.dot(t)/(p.norm()*t_norm)),且p = f_ref*depth_estimation,p.norm()=f_ref.norm()*depth_estimation
    // 约分约掉常量depth_estimation,且f_ref模长为1,因此才有下式的形式
    double alpha = acos( f_ref.dot(t)/t_norm );    
    double beta = acos( -a.dot(t)/(a_norm*t_norm));
	
	// 书中图13-4中gamma位置标的不对,同样beta`应该是:beta_prime = beta - atan(1/fx);
    double beta_prime = beta + atan(1/fx);    
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation; 
    double d_cov2 = d_cov*d_cov;
    
    // 高斯融合,即13.6公式
    double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
    double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];   
    double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2);
    double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 );
    // 更新系统当前的状态量
    depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse; 
    depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;
    
    return true;
}

参考

  • SLAM14讲学习笔记(十四)ch13 建图(代码详述带注释)
  • 怎样利用克莱姆法则解线性方程组

你可能感兴趣的:(SLAM)