KnownLandmarkSquaredRangeFactor

KnownLandmarkSquaredRangeFactor models a range measurement between an $N$ -dimensional Euclidean point $x$ and a fixed known landmark $L$ . Unlike KnownLandmarkFactor, it operates directly on a Euclidean point state and does not involve pose or orientation.

A conventional range residual is

e_{\text{range}}(x) = \|x - L\| - d,

(1)

where $d$ is the measured range. Note the residual is not polynomial in the state because the Euclidean norm contains a square root. Instead, KnownLandmarkSquaredRangeFactor uses the squared-range residual

e_{\text{squared}}(x) = \|x - L\|^2 - d^2,

(2)

which is quadratic in $x$ .

The squared-range form is suitable for lifted QCQP formulations and subsequent semidefinite relaxations. If the semidefinite relaxation is tight, meaning that the lifted solution has the required rank, the relaxed problem recovers a globally optimal solution to the original QCQP.

This factor currently provides the squared-range residual for standard GTSAM nonlinear optimization. QCQP/certifiable conversion is not yet implemented for this factor.

For additional information on GTSAM’s QCQP-to-SDP and chordal-sparsity framework, refer to Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs.

GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved 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 numpy as np
import gtsam

Wrapper names¶

The C++ factor is templated on the Euclidean point dimension N. Python exposes the 2D and 3D instantiations KnownLandmarkSquaredRangeFactor2 and KnownLandmarkSquaredRangeFactor3, respectively.

Prepare one known-landmark range measurement¶

Each factor needs four inputs:

the point key k;
a fixed world-frame landmark wL;
the measured range R; and
a noise model describing uncertainty in the squared-range residual.

The residual $e$ is a scalar, so the corresponding noise model is one-dimensional, with scalar standard deviation $\sigma$ and covariance $\sigma^2$ .

k = 0
wL = np.array([2.0, 3.0, 6.0])
R = 7.1

sigma = 0.1  # squared-range residual standard deviation [m^2]
model = gtsam.noiseModel.Isotropic.Sigma(1, sigma)

Localization with `KnownLandmarkSquaredRangeFactor`¶

Add the factor to an ordinary nonlinear graph and insert an initial point estimate under the same key.

graph = gtsam.NonlinearFactorGraph()
graph.add(
    gtsam.KnownLandmarkSquaredRangeFactor3(k, wL, R, model)
)

wX_initial = np.array([-0.1, 0.2, -0.05])
initial_wXs = gtsam.Values()
initial_wXs.insert(k, wX_initial)

Build a well-constrained graph¶

A single range measurement does not uniquely determine the point state. In 2D, it constrains the point to a circle centered at the known landmark; in 3D, it constrains the point to a sphere. In a real localization graph, add factors for several distinct known landmarks and, where appropriate, point priors or other factors. Each observation should carry its own R and noise model. Reusing the same point key connects all observations made from that point.

A known landmark is fixed factor data, not a variable in Values.

Practical checklist¶

Use KnownLandmarkSquaredRangeFactor2 or KnownLandmarkSquaredRangeFactor3 for point localization from ranges to fixed, known landmarks.
Express the state point and landmark in the same coordinate frame.
Pass the ordinary non-negative measured range R; the factor squares it internally.
Supply a one-dimensional noise model, since the squared-range residual is scalar.
Interpret the noise model as uncertainty on the squared-range residual, not directly on the range measurement.
Add measurements from enough geometrically distinct landmarks, or other factors, to fully constrain the point state.
Keep known landmarks as fixed factor data; they are not variables in Values.

KnownLandmarkSquaredRangeLocalizationExample demonstrates 3D point localization from noisy range measurements to fixed known landmarks.
KnownLandmarkFactor documents Cartesian observations of fixed known landmarks using pose states.