VectorNormFactor - GTSAM Docs

Overview¶

VectorNormFactor<N> is a unary factor constraining the norm of an N-dimensional vector-space variable to a given value, with error

e = \lVert v \rVert - \text{norm}, \qquad \frac{\partial e}{\partial v} = \frac{v^T}{\lVert v \rVert}.

(1)

The typical use is a magnitude pseudo-observation on a physical vector whose direction and magnitude are entangled in a single variable:

an estimated gravity vector, paired with ImuFactorWithGravityVector (Lupton and Sukkarieh, 2012), or
a local magnetic field vector estimated with MagFactor2, whose magnitude is often known from the NOAA tables even when its direction in the nav frame is not.

Add it once per variable: adding it per measurement factor would count the same prior knowledge multiple times. The N=3 instantiation is available in Python as VectorNormFactor3.

Warning: the error is not differentiable at $\lVert v \rVert = 0$ ; the Jacobian is zero there, so a variable initialized at exactly zero receives no gradient from this factor and will not move. Always initialize with a non-zero guess.

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

See LICENSE for the license information

# Install gtsam-develop if not installed
try:
    import gtsam
except ImportError:
    %pip install --quiet gtsam-develop

Usage Example¶

A loose vector prior fixes the direction; the norm factor pulls the magnitude to the target along that direction.

import numpy as np

import gtsam
from gtsam.symbol_shorthand import G

graph = gtsam.NonlinearFactorGraph()
direction = np.array([1.0, 1.0, 1.0]) / np.sqrt(3.0)
graph.addPriorPoint3(G(0), gtsam.Point3(8.0 * direction),
                     gtsam.noiseModel.Isotropic.Sigma(3, 10.0))
graph.add(gtsam.VectorNormFactor3(
    G(0), 9.81, gtsam.noiseModel.Isotropic.Sigma(1, 1e-3)))

values = gtsam.Values()
values.insert(G(0), gtsam.Point3(8.0 * direction))  # non-zero initial guess

result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()
optimized = result.atPoint3(G(0))
print(f"optimized vector: {np.round(optimized, 4)}")
print(f"norm: {np.linalg.norm(optimized):.5f} (target 9.81)")

optimized vector: [5.6638 5.6638 5.6638]
norm: 9.81000 (target 9.81)

Key Functionality / API¶

VectorNormFactor3(key, norm, noiseModel): constrain a Point3/Vector3 variable.
norm(): the target norm.
With a noiseModel::Constrained model it acts as a hard constraint under QR-based elimination (not recommended with Cholesky). If the norm is known exactly, prefer a parametrization that fixes it instead, eg. Unit3 with a fixed magnitude (ImuFactorWithGravityDirection).

Source¶

VectorNormFactor.h