/** Fit GSL derivative function wrapper
* @param x :: Input function arguments
* @param params :: Input data
* @param J :: Output derivatives
* @return A GSL status information
*/
int gsl_df(const gsl_vector *x, void *params, gsl_matrix *J) {
struct GSL_FitData *p = (struct GSL_FitData *)params;
p->J.setJ(J);
// update function parameters
if (x->data) {
size_t ia = 0;
for (size_t i = 0; i < p->function->nParams(); ++i) {
if (p->function->isActive(i)) {
p->function->setActiveParameter(i, x->data[ia]);
++ia;
}
}
}
p->function->applyTies();
// calculate the Jacobian
p->function->functionDeriv(*p->costFunction->getDomain(), p->J);
// p->function->addPenaltyDeriv(&p->J);
// add penalty
size_t n = p->costFunction->getDomain()->size() - 1;
size_t ia = 0;
for (size_t i = 0; i < p->function->nParams(); ++i) {
if (!p->function->isActive(i))
continue;
API::IConstraint *c = p->function->getConstraint(i);
if (c) {
double penalty = c->checkDeriv2();
if (penalty != 0.0) {
double deriv = p->J.get(0, ia);
p->J.set(0, ia, deriv + penalty);
deriv = p->J.get(n, ia);
p->J.set(n, ia, deriv + penalty);
for (size_t j = 9; j < n; j += 10) {
deriv = p->J.get(j, ia);
p->J.set(j, ia, deriv + penalty);
}
}
} // if (c)
++ia;
}
// functionDeriv() return derivatives of calculated data values. Need to
// convert this values into
// derivatives of calculated-observed divided by error values used by GSL
auto values = boost::dynamic_pointer_cast<API::FunctionValues>(
p->costFunction->getValues());
if (!values) {
throw std::invalid_argument("FunctionValues expected");
}
for (size_t iY = 0; iY < p->n; iY++)
for (size_t iP = 0; iP < p->p; iP++) {
J->data[iY * p->p + iP] *= values->getFitWeight(iY);
// std::cerr << iY << ' ' << iP << ' ' << J->data[iY*p->p + iP] <<
// std::endl;
}
return GSL_SUCCESS;
}
/** Calculate the value and the first and second derivatives of the cost function
* @param evalFunction :: If false cost function isn't evaluated and returned value (0.0) should be ignored.
* It is for efficiency reasons.
* @param evalDeriv :: flag for evaluation of the first derivatives
* @param evalHessian :: flag for evaluation of the second derivatives
*/
double CostFuncLeastSquares::valDerivHessian(bool evalFunction, bool evalDeriv, bool evalHessian) const
{
if (m_pushed || !evalDeriv)
{
return val();
}
if (!m_dirtyVal && !m_dirtyDeriv && !m_dirtyHessian) return m_value;
if (m_dirtyVal) evalFunction = true;
checkValidity();
if (evalFunction)
{
m_value = 0.0;
}
if (evalDeriv)
{
m_der.resize(nParams());
m_der.zero();
}
if (evalHessian)
{
m_hessian.resize(nParams(),nParams());
m_hessian.zero();
}
auto seqDomain = boost::dynamic_pointer_cast<SeqDomain>(m_domain);
if (seqDomain)
{
seqDomain->leastSquaresValDerivHessian(*this,evalFunction,evalDeriv,evalHessian);
}
else
{
auto simpleValues = boost::dynamic_pointer_cast<API::FunctionValues>(m_values);
if (!simpleValues)
{
throw std::runtime_error("LeastSquares: unsupported IFunctionValues.");
}
addValDerivHessian(m_function,m_domain,simpleValues,evalFunction,evalDeriv,evalHessian);
}
// Add constraints penalty
size_t np = m_function->nParams();
if (evalFunction)
{
for(size_t i = 0; i < np; ++i)
{
API::IConstraint* c = m_function->getConstraint(i);
if (c)
{
m_value += c->check();
}
}
m_dirtyVal = false;
}
if (evalDeriv)
{
size_t i = 0;
for(size_t ip = 0; ip < np; ++ip)
{
if ( !m_function->isActive(ip) ) continue;
API::IConstraint* c = m_function->getConstraint(ip);
if (c)
{
double d = m_der.get(i) + c->checkDeriv();
m_der.set(i,d);
}
++i;
}
m_dirtyDeriv = false;
}
if (evalDeriv)
{
size_t i = 0;
for(size_t ip = 0; ip < np; ++ip)
{
if ( !m_function->isActive(ip) ) continue;
API::IConstraint* c = m_function->getConstraint(ip);
if (c)
{
double d = m_hessian.get(i,i) + c->checkDeriv2();
m_hessian.set(i,i,d);
}
++i;
}
// clear the dirty flag if hessian was actually calculated
m_dirtyHessian = m_hessian.isEmpty();
}
return m_value;
}