Triangulation: Triangulation: Why Optimize?

Reference:

1. GitHub:openMVG
2. Triangulation: Why Optimize?

1. Preliminary knowledge

• vector v \mathbf{v}Euclidean norm of$v$$\Vert \mathbf{v}\Vert$；
• 单音島地：$\hat{\mathbf{v}}=\mathbf{v}/\Vert \mathbf{v}\Vert$；
• Two lines ${\mathbf{L}}_0$ 和 $L_1$Angle: $\angle ({\mathbf{L}}_0,L_1)\in [0,\pi /2]$；
• 定义 ${\mathbf{x}}_0=[x_0,y_0,z_0{]}^T$ 和 $x_1=[x_1,y_1,z_1{]}^T$ is the camera reference frame$C_0$and $C_1$three-dimensional coordinates;
• $\mathbf{R}$ 和 $t$ is the rotation and displacement between the two cameras, then$x_1=\mathbf{R}{\mathbf{x}}_0+\mathbf{t}$；
• Camera calibration matrix: $\mathbf{K}$；
• The second pixel coordinates observed at each frame point: ${\mathbf{u}}_0=(u_0,v_0,1{)}^{⊺}$ 和 ${\mathbf{u}}_1=(u_1,v_1,1{)}^{⊺}$；
• Normalized image coordinates: ${\mathbf{f}}_0={\left[x_0/z_0,y_0/z_0,1\right]}^T$ 和 $f_1={\left[x_1/z_1,y_1/z_1,1\right]}^T$ , respectively$f_0={\mathbf{K}}^{-1}{u}_0$和$f_1=K^{-1}{u}_1$get;
• In the ideal case of Fig.1a, two back-projected rays intersect, satisfying the epipolar constraint: ${\mathbf{f}}_1\cdot (\mathbf{t}\times {\mathbf{Rf}}_0)=0$ (Exercise P167,${{\mathbf{x}}_2}^T{t}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$)；
• Given the depth scalar $l_0$and $l_1$, intersection point $x_1$Default: $\begin{array}{rl}{x}_1& =l_0\mathbf{R}\widehat{f_0}+\mathbf{t}\end{array}$和$\begin{array}{rl}{\mathbf{x}}_1& =l_1\widehat{{\mathbf{f}}_1}\end{array}$。( $\widehat{f_0}$和$\widehat{f_1}$Ideally, this rarely happens due to inaccuracies in image measurements and camera models. To infer a 3D point from two oblique rays leads to the following method );

1.1 Evaluating 3D point accuracy

Once the 3D point x 1 ′ \mathbf{x}_{1}' is obtained using some triangulation method$x_1^{\prime }$, its accuracy can be assessed in a number of ways:

1. Calculate the 3D error, that is: $e_{3D}=\Vert {\mathbf{x}}_1^{\prime }-x_{\mathrm{true}}\Vert$ (see Fig.1b);
2. 计算 2D 误差，也就是重投影误差：
   $$d_i=\Vert \mathbf{K}\left({\mathbf{f}}_i-{\mathbf{f}}_i^{\prime }\right)\Vert =\Vert \mathbf{K}\left(f_i-{\left(\left[001\right]{\mathbf{x}}_i^{\prime }\right)}^{-1}{x}_i^{\prime }\right)\Vert \text{for}i=0,1\left(1\right)$$ If$x_0^{\prime }={\mathbf{R}}^{⊺}({\mathbf{x}}_1^{\prime }-\mathbf{t})$。这里 ${\left(\left[001\right]{\mathbf{x}}_i^{\prime }\right)}^{-1}$ seeks$The\; inverse\; of\; the\; z-$ axis coordinates, multiplied by$x_1^{\prime }$Normalized plane coordinates are available ( see Fig.1b );
   2D errors represent deviations from measurements, while 3D errors represent deviations from true values. Unlike 3D errors, 2D errors for 3D points can be evaluated under different norms, such as:
   • $L_1$Norm: $d_0+d_1$；
   • $L_2$Norm: $\sqrt{d_0^2+d_1^2}$；
   • $L_{\infty }$Norm: $\max (d_0,d_1)$。
