Class MarginalCostFunction

Inheritance Relationships

Base Type

  • public ceres::CostFunction

Class Documentation

class MarginalCostFunction : public ceres::CostFunction

Implements a cost function designed for precomputed marginal distributions.

The cost function is of the form: 

        ||                                                                        ||^2
cost(x) = || A1 * (x1 - x1_bar) + A2 * (x2 - x2_bar) + … + An * (xn - xn_bar) + b || || ||

where, the A matrices and the b vector are fixed, x_bar is the linearization point used when calculating the A matrices and b vector, and the minus operator in (x - x_bar) is provided by the variable’s local parameterization.

The A matrices can have any number of rows, but they must all be the same. The number of columns of each A matrix must match the associated variable’s local parameterization size, and the number of rows of each x_bar must match the associated variable’s global size. The cost function will have the same number of residuals as the rows of A.

Public Functions

MarginalCostFunction(const std::vector<fuse_core::MatrixXd> &A, const fuse_core::VectorXd &b, const std::vector<fuse_core::VectorXd> &x_bar, const std::vector<fuse_core::LocalParameterization::SharedPtr> &local_parameterizations)

Construct a cost function instance.

Parameters:

  • A – [in] The A matrix of the marginal cost (of the form A*(x - x_bar) + b)

  • b – [in] The b vector of the marginal cost (of the form A*(x - x_bar) + b)

  • x_bar – [in] The linearization point of the involved variables

  • local_parameterizations – [in] The local parameterization associated with the variable

virtual ~MarginalCostFunction() = default

Destructor.

bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override

Compute the cost values/residuals, and optionally the Jacobians, using the provided variable/parameter values.