# Triangulation from calibrated stereo rig

I am using a stereo rig to do SLAM, calibrated using the MATLAB Calibration Tool. I need to compute the 2D coordinates of a landmark using the observation model obtained from triangulation (the images are rectified).

The equations obtained from triangulation are the ones presented in the blue box here. Because I am doing SLAM in 2D the coordinates I need to use are $Z_p$ and $X_p$. The parameters needed to compute those values are $f$, $T$ and $disparity (x_L - x_R)$.

After doing the calibration intrinsics matrices $K_L$ and $K_R$ are obtained and a common intrinsic matrix for the stereo rig is calculated from $K = 1/2*(K_L +K_R)$ so I get the parameters needed in triangulation from this common matrix.

The focal length is supplied from the manufacturer, and for my Logitech C170 is 2.3mm. The baseline $T$ from the calibration is 78.7803 mm. To compute the disparity I am obtaining SURF points and using RANSAC to discard the outliers so I get x coordinates from both rectified images.

The problem is that with those values I can't obtain correct values for $Z_p$ and $X_p$ and I am not sure why or where I am doing the wrong step. Anyone can help with this? Are those the correct steps to do triangulation from rectified stereo images?

EDIT: My stereo rig looks like the figure I attach: If you compare the coordinates system with the one used in the link before is easy to see that my $X_r$ corresponds to the $Z_p$ from the link and the $Y_r$ corresponds to $X_p$, so the equations to calculate the distance using triangulation and with the coordinate system of the figure are:

$x_r=\frac{fb}{x_L-x_R}$

$y_r = \frac{(x_L-p_x)b}{x_L-x_R}-\frac{b}{2}$

Being $f$ the focal length, $b$ the baseline, $p_x$ the x coordinate of the central point and $x_L-x_R$ the disparity. The $X_r$ $Y_r$ coordinate system is situated between the two cameras, so this is the meaning of the $\frac{b}{2}$ displacement in the equations.

Calibration

To obtain the cameras calibration I am using the Stereo Camera Calibrator Toolbox with the chessboard pattern.

After calibration, I made some tests using MATLAB functions triangulate and reconstructScene to know whether the parameteres are well calculated. The distances I obtained using this functions (which use the stereoParams object created by the calibrator) works well and I obtain distances very similar to the actual ones. So I suposse the calibration works well.

The problem, as I explained before, is when I try to calculate the distances using the equations $x_r$ and $y_r$ because I am not sure how to obtain the common matrix $K$ for the stereo rig (the calibrator gives one intrinsic matrix for each camera, so you have two matrices).

The value of the baseline given from calibration make sense, I made a measurement with a ruler and gives me 78 mm approximately.

The $f$ value I assume should be in pixels but here again the calibration gives an $f_x$ and $f_y$ value so I am not sure which one should I use.

Those are the intrinsic matrices I obtain:

Left: $\begin{pmatrix} 672.6879&-0.7752&282.2488\\0&674.3705&240.1287\\0&0&1 \end{pmatrix}$

Right: $\begin{pmatrix} 681.7049&0.0451&331.2612\\0&681.8235&246.1209\\0&0&1\end{pmatrix}$

Being the parameters of $K$: $\begin{pmatrix}f_x&s&p_x\\0&f_y&p_y\\0&0&1\end{pmatrix}$