void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXD>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
{
// allocate the sparse
spinv = SparseBlockMatrix<MatrixXD>(&rowBlockIndices[0],
&rowBlockIndices[0],
rowBlockIndices.size(),
rowBlockIndices.size(), true);
_map.clear();
vector<MatrixElem> elemsToCompute;
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
assert(blockRow>=0);
assert(blockRow < (int)rowBlockIndices.size());
assert(blockCol>=0);
assert(blockCol < (int)rowBlockIndices.size());
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXD *block=spinv.block(blockRow, blockCol, true);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
elemsToCompute.push_back(MatrixElem(r, c));
}
}
// sort the elems to reduce the number of recursive calls
sort(elemsToCompute.begin(), elemsToCompute.end());
// compute the inverse elements we need
for (size_t i = 0; i < elemsToCompute.size(); ++i) {
const MatrixElem& me = elemsToCompute[i];
computeEntry(me.r, me.c);
}
// set the marginal covariance
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXD *block=spinv.block(blockRow, blockCol);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
int idx = computeIndex(r, c);
LookupMap::const_iterator foundIt = _map.find(idx);
assert(foundIt != _map.end());
(*block)(iRow, iCol) = foundIt->second;
}
}
}
void BSplineMotionError<SPLINE_T>::buildHessianImplementation(SparseBlockMatrix & outHessian, Eigen::VectorXd & outRhs, bool /* useMEstimator */) {
// get the coefficients:
Eigen::MatrixXd coeff = _splineDV->spline().coefficients();
// create a column vector of spline coefficients
int dim = coeff.rows();
int seg = coeff.cols();
// build a vector of coefficients:
Eigen::VectorXd c(dim*seg);
// rows are spline dimension
for(int i = 0; i < seg; i++) {
c.block(i*dim,0,dim,1) = coeff.block(0, i, dim,1);
}
// right hand side:
Eigen::VectorXd b_u(_Q.rows()); // number of rows of Q:
b_u.setZero();
/* std::cout <<"b" << std::endl;
for(int i = 0 ; i < b_u->rows(); i++)
std::cout << (*b_u)(i) << std::endl;
std::cout <<"/b" << std::endl; */
_Q.multiply(&b_u, c);
// place the hessian elements in the correct place:
// build hessian:
for(size_t i = 0; i < numDesignVariables(); i++)
{
if( designVariable(i)->isActive()) {
// get the block index
int colBlockIndex = designVariable(i)->blockIndex();
int rows = designVariable(i)->minimalDimensions();
int rowBase = outHessian.colBaseOfBlock(colBlockIndex);
// <- this is our column index
//_numberOfSplineDesignVariables
for(size_t j = 0; j <= i; j++) // upper triangle should be sufficient
{
if (designVariable(j)->isActive()) {
int rowBlockIndex = designVariable(j)->blockIndex();
// select the corresponding block in _Q:
Eigen::MatrixXd* Qblock = _Q.block(i,j, false); // get block and do NOT allocate.
if (Qblock) { // check if block exists
// get the Hessian Block
const bool allocateIfMissing = true;
Eigen::MatrixXd *Hblock = outHessian.block(rowBlockIndex, colBlockIndex, allocateIfMissing);
*Hblock += *Qblock; // insert!
}
}
}
outRhs.segment(rowBase, rows) -= b_u.segment(i*rows, rows);
}
}
//std::cout << "OutHessian" << outHessian.toDense() << std::endl;
// show outRhs:
// for (int i = 0; i < outRhs.rows(); i++)
// std::cout << outRhs(i) << " : " << (*b_u)(i) << std::endl;
}