Error Terms¶

ErrorTermBase¶

namespace Tangent

template<int ResidualDimension, typename ScalarType, typename ...IndependentVariables> class ErrorTermBase<Scalar<ScalarType>, Dimension<ResidualDimension>, VariableGroup<IndependentVariables...>>¶

#include <ErrorTermBase.h>

Base class for error terms that depend on a set of independent variables.

Stores the linearized error term (Jacobians and residual) and keys to access the variables. Derived classes must implement an evaluate function:

void evaluate(VariableContainer<Variables...>& variables, bool relinearize);

The evaluate function must:

Compute the residual vector and store it in the residual member
If relinearize is true, compute the Jacobian of the residual with respect to each independent variable and store them in variableJacobians
Set linearizationValid to true if linearization succeeded, false otherwise

Template Parameters:

ScalarType – The floating point type (typically float or double)
ResidualDimension – The dimension of the residual vector
IndependentVariables... – The variable types this error term depends on

Subclassed by Tangent::AutoDiffErrorTerm< Derived, ScalarType, ResidualDimension, IndependentVariables >

Public Types

typedef ScalarType scalar_type¶

using VariableJacobians = std::tuple<Eigen::Matrix<ScalarType, ResidualDimension, IndependentVariables::dimension>...>¶

using VariableKeys = std::tuple<VariableKey<IndependentVariables>...>¶

using VariablePointers = std::tuple<IndependentVariables*...>¶

Public Functions

template<typename ...Variables> inline bool checkVariablePointerConsistency(VariableContainer<Variables...> &variableContainer)¶: Verifies that the pointers stored are equal to the true variable location.

template<typename ...Variables> inline void updateVariablePointers(VariableContainer<Variables...> &variableContainer)¶: Extracts the most recent pointer to each of the variables using their key and updates them.

Public Members

Eigen::Matrix<ScalarType, ResidualDimension, ResidualDimension> information¶: This is the information matrix for this error term.

bool linearizationValid = false¶: This flag is used to let the user know if the error term was linearized successfully.

Eigen::Matrix<ScalarType, ResidualDimension, 1> residual¶: This is the most recent residual computed for the error term.

VariableJacobians variableJacobians¶: These jacobians are from the most recent linearization.

VariableKeys variableKeys¶

These are the keys used to access the variables over time.

These keys can only be invalidated is the variable is removed or overwritten.

VariablePointers variablePointers¶

These pointers are used to avoid the indirection of the slotmap.

These pointers are invalidated every time a key is added or removed from the slot map.

Public Static Attributes

static const int residual_dimension = ResidualDimension ¶: Compile time acces to the error term’s dimension.

AutoDiffErrorTerm¶

namespace Tangent

template<typename Derived, typename ScalarType, int ResidualDimension, typename ...IndependentVariables> class AutoDiffErrorTerm : public Tangent::ErrorTermBase<Scalar<ScalarType>, Dimension<ResidualDimension>, VariableGroup<IndependentVariables...>>¶

#include <AutoDiffErrorTerm.h>

Base class for error terms with automatic differentiation.

Users derive from this class using CRTP and implement a templated computeError() method that works with any scalar type. The base class automatically computes Jacobians using dual numbers (Jets) when relinearize is true.

Usage example:

class MyReprojectionError
    : public AutoDiffErrorTerm<MyReprojectionError, double, 2,
                                SE3, SE3, InverseDepth> {
public:
    Eigen::Vector2d measurement;
    Eigen::Vector2d bearing;

    MyReprojectionError(VariableKey<SE3> hostKey,
                        VariableKey<SE3> targetKey,
                        VariableKey<InverseDepth> depthKey,
                        Eigen::Vector2d z,
                        Eigen::Vector2d b)
        : measurement(z), bearing(b) {
        std::get<0>(this->variableKeys) = hostKey;
        std::get<1>(this->variableKeys) = targetKey;
        std::get<2>(this->variableKeys) = depthKey;
    }

    // This single templated method works for both double and Jet types.
    // The template parameters are the "lifted" types of each variable:
    // - SE3 -> LiftedSE3<T, N> when T=Jet, or Sophus::SE3d when T=double
    // - InverseDepth -> Jet<T, N> when T=Jet, or double when T=double
    template <typename T, typename Host, typename Target, typename Depth>
    Eigen::Matrix<T, 2, 1> computeError(const Host& host,
                                         const Target& target,
                                         const Depth& depth) const {
        // Implement error computation using the lifted variables
        // The same code works for both value and autodiff evaluation
    }
};

The derived class must implement:

template <typename T, typename... LiftedVars>
Eigen::Matrix<T, ResidualDimension, 1> computeError(
    const LiftedVars&... vars) const;

Where:

T is either ScalarType (for residual-only) or JetType (for autodiff)
LiftedVars are the lifted representations of each variable

