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(); }
void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){ TreePoseGraph2::EdgeSet* temp=sortedEdges; if (eset){ sortedEdges=eset; } computePreconditioner(); propagateErrors(); sortedEdges=temp; }