void CholOptimizer<PG>::transformSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, const typename PG::TransformationType& newRootPose){
typename PG::TransformationType t=newRootPose*rootVertex->transformation.inverse();
for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
v->transformation=t*v->transformation;
}
}
void HCholOptimizer<PG>::bottomToTop(int iterations, double lambda, bool initWithObservations){
if (! _lowerOptimizer)
return;
if (_lowerOptimizer){
_lowerOptimizer->bottomToTop(iterations, lambda,initWithObservations);
}
annotateHiearchicalEdges(iterations, lambda, initWithObservations);
if (this->verbose())
cerr << "_upperOptimizer=" << _upperOptimizer << endl;
if (! _upperOptimizer){
if (this->verbose())
cerr << "Optimizing the top level" << endl;
HVertex* rootVertex=_MY_CAST_<HVertex*>(this->vertices().begin()->second);
this->initialize(rootVertex->id());
//ofstream oss("topLevel.graph");
//save(oss);
//oss.close();
Graph::VertexSet vset;
for (Graph::VertexIDMap::const_iterator it=this->vertices().begin(); it!=this->vertices().end(); it++){
vset.insert(it->second);
}
this->optimizeSubset(rootVertex, vset, iterations*3, lambda, true);
if (this->verbose()) {
cerr << "Done";
//ofstream os("topLevel.dat");
//saveAsGnuplot(os);
//os.close();
cerr << "done" << endl;
}
}
}
bool CholOptimizer<PG>::buildIndexMapping(typename PG::Vertex* rootVertex, Graph::VertexSet& vset){
_ivMap.resize(vset.size());
int i=0;
for (Graph::VertexSet::iterator it=vset.begin(); it!=vset.end(); it++){
typename PG::Vertex* v=_MY_CAST_<typename PG::Vertex*>(*it);
if (v!=rootVertex && ! v->fixed()){
v->tempIndex()=i;
_ivMap[i]=v;
i++;
}
}
_ivMap.resize(i);
return true;
}
int CholOptimizer<PG>::optimize(int iterations, bool online){
Graph::VertexSet vset;
for (Graph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); it++){
vset.insert(it->second);
}
typename PG::Vertex* root=dynamic_cast<typename PG::Vertex*>(vertex(_rootNode));
if (! root)
root=_MY_CAST_<typename PG::Vertex*>(vertices().begin()->second);
if (verbose())
cerr << "# root id " << root->id() << endl;
bool initFromObservations = _guessOnEdges;
optimizeSubset(root, vset, iterations, 0., initFromObservations);
return iterations;
}
int CholOptimizer<PG>::optimizeSubset(typename PG::Vertex* rootVertex, Graph::VertexSet& vset, int iterations, double lambda, bool initFromObservations,
int otherNode, typename PG::InformationType* otherCovariance)
{
if (vset.size() <= 1) {
return 0;
}
if (!buildIndexMapping(rootVertex,vset)){
return 0;
}
// clean up from last call
if (_symbolicCholesky) {
cs_sfree(_symbolicCholesky);
_symbolicCholesky = 0;
}
computeActiveEdges(rootVertex,vset);
if (initFromObservations){
initializeActiveSubsetWithObservations(rootVertex);
if (verbose()){
cerr << "iteration= " << -1
<< "\t chi2= " << this->chi2()
<< "\t time= " << 0.0
<< "\t cumTime= " << 0.0
<< endl;
}
}
int dim = PG::TransformationVectorType::TemplateSize;
int cjIterations=0;
double cumTime=0;
for (int i=0; i<iterations; i++){
struct timeval ts, te;
gettimeofday(&ts,0);
buildLinearSystem(rootVertex,lambda);
if (i == 0) {
// we have to sort the matrix structure only within the first iteration, it stays the same for the following iterations
SparseMatrixEntry* entry = _sparseMatrix;
for (int j = 0; j < _sparseNz; ++j) { // store the pointer to the array
_sparseMatrixPtr[j] = entry;
++entry;
}
std::sort(_sparseMatrixPtr, _sparseMatrixPtr + _sparseNz, SparseMatrixEntryPtrCmp());
}
if (otherNode==-1 || i!=iterations-1){
solveAndUpdate();
} else {
double** pblock = new double*[dim];
for (int k = 0; k < dim; ++k)
pblock[k] = (*otherCovariance)[k];
typename PG::Vertex* otherVertex=_MY_CAST_<typename PG::Vertex*>(vertex(otherNode));
int j=otherVertex->tempIndex()*dim;
solveAndUpdate(pblock, j,j,j+dim,j+dim);
static TransformCovariance<PG> tCov;
tCov(*otherCovariance, rootVertex->transformation, otherVertex->transformation);
assert(otherCovariance->det()>0.);
}
gettimeofday(&te,0);
double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec);
cumTime+=dts;
if (verbose()){
cerr << "iteration= " << i
<< "\t chi2= " << this->chi2()
<< "\t time= " << dts
<< "\t cumTime= " << cumTime
<< endl;
}
if (this->visualizeToStdout())
this->visualizeToStream(cout);
++cjIterations;
}
clearIndexMapping();
return cjIterations;
}
