Using manif with Ceres
While the manif package computes Jacobians by differentiating with respect to a
local perturbation on the tangent space, many non-linear solvers
(including Ceres) expect them to be differentiated
with respect to the underlying representation vector of the group element
(e.g. wrt the quaternion parameters vector
for ).
Considering,
: a group element (e.g. of SO(3)),
: the error vector, defined tangent to the group at
,
: an error function,
one is interested in linearizing the error function,
Therefore we have to compute the Jacobian
that is, the Jacobian of
with respect to a perturbation on the tangent space.
With this, the
state update is computed on the manifold's tangent space.
In the Ceres framework, the computation of this Jacobian is decoupled
in two folds as explained hereafter.
The following terminology should sound familiar to Ceres users.
Ceres CostFunction
is the class representing and implementing a function
, e.g.
class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
public:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
const double x = parameters[0][0];
residuals[0] = 10 - x;
// Compute the Jacobian if asked for.
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1;
}
return true;
}
};It produces the Jacobian,
In Ceres, a LocalParameterization can be associated to a state.
Eigen::Quaterniond my_state;
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Add the state to Ceres problem
problem->AddParameterBlock(my_state.data(), 4);
// Associate a LocalParameterization to the state vector
problem_->SetParameterization(my_state.data(),
new EigenQuaternionParameterization() );The LocalParameterization class (and derived) performs the state update step
of the optimization - the
operation. While the function operates for any
, its Jacobian is evaluated for
thus providing the Jacobian,
Once both the CostFunction and LocalParameterization's Jacobians are evaluated,
Ceres internally computes (1) with the following product,
Voila.
The intermediate Jacobians (2-3) that Ceres requires are not available in manif
since it provide directly the final Jacobian detailed in (1).
However, one still wants to use manif with his Ceres-based project.
For this reason, manif is compliant with Ceres
auto-differentiation and the
ceres::Jet type
to compute (2-3).
Below are presented two small examples illustrating how manif can be used
with Ceres.
Is shown here how one can implement a
ceres::LocalParameterization-derived class using manif,
that does the
for any group implemented in
manif passed as a template parameter.
template <typename _LieGroup>
class CeresLocalParameterization
{
using LieGroup = _LieGroup;
using Tangent = typename _LieGroup::Tangent;
template <typename _Scalar>
using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
template <typename _Scalar>
using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
public:
CeresLocalParameterizationFunctor() = default;
virtual ~CeresLocalParameterizationFunctor() = default;
template<typename T>
bool operator()(const T* state_raw,
const T* delta_raw,
T* state_plus_delta_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
const Eigen::Map<const TangentTemplate<T>> delta(delta_raw);
Eigen::Map<LieGroupTemplate<T>> state_plus_delta(state_plus_delta_raw);
state_plus_delta = state + delta;
return true;
}
};
//
...
// Some typedef helpers
using CeresLocalParameterizationSO2 = CeresLocalParameterizationFunctor<SO2d>;
using CeresLocalParameterizationSE2 = CeresLocalParameterizationFunctor<SE2d>;
using CeresLocalParameterizationSO3 = CeresLocalParameterizationFunctor<SO3d>;
using CeresLocalParameterizationSE3 = CeresLocalParameterizationFunctor<SE3d>;This example highlights the use of the predefined Ceres
helper classes available with manif.
In this example, we compute an average point from 4 points in SE2.
// Tell ceres not to take ownership of the raw pointers
ceres::Problem::Options problem_options;
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
ceres::Problem problem(problem_options);
// We use a first manif helper that creates a ceres cost-function.
// The cost function computes the distance between
// the desired state and the current state
// Create 4 objectives which are 'close' in SE2.
std::shared_ptr<ceres::CostFunction> obj_pi_over_4 = manif::make_objective_autodiff<SE2d>(3, 3, M_PI/4.);
std::shared_ptr<ceres::CostFunction> obj_3_pi_over_8 = manif::make_objective_autodiff<SE2d>(3, 1, 3.*M_PI/8.);
std::shared_ptr<ceres::CostFunction> obj_5_pi_over_8 = manif::make_objective_autodiff<SE2d>(1, 1, 5.*M_PI/8.);
std::shared_ptr<ceres::CostFunction> obj_3_pi_over_4 = manif::make_objective_autodiff<SE2d>(1, 3, 3.*M_PI/4.);
SE2d average_state(0,0,0);
/////////////////////////////////
// Add residual blocks to ceres problem
problem.AddResidualBlock( obj_pi_over_4.get(),
nullptr,
average_state.data() );
problem.AddResidualBlock( obj_3_pi_over_8.get(),
nullptr,
average_state.data() );
problem.AddResidualBlock( obj_5_pi_over_8.get(),
nullptr,
average_state.data() );
problem.AddResidualBlock( obj_3_pi_over_4.get(),
nullptr,
average_state.data() );
// We use a second manif helper that creates a ceres local parameterization
// for our optimized state block.
std::shared_ptr<ceres::LocalParameterization>
auto_diff_local_parameterization =
manif::make_local_parametrization_autodiff<SE2d>();
problem.SetParameterization( average_state.data(),
auto_diff_local_parameterization.get() );
// Run the solver!
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "summary:\n" << summary.FullReport() << "\n";
std::cout << "Average state:\nx:" << average_state.x()
<< "\ny:" << average_state.y()
<< "\nt:" << average_state.angle()
<< "\n\n";