Cal3_S2Stereo

A Cal3_S2Stereo object describes the calibration model for a stereo camera rig, a setup for depth estimation where two cameras are mounted side by side at a fixed horizontal distance known as the baseline. This model assumes both cameras have the same intrinsic parameters.

This class inherits from the standard 5-parameter Cal3_S2 model and adds a sixth parameter, the baseline $b$ .

The calibration matrix $K$ is identical to that of Cal3_S2 and represents the parameters of a single camera from the stereo camera rig:

K = \begin{bmatrix} f_x & s & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{bmatrix}

(1)

However, note that Cal3_S2Stereo itself does not perform the complete calibration of a stereo camera; it merely stores all the parameters necessary for the calibration. For instance, the baseline parameter $b$ is primarily used by other classes like StereoCamera to compute the disparity between the left and right images during 3D projection.

To see how Cal3_S2Stereo is used in practical applications, please refer to StereoVOExample and StereoVOExample_large.

import gtsam
import numpy as np

Initialization¶

A Cal3_S2Stereo object can be initialized in several ways:

# Default constructor
cal_default = gtsam.Cal3_S2Stereo()
print("Default constructor (fx=1, fy=1, s=0, u0=0, v0=0, b=1):")
print(cal_default)
print(f"fx: {cal_default.fx()}, fy: {cal_default.fy()}, s: {cal_default.skew()}, u0: {cal_default.px()}, v0: {cal_default.py()}, {cal_default.baseline()}\n")

# From individual parameters: fx, fy, s, u0, v0, b
fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5
cal_params = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)
print("From parameters (fx, fy, s, u0, v0, b):")
print(cal_params)
print(f"fx: {cal_params.fx()}, fy: {cal_params.fy()}, s: {cal_params.skew()}, u0: {cal_params.px()}, v0: {cal_params.py()}, {cal_params.baseline()}\n")

# From a 6-vector [fx, fy, s, u0, v0, b]
param_vector = np.array([1500.0, 1600.0, 0, 320.0, 240.0, 0.5])
cal_vector = gtsam.Cal3_S2Stereo(param_vector)
print("From a 6-vector [fx, fy, s, u0, v0, b]:")
print(cal_vector)
print(f"fx: {cal_vector.fx()}, fy: {cal_vector.fy()}, s: {cal_vector.skew()}, u0: {cal_vector.px()}, v0: {cal_vector.py()}, {cal_vector.baseline()}\n")

# From fov (in degrees), image dimensions, and stereo baseline
fov, w, h, b = 100, 600, 500, 0.5
cal_fov = gtsam.Cal3_S2Stereo(fov, w, h, b)
print("From fov, image dimensions, and baseline:")
print(cal_fov)
print(f"fx: {cal_vector.fx()}, fy: {cal_vector.fy()}, s: {cal_vector.skew()}, u0: {cal_vector.px()}, v0: {cal_vector.py()}, {cal_vector.baseline()}\n")

Default constructor (fx=1, fy=1, s=0, u0=0, v0=0, b=1):
K: 1 0 0
0 1 0
0 0 1
Baseline: 1

fx: 1.0, fy: 1.0, s: 0.0, u0: 0.0, v0: 0.0, 1.0

From parameters (fx, fy, s, u0, v0, b):
K: 1500    0  320
   0 1600  240
   0    0    1
Baseline: 0.5

fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5

From a 6-vector [fx, fy, s, u0, v0, b]:
K: 1500    0  320
   0 1600  240
   0    0    1
Baseline: 0.5

fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5

From fov, image dimensions, and baseline:
K: 251.73      0    300
     0 251.73    250
     0      0      1
Baseline: 0.5

fx: 1500.0, fy: 1600.0, s: 0.0, u0: 320.0, v0: 240.0, 0.5

Properties¶

Since Cal3_S2Stereo inherits from Cal3_S2, it has access to all of its parent’s property member functions, in addition to its own specific member functions, as follows:

baseline(): Returns the horizontal baseline $b$ .

fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5
cal = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)

print("Calibration object properties:")
print(f"fx: {cal.fx()}")
print(f"fy: {cal.fy()}")
print(f"skew: {cal.skew()}")
print(f"u0: {cal.px()}")
print(f"v0: {cal.py()}")
print(f"Baseline: {cal.baseline()}")
print(f"Principal Point: {cal.principalPoint()}")
print(f"Vector [fx, fy, s, u0, v0, b]: {cal.vector()}")
print(f"Aspect ratio: {cal.aspectRatio()}")
print(f"K matrix:\n{cal.K()}")

