PriorFactor - GTSAM Docs

Overview¶

The PriorFactor represents a prior belief about a variable in the form of a Gaussian distribution. This class is crucial for incorporating prior knowledge into the optimization process, which can significantly enhance the accuracy and robustness of the solutions.

Key Functionalities¶

PriorFactor Construction¶

The PriorFactor is constructed by specifying a key, a prior value, and a noise model. The key identifies the variable in the factor graph, the prior value represents the expected value of the variable, and the noise model encapsulates the uncertainty associated with this prior belief.

Error Calculation¶

The primary role of the PriorFactor is to compute the local error between the estimated value of a variable and its prior. If $x$ is the estimated value and $\mu$ is the prior mean, the unwhitened residual is

e(x) = -\operatorname{Local}(x,\mu).

(1)

The noise model whitens this vector, and the resulting scalar loss contributes to the graph’s objective.

Adding to a Factor Graph¶

NonlinearFactorGraph has a templated method addPrior<T> that provides a convenient way to add priors.

Usage Considerations¶

Noise Model: The choice of noise model is critical as it determines how strongly the prior is enforced. A tighter noise model implies a stronger belief in the prior. Very strong priors can make the linear systems ill-conditioned; in that case, consider using NonlinearEquality.
Integration with Other Factors: The PriorFactor is typically used in conjunction with other factors that model the system dynamics and measurements. It helps anchor the solution, especially in scenarios with limited or noisy measurements.
Applications: Common applications include SLAM (Simultaneous Localization and Mapping), where priors on initial poses or landmarks can significantly improve map accuracy and convergence speed.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np

X = gtsam.symbol_shorthand.X

Noise model and coordinate frame¶

The covariance passed to PriorFactor is the covariance of its local residual $e(x)=-\operatorname{Local}(x,\mu)$ . For vector spaces this is simply $x-\mu$ . For Lie groups whose origin chart uses the logarithm map,

e(x)=\operatorname{Log}(\mu^{-1}x),

(2)

so a noisy value can be written using GTSAM’s right-hand retraction as

x=\operatorname{Retract}_{\mu}(\eta)=\mu\,\operatorname{Exp}(\eta), \qquad \eta\sim\mathcal N(0,\Sigma).

(3)

Consequently, when $\mu={}^WT_B$ is a Pose2 or Pose3 prior, $\eta$ and $\Sigma$ are expressed in the local/body axes of the prior pose $B$ , not in the world axes $W$ . The mean pose itself is still expressed in $W$ . Saying that the covariance lives in a tangent space describes its algebraic representation; the right-hand retraction determines its physical coordinate frame.

For Pose3, tangent vectors and covariance matrices use the order $[\omega_x,\omega_y,\omega_z,\rho_x,\rho_y,\rho_z]$ : rotation first, then translation. The rotational components are local axis-angle increments, not roll, pitch, and yaw. Pose2 uses $[\delta x,\delta y,\delta\theta]$ . noiseModel.Diagonal.Sigmas expects standard deviations; use Variances or Gaussian.Covariance for variances or a full covariance matrix.

For a general manifold, or a type configured with a different origin chart, the literal residual $-\operatorname{Local}(x,\mu)$ is authoritative; see the remarks below.

# A right-hand perturbation of a Pose3 prior is recovered as its residual.
prior_mean = gtsam.Pose3(
    gtsam.Rot3.RzRyRx(0.2, -0.1, 0.3), gtsam.Point3(1.0, 0.5, -0.2)
)
delta_b = np.array([0.01, -0.02, 0.03, 0.10, -0.05, 0.02])
perturbed_pose = prior_mean.retract(delta_b)

unit_noise_6 = gtsam.noiseModel.Unit.Create(6)
prior_factor = gtsam.PriorFactorPose3(X(0), prior_mean, unit_noise_6)
values = gtsam.Values()
values.insert(X(0), perturbed_pose)

computed_delta_b = prior_factor.unwhitenedError(values)
np.testing.assert_allclose(computed_delta_b, delta_b, atol=1e-9)
print("Prior-body perturbation:", computed_delta_b)

Example Notebooks¶

Remarks¶

The PriorFactor class is derived from ExtendedPriorFactor.

For vector spaces, we have

x \ominus \mu = x - \mu

(4)

but the error is actually defined as $$

x.\text{localCoordinates}(\mu) $ $where `localCoordinates` is the inverse of `retract`. We implement it this way, because the Jacobian at$ x$ is identity, which is computationally advantageous.

For Lie groups where localCoordinates is implemented with the logarithm map, the inverse of the exponential map, we have $$

x.\text{localCoordinates}(\mu) = \text{Log}(\mu^{-1} x)
$However, for general manifolds, it might not be true that$
(5)
x.\text{localCoordinates}(\mu) = \mu.\text{localCoordinates}(x) $$ which is actually problematic (a moving target). However, we still choose to implement the prior this way, as otherwise “Manifold architects” are forced to implement the Jacobian.