ManifoldEvaluationFactor

Overview¶

ManifoldEvaluationFactor is a unary factor for manifold-valued measurements such as Rot2, Rot3, Pose2, or Pose3. It evaluates a vector-valued basis and compares the result in local coordinates on the manifold.

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

See LICENSE for the license information

# Install GTSAM from pip if running in Google Colab
try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass  # Not in Colab

Key Functionality / API¶

ManifoldEvaluationFactor(key, z, model, N, x) constrains the manifold value at x.
Optional interval parameters a, b scale the basis domain.

C++ example¶

auto model = gtsam::noiseModel::Isotropic::Sigma(3, 0.05);
size_t N = 6;
double x = 0.1;
gtsam::Rot3 z = gtsam::Rot3::RzRyRx(0.1, 0.2, 0.3);
gtsam::ManifoldEvaluationFactor<gtsam::Chebyshev2, gtsam::Rot3> factor(key, z, model, N, x);

Python Example¶

import numpy as np
import gtsam

names = [n for n in dir(gtsam) if n.startswith("ManifoldEvaluationFactor")]
print("Available ManifoldEvaluationFactor wrappers:")
for name in names:
    print("  ", name)

selected = None
kind = None
for preferred in ["Rot2", "Pose2", "Rot3", "Pose3"]:
    for name in names:
        if preferred in name:
            selected = name
            kind = preferred
            break
    if selected is None:
        print("No ManifoldEvaluationFactor wrapper found; skipping execution.")
    else:
        cls = getattr(gtsam, selected)
        if kind == "Rot2":
            z = gtsam.Rot2(0.3)
            dim = 1
        elif kind == "Rot3":
            z = gtsam.Rot3.RzRyRx(0.1, 0.2, 0.3)
            dim = 3
        elif kind == "Pose2":
            z = gtsam.Pose2(1.0, 0.0, 0.1)
            dim = 3
        else:
            z = gtsam.Pose3(gtsam.Rot3.RzRyRx(0.1, 0.2, 0.3), np.array([1.0, 0.0, 0.0]))
            dim = 6

        model = gtsam.noiseModel.Isotropic.Sigma(dim, 0.1)
        key = gtsam.symbol('x', 0)
        factor = cls(key, z, model, 6, 0.1)
        print(f"Created {selected} with dim {factor.dim()}")

Available ManifoldEvaluationFactor wrappers:
   ManifoldEvaluationFactorChebyshev1BasisPose2
   ManifoldEvaluationFactorChebyshev1BasisPose3
   ManifoldEvaluationFactorChebyshev1BasisRot2
   ManifoldEvaluationFactorChebyshev1BasisRot3
   ManifoldEvaluationFactorChebyshev2BasisPose2
   ManifoldEvaluationFactorChebyshev2BasisPose3
   ManifoldEvaluationFactorChebyshev2BasisRot2
   ManifoldEvaluationFactorChebyshev2BasisRot3
   ManifoldEvaluationFactorChebyshev2Pose2
   ManifoldEvaluationFactorChebyshev2Pose3
   ManifoldEvaluationFactorChebyshev2Rot2
   ManifoldEvaluationFactorChebyshev2Rot3
   ManifoldEvaluationFactorFourierBasisPose2
   ManifoldEvaluationFactorFourierBasisPose3
   ManifoldEvaluationFactorFourierBasisRot2
   ManifoldEvaluationFactorFourierBasisRot3
Created ManifoldEvaluationFactorChebyshev1BasisRot2 with dim 1
Created ManifoldEvaluationFactorChebyshev1BasisPose2 with dim 3
Created ManifoldEvaluationFactorChebyshev1BasisRot3 with dim 3
Created ManifoldEvaluationFactorChebyshev1BasisPose3 with dim 6

Source¶

BasisFactors.h