Skip to article frontmatterSkip to article content

PlanarProjectionFactor

The PlanarProjectionFactor variants provide camera projection factors specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras). They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:

  • The robot’s 2D pose (Pose2 wTb: body in world frame) in the ground plane.
  • The camera’s fixed 3D pose relative to the robot’s body frame (Pose3 bTc: body-to-camera).
  • The camera’s intrinsic calibration (including distortion, typically Cal3DS2 or similar).
  • The 3D landmark position in the world frame.

The core projection logic involves converting the Pose2 wTb to a Pose3 assuming z=0 and yaw=theta, composing it with bTc to get the world-to-camera pose wTc, and then using a standard PinholeCamera model to project the landmark.

Variants:

  • PlanarProjectionFactor1: Unknown is robot Pose2 (wTb). Landmark, bTc, and calibration are fixed. Useful for localization.
  • PlanarProjectionFactor2: Unknowns are robot Pose2 (wTb) and Point3 landmark. bTc and calibration are fixed. Useful for planar SLAM.
  • PlanarProjectionFactor3: Unknowns are robot Pose2 (wTb), camera offset Pose3 (bTc), and Cal3DS2 calibration. Landmark is fixed. Useful for calibration.

Open In Colab

import gtsam
import numpy as np
from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,
                   PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)
from gtsam.symbol_shorthand import X, L, C, O

1. PlanarProjectionFactor1 (Localization)

Used when the landmark, camera offset (bTc), and calibration (calib) are known, and we want to estimate the robot’s Pose2 (wTb).

# Known parameters
landmark_pt = Point3(2.0, 0.5, 0.5)
body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2))  # Cam fwd = body +y
calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5)  # Pixels

# Assume ground truth pose and calculate expected measurement
gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)
gt_world_T_cam = Pose3(gt_pose2) * body_T_cam
gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)
measured_pt2 = gt_camera.project(landmark_pt)
print(f"Ground Truth Pose2: {gt_pose2}")
print(f"Calculated Measurement: {measured_pt2}")

# Create the factor
factor1 = PlanarProjectionFactor1(
    X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise
)
factor1.print("Factor 1: ")

# Evaluate error
values = Values()
values.insert(X(0), gt_pose2)  # Error should be zero here
error1_gt = factor1.error(values)
print(f"\nError at ground truth: {error1_gt}")

noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)
values.update(X(0), noisy_pose2)
error1_noisy = factor1.error(values)
print(f"Error at noisy pose: {error1_noisy}")
Ground Truth Pose2: (1, 0, 0.785398)

Calculated Measurement: [ 909.25565099 1841.1002863 ]
Factor 1:   keys = { x0 }
isotropic dim=2 sigma=1.5

Error at ground truth: 0.0
Error at noisy pose: 3317.6472637491106

2. PlanarProjectionFactor2 (Planar SLAM)

Used when the camera offset (bTc) and calibration (calib) are known, but both the robot Pose2 (wTb) and the Point3 landmark position are unknown.

factor2 = PlanarProjectionFactor2(
    X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise
)
factor2.print("Factor 2: ")

# Evaluate error
values = Values()
values.insert(X(0), gt_pose2)
values.insert(L(0), landmark_pt)  # Error should be zero
error2_gt = factor2.error(values)
print(f"\nError at ground truth: {error2_gt}")

noisy_landmark = Point3(2.1, 0.45, 0.55)
values.update(L(0), noisy_landmark)
error2_noisy = factor2.error(values)
print(f"Error with noisy landmark: {error2_noisy}")
Factor 2:   keys = { x0 l0 }
isotropic dim=2 sigma=1.5

Error at ground truth: 0.0
Error with noisy landmark: 8066.192649473802

3. PlanarProjectionFactor3 (Calibration)

Used when the landmark position is known, but the robot Pose2 (wTb), the camera offset Pose3 (bTc), and the Cal3DS2 calibration are unknown.

offset_key = O(0)
calib_key = C(0)

factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)
factor3.print("Factor 3: ")

# Evaluate error
values = Values()
values.insert(X(0), gt_pose2)
values.insert(offset_key, body_T_cam)
values.insert(calib_key, calib) # Error should be zero
error3_gt = factor3.error(values)
print(f"\nError at ground truth: {error3_gt}")

noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)
values.update(calib_key, noisy_calib)
error3_noisy = factor3.error(values)
print(f"Error with noisy calibration: {error3_noisy}")
Factor 3:   keys = { x0 o0 c0 }
isotropic dim=2 sigma=1.5

Error at ground truth: 0.0
Error with noisy calibration: 92.30212176019934
GTSAM Docs
OrientedPlane3 Factors
GTSAM Docs
PoseRotationPrior