void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks,
int s)
{
deallocate();
resizeVector(s);
if (_doSchur) {
// the following two are only used in schur
assert(_sizePoses > 0 && "allocating with wrong size");
_coefficients = new double [s];
_bschur = new double[_sizePoses];
}
_Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
if (_doSchur) {
_Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
_Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
_DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
_Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
_HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
_HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
#ifdef G2O_OPENMP
_coefficientsMutex.resize(numPoseBlocks);
#endif
}
}
bool CTerrainData::resize(size_t width, size_t height, size_t cubeSize)
{
if (cubeSize==0)
{
cubeSize = 8;
for (size_t i=8; i<30; ++i)
{
if (width%i==0&&width%i==0)
{
cubeSize = i;
break;
}
}
}
width=(width/cubeSize)*cubeSize;
height=(height/cubeSize)*cubeSize;
if (65536<(width+1)*(height+1))
{
return false;
}
TerrainCell val={0,255,0xFFFFFFFF,0xFFFFFFFF,0.0f,Vec3D(0.0f,1.0f,0.0f),0};
resizeVector(m_Cells,m_nVertexXCount,m_nVertexYCount,width+1,height+1,val);
m_nWidth = width;
m_nHeight = height;
m_nCubeSize = cubeSize;
m_nCellCount = m_nWidth * m_nHeight;
m_nVertexXCount = m_nWidth + 1;
m_nVertexYCount = m_nHeight + 1;
m_nVertexCount = m_nVertexXCount * m_nVertexYCount;
return true;
}
sparseVector *createVector(int dimLimit, int initSize)
{
sparseVector *newitem;
CALLOC(newitem, 1);
newitem->limit = dimLimit;
initSize = resizeVector(newitem, initSize);
return(newitem);
}
bool CSceneData::resize(size_t width, size_t height)
{
TerrainCell val={0,255,0xFFFFFFFF,0.0f,0};
resizeVector(m_Cells,m_nWidth+1,m_nHeight+1,width+1,height+1,val);
m_nWidth = width;
m_nHeight = height;
return true;
}
bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
_Hpp->rowBlockIndices().push_back(_sizePoses);
_Hpp->colBlockIndices().push_back(_sizePoses);
_Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
++_numPoses;
int ind = v->hessianIndex();
PoseMatrixType* m = _Hpp->block(ind, ind, true);
v->mapHessianMemory(m->data());
} else {
std::cerr << "updateStructure(): Schur not supported" << std::endl;
abort();
}
}
resizeVector(_sizePoses + _sizeLandmarks);
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
int indexV1Bak = ind1;
if (ind1 == -1)
continue;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock) // make sure, we allocate the upper triangular block
std::swap(ind1, ind2);
if (! v1->marginalized() && !v2->marginalized()) {
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
} else {
std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
}
}
}
}
return true;
}
REAL putItem(sparseVector *sparse, int targetIndex, REAL value)
{
REAL last = 0.0;
int posIndex;
if(targetIndex < 0) {
posIndex = -targetIndex;
if(posIndex > sparse->count)
return(last);
targetIndex = sparse->index[posIndex];
}
else
posIndex = findIndex(targetIndex, sparse->index, sparse->count, BLAS_BASE);
if(fabs(value) < MACHINEPREC)
value = 0;
if(targetIndex == sparse->index[0])
sparse->value[0] = value;
if(posIndex < 0) {
if(value != 0) {
if(sparse->count == sparse->size)
resizeVector(sparse, sparse->size + RESIZEDELTA);
posIndex = -posIndex;
sparse->count++;
if(posIndex < sparse->count)
moveVector(sparse, posIndex+1, posIndex, sparse->count-posIndex);
sparse->value[posIndex] = value;
sparse->index[posIndex] = targetIndex;
}
}
else {
if(value == 0) {
last = sparse->value[posIndex];
if(sparse->count > posIndex)
moveVector(sparse, posIndex, posIndex+1, sparse->count-posIndex);
sparse->count--;
}
else {
sparse->value[posIndex] = value;
sparse->index[posIndex] = targetIndex;
}
}
#ifdef DEBUG_SPARSELIB
verifyVector(sparse);
#endif
return(last);
}
int redimensionVector(sparseVector *sparse, int newDim)
{
int olddim, i;
olddim = sparse->limit;
sparse->limit = newDim;
if(lastIndex(sparse)>newDim) {
i = sparse->count;
while(i>0 && sparse->index[i]>newDim) i--;
sparse->count = i;
resizeVector(sparse, sparse->count);
}
return(olddim);
}
void putVector(sparseVector *sparse, REAL *dense, int indexStart, int indexEnd)
{
int i,n;
n = sparse->count;
if(indexStart<=0)
indexStart=sparse->index[1];
if(indexEnd<=0)
indexEnd=sparse->index[n];
if(n==0 || sparse->index[n]<indexStart) {
i = sparse->index[0];
if(i>=indexStart && i<=indexEnd)
sparse->value[0] = 0;
for(i = indexStart; i<=indexEnd; i++) {
if(dense[i] == 0) continue;
if(sparse->size == sparse->count)
resizeVector(sparse, sparse->size + RESIZEDELTA);
sparse->count++;
sparse->value[sparse->count] = dense[i];
sparse->index[sparse->count] = i;
if(i == sparse->index[0])
sparse->value[0] = dense[i];
}
}
else {
while(indexStart <= indexEnd) {
putItem(sparse, indexStart, dense[indexStart]);
indexStart++;
}
}
#ifdef DEBUG_SPARSELIB
verifyVector(sparse);
#endif
}