Calibration object properties:
fx: 1500.0
fy: 1600.0
skew: 0.0
u0: 320.0
v0: 240.0
Baseline: 0.5
Principal Point: [320. 240.]
Vector [fx, fy, s, u0, v0, b]: [1.5e+03 1.6e+03 0.0e+00 3.2e+02 2.4e+02 5.0e-01]
Aspect ratio: 0.9375
K matrix:
[[1.5e+03 0.0e+00 3.2e+02]
 [0.0e+00 1.6e+03 2.4e+02]
 [0.0e+00 0.0e+00 1.0e+00]]

Basic Operations¶

`calibrate()`¶

The calibrate(p) function converts a 2D point p from pixel coordinates $(u, v)$ to intrinsic coordinates $(x, y)$ . Please review the user guide for Cal3_S2 for a more detailed explanation of this function.

def calibration_demo(cal: gtsam.Cal3_S2Stereo, row: int, col: int):
  u, v = col + 0.5, row + 0.5   # coordinates of pixel center
  x, y = cal.calibrate([u, v])
  print(f"image[{row}, {col}] -> (u, v)=({round(u, 2)}px, {round(v, 2)}px) -> (x, y)=({round(x, 5)}, {round(y, 5)})")
  return x, y

fx, fy, s, u0, v0, b = 1500.0, 1600.0, 0, 320.0, 240.0, 0.5
cal = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)

_ = calibration_demo(cal, 0, 0)
_ = calibration_demo(cal, 768, 1024)
_ = calibration_demo(cal, 1536, 2048)

image[0, 0] -> (u, v)=(0.5px, 0.5px) -> (x, y)=(-0.213, -0.14969)
image[768, 1024] -> (u, v)=(1024.5px, 768.5px) -> (x, y)=(0.46967, 0.33031)
image[1536, 2048] -> (u, v)=(2048.5px, 1536.5px) -> (x, y)=(1.15233, 0.81031)

calibrate(p) can also optionally compute Jacobians.

Dcal_calibrate = np.zeros((2, 6), order='F') # Note shape is 2x6
Dp_calibrate = np.zeros((2, 2), order='F')
p_pixels = [768, 1024]
_ = cal.calibrate(p_pixels, Dcal_calibrate, Dp_calibrate)
print(f"\nJacobian Dcal_calibrate:\n{Dcal_calibrate}")
print(f"\nJacobian Dp_calibrate:\n{Dp_calibrate}")


Jacobian Dcal_calibrate:
[[-0.00019911  0.         -0.00032667 -0.00066667  0.          0.        ]
 [ 0.         -0.00030625  0.          0.         -0.000625    0.        ]]

Jacobian Dp_calibrate:
[[ 0.00066667 -0.        ]
 [ 0.          0.000625  ]]

`uncalibrate()`¶

The uncalibrate(p) method converts a 2D point p from intrinsic coordinates $(x,y)$ back to pixel coordinates $(u, v)$ . Please review the user guide for Cal3_S2 for a more detailed explanation of this function.

def uncalibration_demo(cal: gtsam.Cal3_S2Stereo, x: float, y: float):
  u, v = cal.uncalibrate([x, y])
  print(f"(x, y)=({round(x, 5)}, {round(y, 5)}) -> (u, v)=({round(u, 2)}px, {round(v, 2)}px) -> image[{np.floor(v)}, {np.floor(u)}]")
  return u, v

x, y = calibration_demo(cal, 0, 0)
_ = uncalibration_demo(cal, x, y)
x, y = calibration_demo(cal, 1024, 768)
_ = uncalibration_demo(cal, x, y)

image[0, 0] -> (u, v)=(0.5px, 0.5px) -> (x, y)=(-0.213, -0.14969)
(x, y)=(-0.213, -0.14969) -> (u, v)=(0.5px, 0.5px) -> image[0.0, 0.0]
image[1024, 768] -> (u, v)=(768.5px, 1024.5px) -> (x, y)=(0.299, 0.49031)
(x, y)=(0.299, 0.49031) -> (u, v)=(768.5px, 1024.5px) -> image[1024.0, 768.0]

uncalibrate(p) can optionally compute Jacobians as well.

