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.

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)