Template Parameters:

Derived – The CRTP derived class type
ScalarType – The underlying scalar type (typically double)
ResidualDimension – The dimension of the residual vector
IndependentVariables... – The variable types this error term depends on

Public Types

using Base = ErrorTermBase<Scalar<ScalarType>, Dimension<ResidualDimension>, VariableGroup<IndependentVariables...>>¶

using JetType = Jet<ScalarType, TotalDim>¶

Public Functions

template<typename ...Variables> inline void evaluate(VariableContainer<Variables...> &variables, bool relinearize)¶

Evaluate the error term, computing residual and optionally Jacobians.

When relinearize is false, only the residual is computed using double arithmetic (fast path). When relinearize is true, Jets are used to automatically compute both residual and Jacobians.

Parameters:

variables – The variable container
relinearize – If true, compute Jacobians via autodiff

Public Members

Eigen::Matrix<ScalarType, ResidualDimension, ResidualDimension> information¶: This is the information matrix for this error term.

bool linearizationValid¶: This flag is used to let the user know if the error term was linearized successfully.

Eigen::Matrix<ScalarType, ResidualDimension, 1> residual¶: This is the most recent residual computed for the error term.

VariableJacobians variableJacobians¶: These jacobians are from the most recent linearization.

VariableKeys variableKeys¶

These are the keys used to access the variables over time.

These keys can only be invalidated is the variable is removed or overwritten.

VariablePointers variablePointers¶

These pointers are used to avoid the indirection of the slotmap.

These pointers are invalidated every time a key is added or removed from the slot map.

Public Static Attributes

static constexpr size_t NumVariables = sizeof...(IndependentVariables)¶

static constexpr int TotalDim = internal::TotalDimension<IndependentVariables...>::value¶

Protected Static Functions

template<size_t I> static inline constexpr int getVariableDimension()¶: Get the dimension of variable at index I.

template<size_t I> static inline constexpr int getVariableOffset()¶: Get the dimension offset for variable at index I.

Private Functions

template<size_t... Is> inline Eigen::Matrix<ScalarType, ResidualDimension, 1> computeWithDoubles(std::index_sequence<Is...>)¶: Compute residual using double arithmetic (no autodiff).

template<size_t... Is> inline Eigen::Matrix<JetType, ResidualDimension, 1> computeWithJets(std::index_sequence<Is...>)¶: Compute residual using Jets (with autodiff).

inline Derived &derived()¶: Cast to derived class.

inline const Derived &derived() const¶

template<size_t... Is> inline void extractAllJacobians(const Eigen::Matrix<JetType, ResidualDimension, 1> &jetResidual, std::index_sequence<Is...>)¶: Extract Jacobians for all variables.

template<size_t I> inline void extractJacobianForVariable(const Eigen::Matrix<JetType, ResidualDimension, 1> &jetResidual)¶: Extract Jacobian for a single variable from Jet residual.

template<size_t I> inline auto liftVariableAtIndex() const¶: Lift a single variable to Jet space.

namespace internal¶

template<typename First, typename ...Rest> struct DimensionOffset<0, First, Rest...>¶

#include <AutoDiffErrorTerm.h>

Public Static Attributes

static constexpr int value = 0¶

template<int I, typename First, typename ...Rest> struct DimensionOffset<I, First, Rest...>¶

#include <AutoDiffErrorTerm.h>

Public Static Attributes

static constexpr int value = static_cast<int>(First::dimension) + DimensionOffset<I - 1, Rest...>::value¶

template<typename First, typename ...Rest> struct TotalDimension<First, Rest...>¶

#include <AutoDiffErrorTerm.h>

Public Static Attributes

static constexpr int value = static_cast<int>(First::dimension) + TotalDimension<Rest...>::value¶

template<> struct TotalDimension<>¶

#include <AutoDiffErrorTerm.h>

Public Static Attributes

static constexpr int value = 0¶

ErrorTermValidator¶

namespace Tangent

template<class ErrorTerm> class ErrorTermValidator¶

#include <ErrorTermValidator.h>

Numerically validates the jacobians of a given error term.

Public Functions

inline ErrorTermValidator(ErrorTerm errorTerm)¶

template<typename ...Variables> inline bool validate(VariableContainer<Variables...> &variableContainer)¶: Numerically differentiate the error term w.r.t to the independent variables and verify that the jacobians match.

Public Members

double delta = 1e-8¶

ErrorTerm errorTerm¶

double threshold = 1e-4¶

namespace internal

template<typename ...T> struct errorterm_validator_helper<std::tuple<VariableKey<T>...>>¶

#include <ErrorTermValidator.h>

Public Types

typedef VariableContainer<T...> variable_container¶

typedef std::tuple<T...> variables¶