Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
38 lines (30 sloc) 808 Bytes
#ifndef NUMJAC_H
#define NUMJAC_H
#include <Eigen/Core>
#include <stdexcept>
template<typename T, typename P>
auto numJacobian(T fn, const P& x) -> Eigen::Matrix<double,decltype(fn(x))::RowsAtCompileTime,P::RowsAtCompileTime>
{
const int Rows=decltype(fn(x))::RowsAtCompileTime;
Eigen::Matrix<double,Rows,P::RowsAtCompileTime> jac;
const double eps=0.0001;
Eigen::Matrix<double,P::RowsAtCompileTime,1> xh=x;
for (int i=0;i<x.rows();i++)
{
xh(i)=x(i)+eps;
const auto a=fn(xh);
xh(i)=x(i)-eps;
const auto b=fn(xh);
xh(i)=x(i);
if (Rows==Eigen::Dynamic)
{
if (i==0)
jac.resize(a.rows(),x.rows());
if ((a.rows()!=jac.rows())||(b.rows()!=jac.rows()))
throw std::runtime_error("Dynamic Dimensions do not match!");
}
jac.col(i)=(a-b)/(2.0*eps);
}
return jac;
}
#endif
You can’t perform that action at this time.