final double stepScale = settings.stepScale;
cvSetZero(gradient);
cvSetZero(hessian);
Parallel.loop(0, n, settings.numThreads, new Looper() {
public void loop(int from, int to, int looperID) {
// for (int i = 0; i < n; i++) {
for (int i = from; i < to; i++) {
tempParameters[i].set(parameters);
tempParameters[i].set(i, tempParameters[i].get(i) + /*(1<<pyramidLevel)**/stepScale);
scale[i] = tempParameters[i].get(i) - parameters.get(i);
constraintGrad[i] = tempParameters[i].getConstraintError() - constraintError;
}}});
// final double adjustedRMSE = (1-prevOutlierRatio)*RMSE;
Parallel.loop(0, hessianGradientTransformerData.length, settings.numThreads, new Looper() {
public void loop(int from, int to, int looperID) {
for (int i = 0; i < n; i++) {
Data d = hessianGradientTransformerData[looperID][i];
d.srcImg = template [pyramidLevel];
d.subImg = transformed[pyramidLevel];
d.srcDotImg = residual [pyramidLevel];
d.transImg = d.dstImg = null;
d.mask = roiMask[pyramidLevel];
d.zeroThreshold = /*adjustedRMSE**/settings.zeroThresholds [Math.min(settings.zeroThresholds .length-1, pyramidLevel)];
d.outlierThreshold = /*adjustedRMSE**/settings.outlierThresholds[Math.min(settings.outlierThresholds.length-1, pyramidLevel)];
d.pyramidLevel = pyramidLevel;
}
int y1 = roi.y() + looperID *roi.height()/hessianGradientTransformerData.length;
int y2 = roi.y() + (looperID+1)*roi.height()/hessianGradientTransformerData.length;
subroi[looperID].x (roi.x());
subroi[looperID].y (y1);
subroi[looperID].width (roi.width());
subroi[looperID].height(y2-y1);
transformer.transform(hessianGradientTransformerData[looperID], subroi[looperID], tempParameters, null);
}});
double dstCount = 0;
double dstCountZero = 0;
double dstCountOutlier = 0;
for (Data[] data : hessianGradientTransformerData) {
dstCount += data[0].dstCount;
dstCountZero += data[0].dstCountZero;
dstCountOutlier += data[0].dstCountOutlier;
for (int i = 0; i < n; i++) {
Data d = (Data)data[i];
gradient.put(i, gradient.get(i) - d.srcDstDot);
for (int j = 0; j < n; j++) {
hessian.put(i, j, hessian.get(i, j) + d.dstDstDot[j]);
}
}
}
// prevOutlierRatio = dstCountOutlier/dstCount;
//System.out.println(dstCountZero/dstCount + " " + dstCountOutlier/dstCount);
// if we have a gamma of an alpha, compute the prior for regularization, but
// if prioParameters == null, our prior is zero motion, so no need to compute it
if ((settings.gammaTgamma != null || settings.tikhonovAlpha != 0) &&
prior != null && priorParameters != null) {
for (int i = 0; i < n; i++) {
prior.put(i, parameters.get(i) - priorParameters.get(i));
}
cvMatMul(hessian, prior, prior);
// compute gradient
for (int i = 0; i < n; i++) {
gradient.put(i, gradient.get(i) + prior.get(i));
}
}
//System.out.println(prior);
if (settings.constrained) {
// to get a well-conditionned matrix, compute what
// looks like an appropriate scale for the constraint
double constraintGradSum = 0;
for (double d : constraintGrad) {
constraintGradSum += d;
}
scale[n] = n*constraintGradSum;
for (int i = 0; i < n; i++) {
double c = constraintGrad[i]*scale[n];
hessian.put(i, n, c);
hessian.put(n, i, c);
}
gradient.put(n, -constraintError*scale[n]);
}
if (subspaceParameters != null && subspaceParameters.length > 0 &&
settings.subspaceAlpha != 0.0) {
final int m = subspaceParameters.length;
// double[][] subspaceHessian = new double[n+m][n+m];
// double[] subspaceGradient = new double[n+m];
Arrays.fill(subspaceCorrelated, false);
tempParameters[0].set(parameters);
tempParameters[0].setSubspace(subspaceParameters);
Parallel.loop(0, n+m, settings.numThreads, new Looper() {
public void loop(int from, int to, int looperID) {
// int looperID = 0;
// for (int i = 0; i < n+m; i++) {
for (int i = from; i < to; i++) {
if (i < n) {
Arrays.fill(subspaceJacobian[i], 0);
subspaceJacobian[i][i] = scale[i];
} else {
System.arraycopy(subspaceParameters, 0, tempSubspaceParameters[looperID], 0, m);
tempSubspaceParameters[looperID][i-n] += stepScale;
tempParameters[i-n+1].set(parameters);
tempParameters[i-n+1].setSubspace(tempSubspaceParameters[looperID]);
scale[i] = tempSubspaceParameters[looperID][i-n] - subspaceParameters[i-n];
for (int j = 0; j < n; j++) {
subspaceJacobian[i][j] = tempParameters[0].get(j) - tempParameters[i-n+1].get(j);
subspaceCorrelated[j] |= subspaceJacobian[i][j] != 0; // this may not work in parallel...
}
}
}}});
int subspaceCorrelatedCount = 0;
// double subspaceRMSE = 0;
for (int i = 0; i < n; i++) {
subspaceResidual[i] = parameters.get(i) - tempParameters[0].get(i);
// subspaceRMSE += subspaceResidual[i]*subspaceResidual[i];
if (subspaceCorrelated[i]) {
subspaceCorrelatedCount++;
}
}
// subspaceRMSE = Math.sqrt(subspaceRMSE/n);
//System.out.println((float)RMSE + " " + (float)subspaceRMSE);
final double K = settings.subspaceAlpha*settings.subspaceAlpha * RMSE*RMSE/
subspaceCorrelatedCount;//(subspaceRMSE*subspaceRMSE);
Parallel.loop(0, n+m, settings.numThreads, new Looper() {
public void loop(int from, int to, int looperID) {
// int looperID = 0;
// for (int i = 0; i < n+m; i++) {
for (int i = from; i < to; i++) {
if (i < n && !subspaceCorrelated[i]) {