quad-residual-cost.hpp

Go to the documentation of this file.

#pragma once
 
#include "./composite-costs.hpp"
#include <aligator/context.hpp>
 
namespace aligator {

template <typename _Scalar>
struct QuadraticResidualCostTpl : CostAbstractTpl<_Scalar> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = CostAbstractTpl<Scalar>;
  using CostData = CostDataAbstractTpl<Scalar>;
  using Data = CompositeCostDataTpl<Scalar>;
  using StageFunction = StageFunctionTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
 
  MatrixXs weights_;
  xyz::polymorphic<StageFunction> residual_;
  bool gauss_newton = true;
 
  QuadraticResidualCostTpl(xyz::polymorphic<Manifold> space,
                           xyz::polymorphic<StageFunction> function,
                           const ConstMatrixRef &weights);
 
  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                CostData &data_) const;
 
  void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u,
                        CostData &data_) const;
 
  void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u,
                       CostData &data_) const;
 
  shared_ptr<CostData> createData() const {
    return std::make_shared<Data>(this->ndx(), this->nu,
                                  residual_->createData());
  }

  template <typename Derived> Derived *getResidual() {
    return dynamic_cast<Derived *>(&*residual_);
  }

  template <typename Derived> const Derived *getResidual() const {
    return dynamic_cast<const Derived *>(&*residual_);
  }
};
 
extern template struct QuadraticResidualCostTpl<context::Scalar>;
 
} // namespace aligator
 
#include "./quad-residual-cost.hxx"