本文整理汇总了C++中optimizablegraph::Vertex::clearQuadraticForm方法的典型用法代码示例。如果您正苦于以下问题：C++ Vertex::clearQuadraticForm方法的具体用法？C++ Vertex::clearQuadraticForm怎么用？C++ Vertex::clearQuadraticForm使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类optimizablegraph::Vertex的用法示例。
在下文中一共展示了Vertex::clearQuadraticForm方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: updateInitialization
bool SparseOptimizerIncremental::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
{
if (batchStep) {
return SparseOptimizerOnline::updateInitialization(vset, eset);
}
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->clearQuadraticForm(); // be sure that b is zero for this vertex
}
// get the touched vertices
_touchedVertices.clear();
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
OptimizableGraph::Vertex* v1 = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* v2 = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
if (! v1->fixed())
_touchedVertices.insert(v1);
if (! v2->fixed())
_touchedVertices.insert(v2);
}
//cerr << PVAR(_touchedVertices.size()) << endl;
// updating the internal structures
std::vector<HyperGraph::Vertex*> newVertices;
newVertices.reserve(vset.size());
_activeVertices.reserve(_activeVertices.size() + vset.size());
_activeEdges.reserve(_activeEdges.size() + eset.size());
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it)
_activeEdges.push_back(static_cast<OptimizableGraph::Edge*>(*it));
//cerr << "updating internal done." << endl;
// update the index mapping
size_t next = _ivMap.size();
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(*it);
if (! v->fixed()){
if (! v->marginalized()){
v->setHessianIndex(next);
_ivMap.push_back(v);
newVertices.push_back(v);
_activeVertices.push_back(v);
next++;
}
else // not supported right now
abort();
}
else {
v->setHessianIndex(-1);
}
}
//cerr << "updating index mapping done." << endl;
// backup the tempindex and prepare sorting structure
VertexBackup backupIdx[_touchedVertices.size()];
memset(backupIdx, 0, sizeof(VertexBackup) * _touchedVertices.size());
int idx = 0;
for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin(); it != _touchedVertices.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
backupIdx[idx].hessianIndex = v->hessianIndex();
backupIdx[idx].vertex = v;
backupIdx[idx].hessianData = v->hessianData();
++idx;
}
sort(backupIdx, backupIdx + _touchedVertices.size()); // sort according to the hessianIndex which is the same order as used later by the optimizer
for (int i = 0; i < idx; ++i) {
backupIdx[i].vertex->setHessianIndex(i);
}
//cerr << "backup tempindex done." << endl;
// building the structure of the update
_updateMat.clear(true); // get rid of the old matrix structure
_updateMat.rowBlockIndices().clear();
_updateMat.colBlockIndices().clear();
_updateMat.blockCols().clear();
// placing the current stuff in _updateMat
MatrixXd* lastBlock = 0;
int sizePoses = 0;
for (int i = 0; i < idx; ++i) {
OptimizableGraph::Vertex* v = backupIdx[i].vertex;
int dim = v->dimension();
sizePoses+=dim;
_updateMat.rowBlockIndices().push_back(sizePoses);
_updateMat.colBlockIndices().push_back(sizePoses);
_updateMat.blockCols().push_back(SparseBlockMatrix<MatrixXd>::IntBlockMap());
int ind = v->hessianIndex();
//cerr << PVAR(ind) << endl;
if (ind >= 0) {
MatrixXd* m = _updateMat.block(ind, ind, true);
v->mapHessianMemory(m->data());
lastBlock = m;
}
}
lastBlock->diagonal().array() += 1e-6; // HACK to get Eigen value > 0
for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
//.........这里部分代码省略.........