Program Listing for File CeresNonlinearSolver.h#
↰ Return to documentation for file (include/Karana/SOADyn/CeresNonlinearSolver.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the EigenNonLinearSolver class and associated Eigen
* functors.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include "Karana/SOADyn/NonlinearSolver.h"
#include <ceres/ceres.h>
#include <unsupported/Eigen/NonLinearOptimization>
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
/**
* @brief CeresNonlinearSolver class manages a nonlinear problem and associated solver.
*/
class CeresNonlinearSolver : public NonlinearSolver {
public:
/// Solver types used by the Ceres nonlinear solver
enum SolverType {
/// Levenberg-Marquardt Newton-style method
LEVENBERG_MARQUARDT,
/// Powell's dogleg method
DOGLEG,
/// BFGS with line search
BFGS,
/// Automatically decide based on presence of joint stops
AUTO
};
/// Solver types used by the Ceres nonlinear solver
enum LinearSolverType {
/// Dense QR factorization: the most stable, but sometimes slower
DENSE_QR,
/// Dense Schur factorization: an alternative to QR
DENSE_SCHUR,
/// Dense Cholesky factorization: up to 2x faster than QR less numerically stable
DENSE_NORMAL_CHOLESKY
};
public:
/**
* @brief CeresNonlinearSolver constructor. This is a container that holds the information
* needed to solve a nonlinear problem.
*
* @param name The name of the nonlinear solver.
* @param input_dim The number of inputs.
* @param value_dim The number of values (outputs).
* @param f The cost function. This should take in a vector of inputs and output a
* vector of outputs.
* @param j The jacobian function. This should take in a vector of inputs and output a
* matrix of outputs.
* @param type The type of nonlinear solver to use.
*/
CeresNonlinearSolver(std::string_view name,
int input_dim,
int value_dim,
cost_fn f,
jac_fn j,
SolverType type = SolverType::DOGLEG);
/**
* @brief CeresNonlinearSolver constructor. This is a container that holds the
* information CeresNonlinearSolver needs to solve a nonlinear problem.
*
* @param name The name of the Nonlinear solver.
* @param input_dim The number of inputs.
* @param value_dim The number of values (outputs).
* @param f The cost function. This should take in a vector of inputs and output a
* vector of outputs.
* @param j The jacobian function. This should take in a vector of inputs and output a
* matrix of outputs.
* @param type The type of nonlinear solver to use.
* @return A ks_ptr to a new Nonlinear solver instance.
*/
static kc::ks_ptr<CeresNonlinearSolver> create(std::string_view name,
int input_dim,
int value_dim,
cost_fn f,
jac_fn j,
SolverType type = SolverType::AUTO);
/**
* @brief Get the non-linear solver type.
* @returns The current non-linear solver type.
*/
SolverType getSolverType() const;
/**
* @brief Set the nonlinear solver type.
* @param type New solver type to use.
*/
void setSolverType(const SolverType &type);
/**
* @brief Set the linear solver type to use within iterations.
* @param type New linear solver type to use.
*/
void setLinearSolverType(const LinearSolverType &type);
/**
* @brief Get a reference to the internal ceres options object for more advanced tuning
*
* @return ceres::Solver::Options& Reference to the options field; only use this for
* advanced tuning.
*/
ceres::Solver::Options &getMutableOptions();
protected:
/**
* @brief Internal method to ensure the solver is ready.
*/
virtual void _makeHealthy() override final;
/**
* @brief Build internal state for Ceres solver; this is done internally on _solve() calls.
*/
void _rebuildProblemStructure();
/**
* @brief Update upper and lower bounds on solution variables
*
* @param lower_bounds New lower bound to use (or empty vec for no bounds)
* @param upper_bounds New upper bound to use (or empty vec for no bounds)
*/
void _updateBounds(const km::Vec &lower_bounds, const km::Vec &upper_bounds);
private:
/// The type of nonlinear solver to use internally.
SolverType _solver_type;
/// The type of nonlinear solver to use internally.
LinearSolverType _linear_solver_type;
/// Cached state to use internally
km::Vec _x;
// Cached upper and lower bounds, used to avoid unneccesary updates
km::Vec _lower_bounds;
// Cached upper and lower bounds, used to avoid unneccesary updates
km::Vec _upper_bounds;
/// The ceres problem instance to use internally
kc::ks_ptr<ceres::Problem> _problem = nullptr;
/// The ceres options container to use internally
ceres::Solver::Options _options;
/// Summary containing results of latest run
ceres::Solver::Summary _summary;
/// Ceres context object to reuse more expensive data structures
kc::ks_ptr<ceres::Context> _context;
virtual void _solve(km::Vec &x, const km::Vec &lb, const km::Vec &ub) override final;
/**
* @brief Wrapper for Ceres cost functions to bind in Eigen
*
*/
class AnalyticCost : public ceres::CostFunction {
public:
AnalyticCost(int inputs, int outputs, cost_fn cost_func, jac_fn jac_func)
: _f(std::move(cost_func))
, _j(std::move(jac_func)) {
set_num_residuals(outputs);
mutable_parameter_block_sizes()->push_back(inputs);
}
bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const override {
const int n = parameter_block_sizes()[0];
const int m = num_residuals();
// No copy: points directly to Ceres memory
Eigen::Map<km::Vec> r_map(residuals, m);
_f(Eigen::Map<const km::Vec>(parameters[0], n), r_map);
// jacobians now
if (jacobians && jacobians[0]) {
// No copy: points directly to Ceres memory in Row-Major format
Eigen::Map<km::Mat> J_map(jacobians[0], m, n);
_j(Eigen::Map<const km::Vec>(parameters[0], n), J_map);
}
return true;
}
private:
cost_fn _f;
jac_fn _j;
};
};
} // namespace Karana::Dynamics