StereoCamera

A StereoCamera in GTSAM models a calibrated stereo camera rig composed of a Pose3 representing the left camera’s pose in the world frame and a Cal3_S2Stereo object that contains the intrinsic parameters of both cameras and the baseline between them.

It has two main functionalities:

Project a 3D point from the world frame into stereo image coordinates $(u_L, u_R, v)$ , represented by a StereoPoint2
Backproject a stereo measurement $(u_L, u_R, v)$ into a 3D point in the world frame, using the known pose and calibration of the camera.

In simulation, StereoCamera.project() allows you to generate synthetic measurements from known 3D points, which is useful for testing and verifying SLAM or structure-from-motion algorithms.

In real-world applications, stereo image pairs are used to extract matching pixel coordinates $(u_L, u_R, v)$ , which can then be passed to StereoCamera.backproject() to recover 3D points based on the pose of the camera.

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

Initialization¶

A StereoCamera can be initialized in two ways:

# Default constructor
stereo_camera_default = gtsam.StereoCamera()
print("Default constructor:")
print(stereo_camera_default)

# With left-camera pose and shared calibration
left_camera_pose = gtsam.Pose3(gtsam.Rot3.Ypr(-np.pi/2, 0, -np.pi/2), gtsam.Point3(0, 0, 5.0))
fx, fy, s, u0, v0, b = 1000, 1000, 0, 640, 360, 0.5
K = gtsam.Cal3_S2Stereo(fx, fy, s, u0, v0, b)
stereo_camera = gtsam.StereoCamera(left_camera_pose, K)
print("From left-camera pose and shared calibration K:")
print(stereo_camera)

Default constructor:
.camera. R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 0 0 0
.calibration. K: 1 0 0
0 1 0
0 0 1
Baseline: 1

From left-camera pose and shared calibration K:
.camera. R: [
	6.12323e-17, 6.12323e-17, 1;
	-1, 3.7494e-33, 6.12323e-17;
	-0, -1, 6.12323e-17
]
t: 0 0 5
.calibration. K: 1000    0  640
   0 1000  360
   0    0    1
Baseline: 0.5

Properties¶

StereoCamera properties can be accessed by the following member functions:

pose(): Returns a Pose3 object representing the pose of the left camera in the world frame.
calibration(): Returns a Cal3_S2Stereo object, which includes the instrinsic parameters shared by both cameras.
baseline(): Returns the baseline, the distance between left and right cameras.

camera_pose = stereo_camera.pose()
calibration = stereo_camera.calibration()
baseline = stereo_camera.baseline()

print("Stereo camera properties:")
print(f"Camera pose: {stereo_camera.pose()}")
print(f"Calibration: {stereo_camera.calibration()}")
print(f"Baseline: {stereo_camera.baseline()}")

Stereo camera properties:
Camera 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

Calibration: K: 1000    0  640
   0 1000  360
   0    0    1
Baseline: 0.5

Baseline: 0.5

Basic Operations¶

`project()`¶

The project() member function maps a 3D point in the world frame to 2D stereo image coordinates. It takes a Point3 and returns a StereoPoint2 containing the pixel coordinates $(u_L, u_R, v)$ observed by the left and right cameras.

This function is especially useful in simulation, where the 3D location of a point is known in advance. It enables the projection of ground-truth landmarks into pixel measurements based on the camera’s pose and calibration.

If the point lies behind the camera or is not visible in the stereo field of view, a StereoCheiralityException is thrown.

p_world = gtsam.Point3(5, 3, 2)
p_stereo = stereo_camera.project(p_world)

print(f"3D Point in world frame: {p_world}")
print(f"Projected StereoPoint2 (uL, uR, v): {p_stereo}")

3D Point in world frame: [5. 3. 2.]
Projected StereoPoint2 (uL, uR, v): (40, -60, 960)

`project2()`¶

The project2() member function is the same as project(), except that it can also calculate the Jacobians of the projection with respect to the camera pose (H1) and the 3D point (H2).

H1 = np.zeros((3, 6), order='F') # Jacobian w.r.t. camera pose
H2 = np.zeros((3, 3), order='F') # Jacobian w.r.t. point

p_stereo = stereo_camera.project2(p_world, H1, H2)

print(f"3D Point in world frame: {p_world}")
print(f"Projected StereoPoint2 (uL, uR, v): {p_stereo}")
print("Jacobian w.r.t. Pose (H1):\n", H1)
print("\nJacobian w.r.t. Point (H2):\n", H2)

