Probabilistic Triangulation
We denote a camera pose at time $t$ with respect to a global frame as $P_t=\{R_t , \mathbf c_t\}$, where $R_t$ is a rotation matrix encoding the orientation of the camera and $\mathbf c_t$ shows the position of the camera in the global frame. If a landmark with the coordinate $\mathbf p=[p_x,p_y,p_z]^T$ is observed at two camera poses $P_k=\{R_k , \mathbf c_k\}$ and $P_t=\{R_t \ \mathbf c_t\}$, at the points $(x_k,y_k)$ and $(x_t,y_t)$ on the retina of the camera, the landmark can be localized in space using triangulation. Our triangulation method is based on the fact that the point should lie on the lines drawn from the center of each camera pose in the directions of the observations. As a result, the following equations hold: \begin{align} \mathbf p &= \mathbf c_k+ d_k \mathbf v_k \\ \mathbf p &= \mathbf c_t+ d_t \mathbf v_t \end{align} where $\mathbf v_k=R_{k}[x_k,y_k,1]^T$ and $\mathbf v_t=R_{t}[x_t,y_t,1]^T$. $d_k$ and $d_t$ are the depths of the landmark in the camera frames attached to each camera pose. Using the two equations, the following linear equation system is obtained: \begin{align} [ \mathbf v_k | -\mathbf v_t] \begin{bmatrix} d_k\\ d_t \end{bmatrix}= \mathbf c_t - \mathbf c_k=\mathbf c_{t,k} \end{align} By solving the above equation system, the depth of the landmark in the $k^{th}$ camera frame will be: \begin{align} d_k=\frac{\rho}{\nu}=\frac{\left(\mathbf v_t^T \mathbf v_t \mathbf v_k^T - \mathbf v_k^T \mathbf v_t \mathbf v_t^T \right) \mathbf c_{t,k}} {\mathbf v_t^T \mathbf v_t \mathbf v^T_k \mathbf v_k -(\mathbf v_k^T \mathbf v_t)^2 } \label{eq_depth} \end{align} It can be shown that if there are measurement noise or errors in the estimation of rotation and translation parameters, $\rho$ and $\nu$ will be joint Gaussian random variables: $[\rho,\nu]^T \sim \mathcal N([\bar \rho, \bar \nu]^T, \Sigma)$. As a result, $d_k$ has the distribution of the ratio of two dependent Gaussian random variables. It can be shown that: \begin{equation} p(d_k|\rho)=\frac{1}{\sqrt{2\pi}\sigma_\nu}\frac{|\rho|}{d_k^2}\exp \left(-\frac{(\rho -\bar \nu d_k)^2}{2\sigma^2_\nu d^2_k} \right) \label{eq_d_distribution} \end{equation} where $\sigma^2_\nu$ is the variance of $\nu$ obtained from the marginalization of $\rho$ from the joint distribution of $\rho$ and $\nu$. The goal of probabilistic triangulation is to find a confidence range for $d_k$ such as $[d^{min}_{k},d^{max}_{k}]$ at each new observation of the landmark. To this end, we use Eq. \ref{eq_d_distribution} for $\rho=\bar \rho-2 \sigma_\rho$ and $\rho=\bar \rho+2 \sigma_\rho$ and find two positive $d_k$ at which the probability of $p(d_k|\rho)$ is equal to a small ratio of the maximum pick of the distribution. In Fig. \ref{fig_pda}, the two distributions for $\rho=1$, $\sigma_\rho=0.1$, $\nu=0.1$ and $\sigma_\nu=0.1$ are depicted.
