df().cpp

// Compute the jacobian of the functions
int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
{
    // 'x' has dimensions n x 1
    // It contains the current estimates for the parameters.

    // 'fjac' has dimensions m x n
    // It will contain the jacobian of the errors, calculated numerically in this case.

    float epsilon;
    epsilon = 1e-5f;

    for (int i = 0; i < x.size(); i++) {
        Eigen::VectorXf xPlus(x);
        xPlus(i) += epsilon;
        Eigen::VectorXf xMinus(x);
        xMinus(i) -= epsilon;

        Eigen::VectorXf fvecPlus(values());
        operator()(xPlus, fvecPlus);

        Eigen::VectorXf fvecMinus(values());
        operator()(xMinus, fvecMinus);

        Eigen::VectorXf fvecDiff(values());
        fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

        fjac.block(0, i, values(), 1) = fvecDiff;
    }

    return 0;
}