# This method can also optionally compute Jacobians.
Dcal_uncalibrate = np.zeros((2, 6), order='F')
Dp_uncalibrate = np.zeros((2, 2), order='F')
p_intrinsic = [0, 0]
_ = cal.uncalibrate(p_intrinsic, Dcal_uncalibrate, Dp_uncalibrate)
print(f"\nJacobian Dcal_uncalibrate:\n{Dcal_uncalibrate}")
print(f"\nJacobian Dp_uncalibrate:\n{Dp_uncalibrate}")


Jacobian Dcal_uncalibrate:
[[0. 0. 0. 1. 0. 0.]
 [0. 0. 0. 0. 1. 0.]]

Jacobian Dp_uncalibrate:
[[1500.    0.]
 [   0. 1600.]]

Manifold Operations¶

Like other calibration classes in GTSAM, Cal3_S2Stereo is treated as a manifold type. This abstraction allows it to integrate seamlessly into GTSAM’s optimization framework.

retract(d) maps a small update vector d in the tangent space to a new calibration object on the manifold.
localCoordinates(T2) computes the tangent-space vector that moves from the current calibration to another calibration T2.

print("Original cal_model:")
print(cal)

# Retract: Apply a delta to the calibration parameters
delta_vec = np.array([10.0, 20.0, 0.05, 1.0, -1.0, 0.01])
cal_retracted = cal.retract(delta_vec)
print(f"\nDelta vector: {delta_vec}")
print("\nRetracted cal_retracted:")
print(cal_retracted)

# Local coordinates: Find the delta between two calibrations
local_coords = cal.localCoordinates(cal_retracted)
print("\nLocal coordinates from cal_model to cal_retracted:")
print(local_coords)

Original cal_model:
K: 1500    0  320
   0 1600  240
   0    0    1
Baseline: 0.5


Delta vector: [ 1.e+01  2.e+01  5.e-02  1.e+00 -1.e+00  1.e-02]

Retracted cal_retracted:
K: 1510 0.05  321
   0 1620  239
   0    0    1
Baseline: 0.51


Local coordinates from cal_model to cal_retracted:
[ 1.e+01  2.e+01  5.e-02  1.e+00 -1.e+00  1.e-02]

Relationship with `StereoCamera`¶

The Cal3_S2Stereo object should be used with the StereoCamera class. A StereoCamera combines a Pose3 (the pose of the left camera) and a Cal3_S2Stereo (holding the calibration parameters) to project 3D points into the stereo image plane, producing a StereoPoint2 which contains the left and right image coordinates ( $u_L, u_R, v$ ). Below is a basic example; please look at the user guide of StereoCamera for a more thorough explanation.

fx, fy, s, u0, v0, b = 1000, 1000, 0, 640, 360, 0.5
stereo_calibration = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)

# Define the pose of the stereo camera rig in the world frame (left camera's pose)
camera_pose = gtsam.Pose3(gtsam.Rot3.Ypr(-np.pi/2, 0, -np.pi/2), [0, 0, 5.0])

# Create a StereoCamera object
stereo_camera = gtsam.StereoCamera(camera_pose, stereo_calibration)

# Define a 3D point in the world frame
p_world = gtsam.Point3(5, 3, 2)

# Project the 3D point into the stereo camera
p_stereo = stereo_camera.project(p_world)

print(f"3D Point in world frame: {p_world}")
print(f"StereoCamera pose:\n{stereo_camera.pose()}")
print(f"Stereo calibration:\n{stereo_camera.calibration()}")
print(f"\nProjected StereoPoint2 (u_L, u_R, v): {p_stereo}")

3D Point in world frame: [5. 3. 2.]
StereoCamera pose:
R: [
	6.12323e-17, 6.12323e-17, 1;
	-1, 3.7494e-33, 6.12323e-17;
	-0, -1, 6.12323e-17
]
t: 0 0 5

Stereo calibration:
K: 1000    0  640
   0 1000  360
   0    0    1
Baseline: 0.5


Projected StereoPoint2 (u_L, u_R, v): (40, -60, 960)

Additional Resources¶

The following resources provide more detailed explanations of camera calibration and stereo vision.

Article(s)¶

“5.3. Cameras for Robot Vision” by Dr. Frank Dellaert and Dr. Seth Hutchinson

Video(s)¶

“Simple Stereo | Camera Calibration” by Dr. Shree Nayar from Columbia University