SVO笔记 - 三角量测

by luoshi006

参考：
$\quad \quad$https://blog.youkuaiyun.com/heyijia0327/article/details/50774104
$\quad \quad$https://blog.youkuaiyun.com/heyijia0327/article/details/51083398
$\quad \quad$https://github.com/uzh-rpg/rpg_svo/issues/62
$\quad \quad$https://blog.youkuaiyun.com/kokerf/article/details/72844455
$\quad \quad$https://github.com/gaoxiang12/slambook/blob/master/ch13/dense_monocular/dense_mapping.cpp#L240

深度滤波器的深度估计过程：

1. 在 $ref$ 帧，选取特征点；
2. 在 $cur$ 帧，通过极线搜索和块匹配 (ZSSD) ，确定投影点位置；
3. 通过三角量测，计算深度和方差；
4. 在滤波器中融合深度，直至收敛；

对深度的求解过程，作者 Forster 给出了三种解法：

• SVO 中 depth_filter 更新过程中的最小二乘深度求解；
• rpg_vikit 中 给出了最小二乘求解 3D 点坐标；
• REMODE 中，采用 cramer 法则，求解 3D 点坐标；

---

解法一：

SVO depth_filter 中的深度求解。

已知：
$\quad \quad$ $R = R_{cr}$，$t = t_{cr}$，
$\quad \quad$ $ref$ 帧中特征点归一化坐标 $f_r$，
$\quad \quad$ $cur$ 帧中特征点归一化坐标 $f_c$;

求：
$\quad \quad$ 深度 $d_r = depth_{ref}$， $d_c = depth_{cur}$

---

两特征对应同一 3D 点

$$d_c \mathbf f_c = d_r \mathbf R \mathbf f_r + \mathbf t$$

整理，得：

$$d_c \mathbf f_c - d_r\mathbf R\mathbf f_r = \mathbf t$$

$$\begin{bmatrix}\mathbf R \mathbf f_r & \mathbf f_c \end{bmatrix} \begin{bmatrix} - d_r \\ d_c \end{bmatrix} = \mathbf t$$

$$\begin{bmatrix}\mathbf R \mathbf f_r \\ \mathbf f_c \end{bmatrix} \begin{bmatrix}\mathbf R \mathbf f_r &\mathbf f_c \end{bmatrix} \begin{bmatrix} -d_r \\ d_c \end{bmatrix} =\begin{bmatrix}\mathbf R \mathbf f_r \\ \mathbf f_c \end{bmatrix} \mathbf t$$

$$\mathbf {A^TA} \begin{bmatrix} -d_r \\ d_c \end{bmatrix} =\mathbf {A^T} \mathbf t$$

$$\begin{bmatrix} d_r \\ -d_c \end{bmatrix} =-\left (\mathbf {A^TA}\right )^{-1}\mathbf {A^T} \mathbf t$$

https://github.com/uzh-rpg/rpg_svo/blob/80744bbe424e8a9656cdba2bdfee8c472bf01ced/svo/src/matcher.cpp#L109

bool depthFromTriangulation(
    const SE3& T_search_ref,
    const Vector3d& f_ref,
    const Vector3d& f_cur,
    double& depth)
{
  Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
  const Matrix2d AtA = A.transpose()*A;
  if(AtA.determinant() < 0.000001)
    return false;
  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
  depth = fabs(depth2[0]);
  return true;
}

---

解法二：

SVO rpg_vikit 中的 3D 点求解。

已知：
$\quad \quad$ $\mathbf R = \mathbf R_{12}$，$\mathbf t = \mathbf t_{12}$，
$\quad \quad$ 特征点归一化坐标 $\mathbf feature_1$，
$\quad \quad$ 特征点归一化坐标 $\mathbf feature_2$;

求：
$\quad \quad$ 3D 坐标

---

$$\mathbf f_2 = \mathbf R \cdot \mathbf feature_2$$

$$\mathbf f_1 = \mathbf feature_1$$

$$z_1 \mathbf f_1 - z_2 \mathbf f_2 = \mathbf t$$

$$\begin{bmatrix} \mathbf f_1 & \mathbf f_2 \end{bmatrix}\begin{bmatrix} z_1 \\ -z_2 \end{bmatrix} = \mathbf t$$

$$\begin{bmatrix}\mathbf f_1 \\ \mathbf f_2 \end{bmatrix}\begin{bmatrix} \mathbf f_1 &\mathbf f_2 \end{bmatrix}\begin{bmatrix} z_1 \\ -z_2 \end{bmatrix} = \begin{bmatrix} \mathbf f_1 \\ \mathbf f_2 \end{bmatrix}\mathbf t$$

