《视觉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 s1x1−s2Rx2=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. ⎩⎨⎧s1x1Tx1−s2x1TRx2=x1Tts1(Rx2)Tx1−s2x2Tx2=(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=fref、s1=dref、x2=fcurr、s2=dcurr、f2=RRC∗fcurr，并带入式(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. ⎩⎨⎧dref∗frefT∗fref−dcurr∗frefT∗f2=frefT∗tdref∗f2T∗fref−dcurr∗fcurrT∗fcurr=f2T∗t(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} ⎣⎡frefT∗freff2T∗fref−frefT∗f2−fcurrT∗fcurr⎦⎤⎣⎡drefdcurr⎦⎤=⎣⎡frefT∗tf2T∗t⎦⎤(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=∣∣∣∣∣∣frefT∗freff2T∗fref−frefT∗f2−fcurrT∗fcurr∣∣∣∣∣∣=∣∣∣∣A0A2A1A3∣∣∣∣,b=⎣⎡frefT∗tf2T∗t⎦⎤=⎣⎡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}=常数 frefT∗f2=f2T∗fref=常数，因此 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 建图（代码详述带注释）
- 怎样利用克莱姆法则解线性方程组