VectorNormFactor - GTSAM Docs

This API walkthrough introduces the wrapped classes VectorNormFactor declared by VectorNormFactor.h and demonstrates their principal Python operations.

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
from gtsam.symbol_shorthand import L, X

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.

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