$$\begin{bmatrix} \mathbf f_1\mathbf f_1 & -\mathbf f_1\mathbf f_2 \\ \mathbf f_2\mathbf f_1 & -\mathbf f_2\mathbf f_2 \end{bmatrix}\begin{bmatrix} z_1 \\ z_2 \end{bmatrix} = \begin{bmatrix} \mathbf f_1 \mathbf t\\ \mathbf f_2\mathbf t \end{bmatrix}$$

$$\begin{bmatrix} z_1 \\ z_2 \end{bmatrix} =\begin{bmatrix} \mathbf f_1\mathbf f_1 & -\mathbf f_1\mathbf f_2 \\ \mathbf f_2\mathbf f_1 & -\mathbf f_2\mathbf f_2 \end{bmatrix} ^{-1}\begin{bmatrix} \mathbf f_1\mathbf t \\ \mathbf f_2\mathbf t \end{bmatrix}$$

https://github.com/uzh-rpg/rpg_vikit/blob/master/vikit_common/src/math_utils.cpp#L15

Vector3d
triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
                         const Vector3d& feature1, const Vector3d& feature2 )
{
  Vector3d f2 = R * feature2;
  Vector2d b;
  b[0] = t.dot(feature1);
  b[1] = t.dot(f2);
  Matrix2d A;
  A(0,0) = feature1.dot(feature1);
  A(1,0) = feature1.dot(f2);
  A(0,1) = -A(1,0);
  A(1,1) = -f2.dot(f2);
  Vector2d lambda = A.inverse() * b;
  Vector3d xm = lambda[0] * feature1;
  Vector3d xn = t + lambda[1] * f2;
  return ( xm + xn )/2;
}

---

解法三：

SVO-REMODE 中的 3D 点求解。

$$\begin{bmatrix} \mathbf f_r \\ \mathbf R \mathbf f_c \end{bmatrix} \begin{bmatrix} \mathbf f_r & \mathbf R \mathbf f_c \end{bmatrix} \begin{bmatrix} d_r \\ -d_c \end{bmatrix} =\begin{bmatrix}\mathbf f_r \mathbf t \\ \mathbf R \mathbf f_c \mathbf t \end{bmatrix}$$

$$\begin{bmatrix} \mathbf f_r \mathbf f_r & -\mathbf f_r \mathbf R \mathbf f_c \\ \mathbf R \mathbf f_c \mathbf f_r & -\mathbf R \mathbf f_c\mathbf R \mathbf f_c \end{bmatrix} \begin{bmatrix} d_r \\ d_c \end{bmatrix} =\begin{bmatrix}\mathbf f_r \mathbf t \\ \mathbf R \mathbf f_c \mathbf t \end{bmatrix}$$

cramer 法则：

$$\begin{bmatrix}a_0 & a_1 \\ a_2 & a_3 \end{bmatrix}\begin{bmatrix}d_r \\ d_c \end{bmatrix}=\begin{bmatrix}b_x \\ b_y \end{bmatrix}$$

$$D = det(\mathbf A) = a_0a_3-a_1a_2$$

$$D_1 = det \begin{bmatrix}b_x & a_1\\ b_y &a_3 \end{bmatrix} =b_xa_3-b_ya_1$$

$$D_2 = det \begin{bmatrix} a_0& b_x\\ a_2 &b_y \end{bmatrix} =a_0b_y-b_xa_2$$

$$d_r = \frac{D_1}{D}$$

$$d_c = \frac{D_2}{D}$$

https://github.com/uzh-rpg/rpg_open_remode/blob/master/src/triangulation.cu#L27

// Returns 3D point in reference frame
// Non-linear formulation (ref. to the book 'Autonomous Mobile Robots')
__device__ __forceinline__
float3 triangulatenNonLin(
    const float3 &bearing_vector_ref,
    const float3 &bearing_vector_curr,
    const SE3<float> &T_ref_curr)
{
  const float3 t = T_ref_curr.getTranslation();
  float3 f2 = T_ref_curr.rotate(bearing_vector_curr);
  const float2 b = make_float2(dot(t, bearing_vector_ref),
                               dot(t, f2));
  float A[2*2];
  A[0] = dot(bearing_vector_ref, bearing_vector_ref);
  A[2] = dot(bearing_vector_ref, f2);
  A[1] = -A[2];
  A[3] = dot(-f2, f2);

  const float2 lambdavec = make_float2(A[3]*b.x - A[1]*b.y,
      -A[2]*b.x + A[0]*b.y) / (A[0]*A[3] - A[1]*A[2]);
  const float3 xm = lambdavec.x * bearing_vector_ref;
  const float3 xn = t + lambdavec.y * f2;
  return (xm + xn)/2.0f;
}