void Sbs2SourceReconstrucionLoreta::updateModel()
{
if(!modelUpdateReady)
return;
modelUpdateReady = 0;
//TODO: signal when new beta and alpha are ready
//qDebug() << "****";
//we collect only raw values and weight them right before updating the model
w->multiply(currentModelUpdateValues,currentModelUpdateValuesVertices);
//Sbs2Timer::tic("updateAlpha()");
updateAlpha();
//Sbs2Timer::toc();
//Sbs2Timer::tic("updateBeta()");
updateBeta();
//Sbs2Timer::toc();
//Sbs2Timer::tic("updateW()");
updateW();
//Sbs2Timer::toc();
modelUpdateReady = 1;
tempModelUpdatedReady = 1;
}
void gradientDescent(double *wPrimeWPrimeTranspose, int iFrame, int jFrame, double * wPairwiseProd, double *errArray, double *quaternionArray, char *isDone, int nFrame, int nPoint, double *RTransposeR, double *dRTransposeR, double *quaternionTmp, double *gradient) {
int i, ii, j, k;
double errMin, errTmp, errMinOld;
double *quaternionMin;
int nItr;
double lambda;
double normGradSq;
int nItrLine;
/* update the wPrimeWPrimeTranspose matrix */
updateW(wPairwiseProd, iFrame, jFrame, nFrame, wPrimeWPrimeTranspose);
/* Figure out the best guess to start from */
quaternionMin = quaternionArray + 4 * (iFrame * nFrame + jFrame);
/* Go over all the neighbors around the current location to find the best one */
if (isDone[jFrame * nFrame + iFrame])
errMin = errArray[jFrame * nFrame + iFrame];
else
errMin = -1;
for (i = iFrame - 1; i <= iFrame + 1; ++i)
for (j = jFrame - 1; j <= jFrame + 1; ++j)
if ((i >= 0) && (i < nFrame) && (j >= 0) && (j < nFrame)
&& isDone[j * nFrame + i]) {
/* and check which one already gives the smallest value */
copyQuaternion(quaternionArray + 4 * (i * nFrame + j), quaternionTmp, 1);
errTmp = reconstrPairVal(quaternionTmp, wPrimeWPrimeTranspose, RTransposeR);
/* Keep the best pick */
if ((errTmp < errMin) || (errMin < 0)) {
errMin = errTmp;
copyQuaternion(quaternionTmp, quaternionMin, 0);
}
}
/* Start from that optimal value and perform gradient descent */
lambda=1.0;
/* fprintf(stderr,"Start error %f\n", errMin); */
for (nItr = 0; nItr < 40; ++nItr) {
errMin = reconstrPairValGrad(quaternionMin, wPrimeWPrimeTranspose, gradient, RTransposeR, dRTransposeR);
/* stop if the gradient is too small or the error too */
normGradSq = normSq(4, gradient);
if ((normGradSq < NUM_TOL_SQ) || (errMin < NUM_TOL ))
break;
/* Perform line search */
errMinOld=errMin;
for (nItrLine = 0; nItrLine < 20; ++nItrLine) {
/* get the value for the current quaternion */
for (k = 0; k < 4; ++k)
quaternionTmp[k] = quaternionMin[k] - lambda * gradient[k];
cleanQuaternion(quaternionTmp);
errTmp = reconstrPairVal(quaternionTmp, wPrimeWPrimeTranspose, RTransposeR);
/* check if the error is better than what we had before */
if (errTmp < errMin) {
errMin = errTmp;
copyQuaternion(quaternionTmp, quaternionMin, 0);
break;
} else
lambda /= 2;
}
/* stop if the error does not change much, < 0.1 percent */
if ((errMinOld-errMin)<0.001*errMinOld)
break;
}
/* fprintf(stderr,"%i ", nItr);
*update if nothing before or if the current result is better than before
*/
if ((!isDone[iFrame * nFrame + jFrame]) || ((isDone[iFrame * nFrame + jFrame]) && (errMin <errArray[iFrame * nFrame + jFrame]))) {
errArray[iFrame * nFrame + jFrame] = errMin;
errArray[jFrame * nFrame + iFrame] = errMin;
normalizeQuaternion(quaternionMin);
copyQuaternion(quaternionMin, quaternionArray + 4 * (jFrame * nFrame + iFrame), 0);
isDone[iFrame * nFrame + jFrame] = 1;
isDone[jFrame * nFrame + iFrame] = 1;
}
}
