Triangulation

To find out the feature point in world coordinate when it is observed at least in two camera frames

Given:

• 2D image observations [u,v] in multiple frames.
• Camera intrinsic matrix K.
• Camera poses [$R_w^{c_i}$,$t_w^{c_i}$] for each observation frame: it is from world frame to each camera frame

Theory §

$$\begin{array}{rlrl}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\frac{1}{s}\ast [R_w^{c_i},t_w^{c_i}]\ast P^w& \left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =K^{-1}\ast \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$

Homogeneous Method §

• From Equation (1)

$$\begin{array}{rl}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\frac{1}{s}\ast \left[\begin{array}{c}{T_w^{c_i}}^1\ast P\\ {T_w^{c_i}}^2\ast P\\ {T_w^{c_i}}^3\ast P\end{array}\right]\end{array}$$

• Do the Cross Product on both side

$$0=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\times \left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\times \left[\begin{array}{c}{T_w^{c_i}}^1\ast P\\ {T_w^{c_i}}^2\ast P\\ {T_w^{c_i}}^3\ast P\end{array}\right]$$

• Transform into Skew Symmetric Matrix

$$\left[\begin{array}{c}\begin{array}{rl}-{T_w^{c_i}}^{2}P& +y{T_w^{c_i}}^{3}P\\ {T_w^{c_i}}^{1}P& -x{T_w^{c_i}}^{3}P\\ -y{T_w^{c_i}}^{1}P& +x{T_w^{c_i}}^{3}P\end{array}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\text{\hspace{1em}(2)}$$

Since row3 is linearly dependant on row1 & row2, so we need another set of Equation(2) in order to recover the 3D Position of the feature point.

• We can solve the Equation(2) by using Singular Value decomposition. Since P is Homogeneous Coordinates, so we need to homogeneous V by $P=V[0:3]/V[4]$.
• When there is noise in the measurement, $V[4]$ could be a value that is close to zero. Then the recovered position from P might be a infinity point. This is a problem of doing triangulation by homogeneous method.

Non-Homogeneous Method §

Code Implementation §

Homogeneous Method §

Reference §

1. 多帧三角化原理
2. 三角化特征点(triangulation)方法及实现对比
3. SLAM中多目三角化