3. In addition to 2D and 3D accuracy, the obtained parallax angle accuracy can also be evaluated ( see Fig.1c ), and the parallax error is defined as follows ( the expression here feels a bit confused, ${\mathbf{x}}_{\mathrm{true}}$It should be $C_1$和$x_{true}$The straight line of $x_{true}-t$ is$C_0$和$x_{true}$Definition :
   $$e_{}=|{\beta }_{\mathrm{true}}-b^{\prime }|=|\angle \left({\mathbf{x}}_{true},{\mathbf{x}}_{true}-\mathbf{t}\right)-\angle \left(x_1^{\prime },x_1^{\prime }-t\right)|(2)$$ Define "raw parallax" as the angle between raw backprojected rays:
   $$b_{\mathrm{raw}}=\angle \left({\mathbf{Rf}}_0,{\mathbf{f}}_1\right).\left(3\right)$$ This gives a rough estimate of the disparity angle independent of translation and triangulation methods.

2. Proposed method

2.1 Generalized weighted midpoint method

The generalized weighted midpoint method Generalized Weighted Midpoint (GWM)consists the following three steps:

1. Given two backprojected rays corresponding to the same point, use some method to estimate the two rays $(l_0,l_1)$ depth;
2. Calculate the depth of these two lines as $l_0$and $l_1$The 3D point of the ray, that is, at $C_1$frame $\mathbf{t}+l_0\mathbf{R}\widehat{{\mathbf{f}}_0}$及$l_1\widehat{f_1}$；
3. The final estimate of the 3D points is obtained by computing their weighted average.

In the previous classic midpoint method, the two points of each ray are the closest pair of points with the same weight. Yet another possible example of a generalized weighted midpoint is shown in the figure below:

2.2 Optional Midpoint Method

An optional midpoint method belonging to GWM is proposed here. Consider first the case where two backprojections happen to intersect, as in Fig.1a. In this case, the most reasonable solution is the intersection, and the corresponding depth along the ray can be obtained using the sine rule:
$$l_0=\frac{\sin \left(\angle \left({\mathbf{f}}_1,\mathbf{t}\right)\right)}{\sin \left(\angle \left(\mathbf{R}{\mathbf{f}}_0,{\mathbf{f}}_1,\right)\right)}\Vert \mathbf{t}\Vert =\frac{\Vert {\hat{f}}_1\times \mathbf{t}\Vert }{\Vert \mathbf{R}{\hat{f}}_0\times {\hat{f}}_1\Vert },l_1=\frac{\sin \left(\angle \left(R{f}_0,t\right)\right)}{\sin \left(\angle \left(R{f}_0,f_1,\right)\right)}\Vert t\Vert =\frac{\Vert R{\hat{f}}_0\times t\Vert }{\Vert R{\hat{f}}_0\times {\hat{f}}_1\Vert }.\left(4\right)$$ When the two rays are oblique, the formula is also used to estimate the depth. Calculate the depth λ 0 \lambda_0of the 3D point on each ray separately$l_0$and $l_1$，得：
$$\mathbf{t}+l_0\mathbf{R}{\hat{\mathbf{f}}}_0=t+\frac{\Vert {\mathbf{f}}_1\times \mathbf{t}\Vert }{\Vert {\mathbf{Rf}}_0\times f_1\Vert }{\mathbf{Rf}}_0\text{and}{l}_1\widehat{f_1}=\frac{\Vert {Rf}_0\times t\Vert }{\Vert {Rf}_0\times f_1\Vert }{f}_1\left(5\right)$$ Take the two points of the 中点有：
$${\mathbf{x}}_1^{\prime }=\frac{1}{2}\left(\mathbf{t}+\frac{\Vert {\mathbf{f}}_1\times \mathbf{t}\Vert }{\Vert \mathbf{R}{\mathbf{f}}_0\times {\mathbf{f}}_1\Vert }\mathbf{R}{\mathbf{f}}_0+\frac{\Vert R{f}_0\times t\Vert }{\Vert R{f}_0\times f_1\Vert }{f}_1\right).\left(6\right)$$令$\mathbf{p}=\mathbf{R}{\hat{f}}_0\times {\hat{f}}_1$, $\mathbf{q}=\mathbf{R}{\hat{\mathbf{f}}}_0\times t$ er$\mathbf{r}={\hat{f}}_1\times t$ , the depth formula can be replaced by:
$$l_0=\frac{||\mathbf{r}||}{||\mathbf{p}||},l_1=\frac{||\mathbf{q}||}{||p||}\left(7\right)$$ These forms are similar to the depths given by the classic midpoint method before:
$$l_{\text{mid}0}=\frac{\hat{\mathbf{p}}\cdot \mathbf{r}}{\Vert \mathbf{p}\Vert },l_{\text{mid}1}=\frac{\hat{p}\cdot \mathbf{q}}{\Vert p\Vert }.\left(8\right)$$ The difference between these two formulas is the molecule,equ.7has size$r$和$q$ , andequ.8projects them onto$on\; p$ . Therefore, we always get$l_0\ge l_{\text{mid}0}$λ 1 ≥ λ mid 1 \lambda_1\geq \lambda _ $l_1\ge l_{\text{mid}1}$. In most cases, this means that the midpoint obtained by this method will be further away than the classical midpoint. In the example depicted in Fig.2, when we estimate a point far from the camera, it usually (traditional triangulation method) results in a smaller estimated disparity angle.

2.3 Cheirality (positive field depth constraints representing 3D points in multi-view geometry)

The positive depth constraint of a 3D point is violated when the triangulated point has a negative depth. This can happen for a number of reasons, such as wrong correlation of point pairs or noisy image points near poles. Usually it doesn't cause serious problems, since it's easy to check the positive depth of field at each point and discard bad ones. For the classical midpoint method, this can be confirmed by checking the sign of depth given by equ.8 . But this cannot be confirmed for the method given in this paper, because the given depth is always positive. The figure above illustrates the difference between the two methods. Therefore, in this method, the depth alone cannot tell whether the triangulation result is reliable or not.

At this time, you need to use other methods to test whether the point is a bad point (it is a bad point if the following inequality is satisfied ): If changing the sign of at least one depth to negative will cause the distance between two points on the light to become smaller, discard the corresponding point Relation:
$$\Vert \mathbf{t}+l_0\mathbf{R}\widehat{{\mathbf{f}}_0}-l_1{\hat{f}}_1{\Vert }^2\ge \min \left(\Vert \mathbf{t}+l_{0}R\widehat{f_0}+l_1\widehat{f_1}{\Vert }^2,\Vert t-l_{0}R\widehat{f_0}-l_1{\hat{f}}_1{\Vert }^2,\Vert t-l_{0}R\widehat{f_0}+l_1\widehat{f_1}{\Vert }^2\right)\left(9\right)$$ For the classic midpoint method, let$l_0=|{\lambda }_{mid0}|$和$l_1=|{\lambda }_{mid1}|$ can effectively get the same result as testing the positive depth of field constraint (obviously, it is to verify whether this number is greater than zero). And the current method is established inequ.9and Fig.3a, because when$l_0=-|{\lambda }_{mid0}|$及$l_1=-|{\lambda }_{mid1}|$ when the two points are closest.

2.4 Inverse Depth Weighted (IDW) Midpoint

The weighted midpoint given by equ . 6 usually results in a disproportionate reprojection error of the two images, and Fig. 3c gives an example. Note that rays with smaller depths tend to have larger reprojection errors. To compensate for this imbalance, the paper proposes to use the inverse depth as the weight:
$${\mathbf{x}}_1^{\prime }=\frac{l_0^{-1}\left(\mathbf{t}+l_0\mathbf{R}\widehat{{\mathbf{f}}_0}\right)+l_1^{-1}\left(l_1\widehat{{\mathbf{k}}_1}\right)}{l_0^{-1}+l_1^{-1}}=\frac{\Vert \mathbf{q}\Vert }{\Vert \mathbf{q}\Vert +\Vert \mathbf{r}\Vert }\left(\mathbf{t}+\frac{\Vert \mathbf{r}\Vert }{\Vert \mathbf{p}\Vert }\left(\mathbf{k}\widehat{{\mathbf{f}}_0}+\widehat{f_1}\right)\right)(10)$$

3. Implement the code

// Helper function
// Compute Relative motion between two absolute poses parameterized by Rt
// Rotate one bearing vector according to the relative motion
inline void AbsoluteToRelative(const Mat3 &R0, const Vec3 &t0, const Mat3 &R1, const Vec3 &t1, const Vec3 &x0, Mat3 &R, Vec3 &t, Vec3 &Rx0)
{
    
    
  R = R1 * R0.transpose();
  t = t1 - R * t0;
  Rx0 = R * x0;
}

bool TriangulateIDWMidpoint(const Mat3 & R0, const Vec3 & t0, const Vec3 & x0, const Mat3 & R1, const Vec3 & t1, const Vec3 & x1, Vec3* X_euclidean)
{
    
    
  // absolute to relative
  Mat3 R;
  Vec3 t, Rx0;
  AbsoluteToRelative(R0, t0, R1, t1, x0, R, t, Rx0);

  const double p_norm = Rx0.cross(x1).norm();
  const double q_norm = Rx0.cross(t).norm();
  const double r_norm = x1.cross(t).norm();

  // Eq. (10)
  const auto xprime1 = ( q_norm / (q_norm + r_norm) )
    * ( t + (r_norm / p_norm) * (Rx0 + x1) );

  // relative to absolute
  *X_euclidean = R1.transpose() * (xprime1 - t1);

  // Eq. (7)
  const Vec3 lambda0_Rx0 = (r_norm / p_norm) * Rx0;
  const Vec3 lambda1_x1 = (q_norm / p_norm) * x1;

  // Eq. (9) - test adequation
  return (t + lambda0_Rx0 - lambda1_x1).squaredNorm()
    <
    std::min(std::min(
      (t + lambda0_Rx0 + lambda1_x1).squaredNorm(),
      (t - lambda0_Rx0 - lambda1_x1).squaredNorm()),
      (t - lambda0_Rx0 + lambda1_x1).squaredNorm());
}