21 const double EPS = 3e-8;
46 template <
typename EnergyFunctor>
47 void linearSearch(
unsigned int dim,
double *oldPt,
double oldVal,
double *grad,
48 double *dir,
double *newPt,
double &newVal,
49 EnergyFunctor func,
double maxStep,
int &resCode) {
55 const unsigned int MAX_ITER_LINEAR_SEARCH = 1000;
56 double sum = 0.0, slope = 0.0, test = 0.0,
lambda = 0.0;
57 double lambda2 = 0.0, lambdaMin = 0.0, tmpLambda = 0.0, val2 = 0.0;
63 for (
unsigned int i = 0; i < dim; i++) sum += dir[i] * dir[i];
68 for (
unsigned int i = 0; i < dim; i++) dir[i] *= maxStep / sum;
74 for (
unsigned int i = 0; i < dim; i++) {
75 slope += dir[i] * grad[i];
82 for (
unsigned int i = 0; i < dim; i++) {
83 double temp = fabs(dir[i]) / std::max(fabs(oldPt[i]), 1.0);
84 if (temp > test) test = temp;
87 lambdaMin = MOVETOL / test;
90 while (it < MAX_ITER_LINEAR_SEARCH) {
97 for (
unsigned int i = 0; i < dim; i++) {
98 newPt[i] = oldPt[i] +
lambda * dir[i];
100 newVal = func(newPt);
102 if (newVal - oldVal <= FUNCTOL *
lambda * slope) {
110 tmpLambda = -slope / (2.0 * (newVal - oldVal - slope));
112 double rhs1 = newVal - oldVal -
lambda * slope;
113 double rhs2 = val2 - oldVal - lambda2 * slope;
114 double a = (rhs1 / (
lambda *
lambda) - rhs2 / (lambda2 * lambda2)) /
117 lambda * rhs2 / (lambda2 * lambda2)) /
120 tmpLambda = -slope / (2.0 * b);
122 double disc = b * b - 3 * a * slope;
125 }
else if (b <= 0.0) {
126 tmpLambda = (-b + sqrt(disc)) / (3.0 * a);
128 tmpLambda = -slope / (b + sqrt(disc));
131 if (tmpLambda > 0.5 *
lambda) {
142 for (
unsigned int i = 0; i < dim; i++) {
151 delete[] hessDGrad; \ 154 delete[] invHessian; \ 174 template <
typename EnergyFunctor,
typename GradientFunctor>
175 int minimize(
unsigned int dim,
double *pos,
double gradTol,
176 unsigned int &numIters,
double &funcVal, EnergyFunctor func,
177 GradientFunctor gradFunc,
double funcTol = TOLX,
178 unsigned int maxIts = MAXITS) {
183 double sum, maxStep, fp;
185 double *grad, *dGrad, *hessDGrad;
189 grad =
new double[dim];
190 dGrad =
new double[dim];
191 hessDGrad =
new double[dim];
192 newPos =
new double[dim];
193 xi =
new double[dim];
194 invHessian =
new double[dim * dim];
201 memset(invHessian, 0, dim * dim *
sizeof(
double));
202 for (
unsigned int i = 0; i < dim; i++) {
203 unsigned int itab = i * dim;
205 invHessian[itab + i] = 1.0;
208 sum += pos[i] * pos[i];
211 maxStep = MAXSTEP * std::max(sqrt(sum), static_cast<double>(dim));
213 for (
unsigned int iter = 1; iter <= maxIts; iter++) {
218 linearSearch(dim, pos, fp, grad, xi, newPos, funcVal, func, maxStep,
227 for (
unsigned int i = 0; i < dim; i++) {
228 xi[i] = newPos[i] - pos[i];
230 double temp = fabs(xi[i]) / std::max(fabs(pos[i]), 1.0);
231 if (temp > test) test = temp;
242 double gradScale = gradFunc(pos, grad);
246 double term = std::max(funcVal * gradScale, 1.0);
247 for (
unsigned int i = 0; i < dim; i++) {
248 double temp = fabs(grad[i]) * std::max(fabs(pos[i]), 1.0);
249 test = std::max(test, temp);
250 dGrad[i] = grad[i] - dGrad[i];
255 if (test < gradTol) {
265 double fac = 0, fae = 0, sumDGrad = 0, sumXi = 0;
266 for (
unsigned int i = 0; i < dim; i++) {
268 unsigned int itab = i * dim;
270 for (
unsigned int j = 0; j < dim; j++) {
271 hessDGrad[i] += invHessian[itab + j] * dGrad[j];
275 double *ivh = &(invHessian[i * dim]);
276 double &hdgradi = hessDGrad[i];
279 for (
unsigned int j = 0; j < dim; ++j, ++ivh, ++dgj) {
280 hdgradi += *ivh * *dgj;
283 fac += dGrad[i] * xi[i];
284 fae += dGrad[i] * hessDGrad[i];
285 sumDGrad += dGrad[i] * dGrad[i];
286 sumXi += xi[i] * xi[i];
288 if (fac > sqrt(EPS * sumDGrad * sumXi)) {
290 double fad = 1.0 / fae;
291 for (
unsigned int i = 0; i < dim; i++) {
292 dGrad[i] = fac * xi[i] - fad * hessDGrad[i];
295 for (
unsigned int i = 0; i < dim; i++) {
296 unsigned int itab = i * dim;
298 for (
unsigned int j = i; j < dim; j++) {
299 invHessian[itab + j] += fac * xi[i] * xi[j] -
300 fad * hessDGrad[i] * hessDGrad[j] +
301 fae * dGrad[i] * dGrad[j];
302 invHessian[j * dim + i] = invHessian[itab + j];
305 double pxi = fac * xi[i], hdgi = fad * hessDGrad[i],
306 dgi = fae * dGrad[i];
307 double *pxj = &(xi[i]), *hdgj = &(hessDGrad[i]), *dgj = &(dGrad[i]);
308 for (
unsigned int j = i; j < dim; ++j, ++pxj, ++hdgj, ++dgj) {
309 invHessian[itab + j] += pxi * *pxj - hdgi * *hdgj + dgi * *dgj;
310 invHessian[j * dim + i] = invHessian[itab + j];
316 for (
unsigned int i = 0; i < dim; i++) {
317 unsigned int itab = i * dim;
320 for (
unsigned int j = 0; j < dim; j++) {
321 xi[i] -= invHessian[itab + j] * grad[j];
324 double &pxi = xi[i], *ivh = &(invHessian[itab]), *gj = grad;
325 for (
unsigned int j = 0; j < dim; ++j, ++ivh, ++gj) {
const double MAXSTEP
Default maximim step size in the minimizer.
#define CHECK_INVARIANT(expr, mess)
int minimize(unsigned int dim, double *pos, double gradTol, unsigned int &numIters, double &funcVal, EnergyFunctor func, GradientFunctor gradFunc, double funcTol=TOLX, unsigned int maxIts=MAXITS)
Do a BFGS minimization of a function.
const double MOVETOL
Default tolerance for x changes in the minimizer.
const double lambda
scaling factor for rBO correction
const double FUNCTOL
Default tolerance for function convergence in the minimizer.
void linearSearch(unsigned int dim, double *oldPt, double oldVal, double *grad, double *dir, double *newPt, double &newVal, EnergyFunctor func, double maxStep, int &resCode)
Do a Quasi-Newton minimization along a line.
#define RDUNUSED_PARAM(x)
const double TOLX
Default direction vector tolerance in the minimizer.
#define PRECONDITION(expr, mess)
const double EPS
Default gradient tolerance in the minimizer.
const int MAXITS
Default maximum number of iterations.