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 error between the estimated value of a variable and its prior. You can think of the error as

e(x) = x \ominus \mu

(1)

where $x$ is the estimated value, and $\mu$ is the prior mean, and $\ominus$ is like subtraction. The error is then weighted by the noise model to form the contribution of this factor to the overall objective function.

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. *Note that very strong priors might make the condition number of the linear systems to be solved very high. In this case consider adding a [NonlinearEqualityFactor]
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

Example Notebooks¶

Remarks¶

The PriorFactor class is derived from ExtendedPriorFactor.

For vector spaces, we have

x \ominus \mu = x - \mu

(2)

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 $\cal G$ where $\text{localCoordinates}$ is implemented as $\text{Log}$ , the inverse of the exponential map $\text{Exp}$ , we have $$

x.\text{localCoordinates}(\mu) = \text{Log}(\mu^{-1} x)

Can't use function '$' in math mode at position 32: …eral manifolds $̲\cal M$, it mig…

However, for general manifolds $\cal M$, it might not be true that

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.