3D Point in world frame: [5. 3. 2.]
Projected StereoPoint2 (uL, uR, v): (40, -60, 960)

Jacobian w.r.t. Pose (H1):
 [[ -360. -1360.   600.  -200.     0.  -120.]
 [ -420. -1420.   600.  -200.     0.  -140.]
 [ 1360.   360.   600.     0.  -200.   120.]]

Jacobian w.r.t. Point (H2):
 [[ 1.20000000e+02 -2.00000000e+02  7.34788079e-15]
 [ 1.40000000e+02 -2.00000000e+02  8.57252759e-15]
 [-1.20000000e+02 -7.34788079e-15 -2.00000000e+02]]

`backproject`¶

The backproject() member function performs the inverse operation of project(). Given a StereoPoint2 measurement $(u_L, u_R, v)$ , it reconstructs and returns the corresponding 3D point in the world frame as a Point3.

This function is especially useful in real-world applications, where stereo image pairs are used to extract pixel correspondences. When the stereo rig’s pose and calibration are known (which should be true if you are using a StereoCamera in the first place), backproject() allows recovering the estimated position of a point in space from its stereo measurement.

reprojected_point = stereo_camera.backproject(p_stereo)

print(f"Original Point: {p_world}")
print(f"Reprojected Point: {reprojected_point}")

Original Point: [5. 3. 2.]
Reprojected Point: [5. 3. 2.]

`backproject2()`¶

The backproject2() member function is the same as backproject(), except that it can also calculater the Jacobians of the backprojection with respect to the stereo measurement (H1) and the camera pose (H2).

H1 = np.zeros((3, 6), order='F') # Jacobian w.r.t. camera pose
H2 = np.zeros((3, 3), order='F') # Jacobian w.r.t. point

reprojected_point = stereo_camera.backproject2(p_stereo, H1, H2)

print(f"Original Point: {p_world}")
print(f"Reprojected Point: {reprojected_point}")
print("\nJacobian w.r.t. Pose (H1):\n", H1)
print("\nJacobian w.r.t. Point (H2):\n", H2)

Original Point: [5. 3. 2.]
Reprojected Point: [5. 3. 2.]

Jacobian w.r.t. Pose (H1):
 [[ 3.00000000e+00  3.00000000e+00 -3.67394040e-16  6.12323400e-17
   6.12323400e-17  1.00000000e+00]
 [ 1.83697020e-16 -5.00000000e+00  3.00000000e+00 -1.00000000e+00
   3.74939946e-33  6.12323400e-17]
 [ 5.00000000e+00  1.83697020e-16  3.00000000e+00 -0.00000000e+00
  -1.00000000e+00  6.12323400e-17]]

Jacobian w.r.t. Point (H2):
 [[-5.00000000e-02  5.00000000e-02  3.06161700e-19]
 [-3.50000000e-02  3.00000000e-02  1.87469973e-35]
 [ 3.00000000e-02 -3.00000000e-02 -5.00000000e-03]]

Manifold Operations¶

StereoCamera is a manifold, meaning it supports operations such as retract() and localCoordinates() that defines a continuous space of camera poses. These operations apply to the Pose3 component (the left camera’s pose), while the stereo calibration remains fixed.

This structure allows StereoCamera to be integrated into optimization routines where small updates to the camera pose are computed on the manifold.

print("Original StereoCamera:")
stereo_camera.print()

# Define a delta vector in the tangent space.
delta = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])

# Retract the camera pose.
retracted_camera = stereo_camera.retract(delta)
print("\nRetracted StereoCamera:")
retracted_camera.print()

# Calculate the local coordinates between the original and retracted cameras.
local_coords = stereo_camera.localCoordinates(retracted_camera)
print("\nLocal Coordinates:", local_coords)

Original StereoCamera:
.camera. R: [
	6.12323e-17, 6.12323e-17, 1;
	-1, 3.7494e-33, 6.12323e-17;
	-0, -1, 6.12323e-17
]
t: 0 0 5
.calibration. K: 1000    0  640
   0 1000  360
   0    0    1
Baseline: 0.5

Retracted StereoCamera:
.camera. R: [
	-0.18054, 0.127335, 0.97529;
	-0.935755, 0.283165, -0.210192;
	-0.302933, -0.950581, 0.0680313
]
t:   0.58716 -0.381202   4.47134
.calibration. K: 1000    0  640
   0 1000  360
   0    0    1
Baseline: 0.5

Local Coordinates: [0.1 0.2 0.3 0.4 0.5 0.6]

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

Source¶

GTSAM Docs

SO3