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>::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;
}
