void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
TreePoseGraph2::EdgeSet* temp=sortedEdges;
if (eset){
sortedEdges=eset;
}
computePreconditioner();
propagateErrors();
sortedEdges=temp;
}
returnValue ConjugateGradientMethod::setMatrix( double *A_ ){
if( dim <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
if( nDense <= 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
pCounter = 0;
A = new double[nDense];
return computePreconditioner( A_ );
}
void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
TreePoseGraph3::EdgeSet* temp=sortedEdges;
if (eset){
sortedEdges=eset;
}
if (noPreconditioner)
propagateErrors(false);
else {
if (iteration==1)
computePreconditioner();
propagateErrors(true);
}
sortedEdges=temp;
onRestartBegin();
if (restartOnDivergence){
double mte, ate;
double mre, are;
error(&mre, &mte, &are, &ate);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
int interval=3;
if ((int)maxRotationalErrors.size()>=interval){
uint s=(uint)maxRotationalErrors.size();
double re0 = maxRotationalErrors[s-interval];
double re1 = maxRotationalErrors[s-1];
if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
double rg=rotGain;
if (sqrt(re1)>M_PI/4){
cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
cerr << " err=" << re0 << " -> " << re1 << endl;
cerr << "Restarting optimization and reducing the rotation factor" << endl;
cerr << rg << " -> ";
initializeOnTree();
initializeTreeParameters();
initializeOptimization();
error(&mre, &mte);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
rg*=0.1;
rotGain=rg;
cerr << rotGain << endl;
}
else {
cerr << "decreasing angular gain" << rotGain*0.1 << endl;
rotGain*=0.1;
}
}
}
}
onRestartDone();
}
