This header defines factors for working with planar landmarks represented by gtsam.OrientedPlane3.
OrientedPlane3 represents a plane using normalized coefficients , where is the unit normal vector and is the distance from the origin along the normal.
Factors defined:
OrientedPlane3Factor: A binary factor connecting aPose3and anOrientedPlane3. It measures the difference between the plane parameters as observed from the given pose and a measured plane.OrientedPlane3DirectionPrior: A unary factor on anOrientedPlane3. It penalizes the difference between the plane’s normal direction and a measured direction (represented by the normal of a measuredOrientedPlane3). Note: The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured.
import gtsam
import numpy as np
from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values
from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior
from gtsam.symbol_shorthand import X, P1. OrientedPlane3Factor¶
Connects a Pose3 (camera/robot pose) and an OrientedPlane3 (landmark). The measurement is the plane as observed from the sensor frame.
The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (localCoordinates) with the measured plane.
# Ground truth plane (e.g., z=1 in world frame)
gt_plane_world = OrientedPlane3(0, 0, 1, 1)
# Ground truth pose
gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))
# Measurement: transform the world plane into the camera frame
# measured_plane = gt_plane_world.transform(gt_pose)
# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:
measured_plane_coeffs = gt_plane_world.planeCoefficients()
plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)
pose_key = X(0)
plane_key = P(0)
plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)
plane_factor.print("OrientedPlane3Factor: ")
# Evaluate error
values = Values()
values.insert(pose_key, gt_pose)
values.insert(plane_key, gt_plane_world)
error1 = plane_factor.error(values)
print(f"\nError at ground truth: {error1}")
# Evaluate with slightly different plane estimate
noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)
values.update(plane_key, noisy_plane)
error2 = plane_factor.error(values)
print(f"\nError with noisy plane: {error2}")OrientedPlane3Factor:
OrientedPlane3Factor Factor (x0, p0)
Measured Plane : 0 0 1 1
isotropic dim=3 sigma=0.05
Error at ground truth: 0.0
Error with noisy plane: 0.6469041114912286
2. OrientedPlane3DirectionPrior¶
A unary factor that puts a prior on the direction (normal vector) of an OrientedPlane3. The distance component (d) of the measured plane is ignored.
The error is the difference between the estimated plane’s normal and the measured plane’s normal, but as directions only have 2 DOF, the noise model also has to have dimension 2.
# Measured direction prior (e.g., plane normal is close to world Z axis)
measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored
direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.02)
prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)
prior_factor.print("OrientedPlane3DirectionPrior: ")
# Evaluate error using the 'noisy_plane' from the previous step
error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane
print(f"\nError for prior on noisy_plane: {error_prior}")
# Evaluate error for ground truth plane
values.update(plane_key, gt_plane_world)
error_prior_gt = prior_factor.error(values)
print(f"Error for prior on gt_plane_world: {error_prior_gt}")OrientedPlane3DirectionPrior:
OrientedPlane3DirectionPrior: Prior Factor on p0
Measured Plane : 0 0 1 0
isotropic dim=2 sigma=0.02
Error for prior on noisy_plane: 0.2550239722533919
Error for prior on gt_plane_world: 0.0