本文整理汇总了C++中hypergraph::VertexSet类的典型用法代码示例。如果您正苦于以下问题:C++ VertexSet类的具体用法?C++ VertexSet怎么用?C++ VertexSet使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了VertexSet类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: updateInitialization
bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
{
std::vector<HyperGraph::Vertex*> newVertices;
newVertices.reserve(vset.size());
_activeVertices.reserve(_activeVertices.size() + vset.size());
//for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it)
//_activeVertices.push_back(static_cast<OptimizableGraph::Vertex*>(*it));
_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));
// 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->setTempIndex(next);
_ivMap.push_back(v);
newVertices.push_back(v);
_activeVertices.push_back(v);
next++;
}
else // not supported right now
abort();
}
else {
v->setTempIndex(-1);
}
}
//if (newVertices.size() != vset.size())
//cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl;
return _solver->updateStructure(newVertices, eset);
}
开发者ID:RoboWGT,项目名称:robo_groovy,代码行数:35,代码来源:graph_optimizer_sparse.cpp
示例2: starsInEdge
void starsInEdge(StarSet& stars, HyperGraph::Edge* e, EdgeStarMap& esmap, HyperGraph::VertexSet& gauge){
for (size_t i=0; i<e->vertices().size(); i++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)e->vertices()[i];
if (gauge.find(v)==gauge.end())
starsInVertex(stars, v, esmap);
}
}
开发者ID:MichaelRuhnke,项目名称:g2o,代码行数:7,代码来源:simple_star_ops.cpp
示例3: push
void OptimizableGraph::push(HyperGraph::VertexSet& vset) {
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end();
it++) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->push();
}
}
开发者ID:AIRLab-POLIMI,项目名称:ROAMFREE,代码行数:7,代码来源:optimizable_graph.cpp
示例4: shortestPaths
void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance,
double comparisonConditioner, bool directed, double maxEdgeCost)
{
HyperGraph::VertexSet vset;
vset.insert(v);
shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
}
开发者ID:CHItA,项目名称:ORB_SLAM2,代码行数:7,代码来源:hyper_dijkstra.cpp
示例5: setFixed
void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->setFixed(fixed);
}
}
开发者ID:Crusty82,项目名称:g2o_tutorial,代码行数:7,代码来源:optimizable_graph.cpp
示例6: discardTop
void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->discardTop();
}
}
开发者ID:Crusty82,项目名称:g2o_tutorial,代码行数:7,代码来源:optimizable_graph.cpp
示例7: saveGnuplot
bool saveGnuplot(const std::string& gnudump, const OptimizableGraph& optimizer)
{
HyperGraph::VertexSet vset;
for (HyperGraph::VertexIDMap::const_iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
vset.insert(it->second);
}
return saveGnuplot(gnudump, vset, optimizer.edges());
}
开发者ID:2maz,项目名称:g2o,代码行数:8,代码来源:output_helper.cpp
示例8: push
void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
{
for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
if (v)
v->push();
else
cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
}
}
开发者ID:skarlsson,项目名称:tmp-android,代码行数:10,代码来源:sparse_optimizer.cpp
示例9: pop
void SparseOptimizer::pop(HyperGraph::VertexSet& vlist)
{
for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*> (*it);
if (v)
v->pop();
else
cerr << "FATAL POP SET" << endl;
}
}
开发者ID:RoboWGT,项目名称:robo_groovy,代码行数:10,代码来源:graph_optimizer_sparse.cpp
示例10: shortestPaths
void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost,
double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)
{
reset();
std::priority_queue< AdjacencyMapEntry > frontier;
for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
HyperGraph::Vertex* v=*vit;
AdjacencyMap::iterator it=_adjacencyMap.find(v);
assert(it!=_adjacencyMap.end());
it->second._distance=0.;
it->second._parent=0;
frontier.push(it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry entry=frontier.top();
frontier.pop();
HyperGraph::Vertex* u=entry.child();
AdjacencyMap::iterator ut=_adjacencyMap.find(u);
assert(ut!=_adjacencyMap.end());
double uDistance=ut->second.distance();
std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
HyperGraph::EdgeSet::iterator et=u->edges().begin();
while (et != u->edges().end()){
HyperGraph::Edge* edge=*et;
++et;
if (directed && edge->vertex(0) != u)
continue;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
HyperGraph::Vertex* z = edge->vertex(i);
if (z == u)
continue;
double edgeDistance=(*cost)(edge, u, z);
if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
continue;
double zDistance=uDistance+edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot=_adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
ot->second._distance=zDistance;
ot->second._parent=u;
ot->second._edge=edge;
frontier.push(ot->second);
}
}
}
}
}
开发者ID:Florenc,项目名称:g2o,代码行数:55,代码来源:hyper_dijkstra.cpp
示例11: initializeOptimization
bool SparseOptimizer::initializeOptimization(int level)
{
HyperGraph::VertexSet vset;
for (VertexIDMap::iterator
it = vertices().begin();
it != vertices().end();
it++)
vset.insert(it->second);
return initializeOptimization(vset,level);
}
开发者ID:RoboWGT,项目名称:robo_groovy,代码行数:12,代码来源:graph_optimizer_sparse.cpp
示例12: computeInitialGuess
void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
{
OptimizableGraph::VertexSet emptySet;
std::set<Vertex*> backupVertices;
HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
OptimizableGraph::Edge* e = *it;
for (size_t i = 0; i < e->vertices().size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
if (!v)
continue;
if (v->fixed())
fixedVertices.insert(v);
else { // check for having a prior which is able to fully initialize a vertex
for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
//cerr << "Initialize with prior for " << v->id() << endl;
vedge->initialEstimate(emptySet, v);
fixedVertices.insert(v);
}
}
}
if (v->hessianIndex() == -1) {
std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
if (foundIt == backupVertices.end()) {
v->push();
backupVertices.insert(v);
}
}
}
}
EstimatePropagator estimatePropagator(this);
estimatePropagator.propagate(fixedVertices, costFunction);
// restoring the vertices that should not be initialized
for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
Vertex* v = *it;
v->pop();
}
if (verbose()) {
computeActiveErrors();
cerr << "iteration= -1\t chi2= " << activeChi2()
<< "\t time= 0.0"
<< "\t cumTime= 0.0"
<< "\t (using initial guess from " << costFunction.name() << ")" << endl;
}
}
开发者ID:2maz,项目名称:g2o,代码行数:49,代码来源:sparse_optimizer.cpp
示例13: main
int main(int argc, char** argv) {
CommandArgs arg;
std::string outputFilename;
std::string inputFilename;
arg.param("o", outputFilename, "", "output file name");
arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
arg.parseArgs(argc, argv);
OptimizableGraph graph;
if (!graph.load(inputFilename.c_str())){
cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
return 0;
}
HyperGraph::EdgeSet removedEdges;
HyperGraph::VertexSet removedVertices;
for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
HyperGraph::Edge* e = *it;
EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
if (edgePointXY) {
VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
FeaturePointXYData * feature = new FeaturePointXYData();
feature->setPositionMeasurement(edgePointXY->measurement());
feature->setPositionInformation(edgePointXY->information());
pose->addUserData(feature);
removedEdges.insert(edgePointXY);
removedVertices.insert(landmark);
}
}
for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
graph.removeEdge(e);
}
for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
graph.removeVertex(v);
}
if (outputFilename.length()){
graph.save(outputFilename.c_str());
}
}
开发者ID:9578577,项目名称:g2o_frontend,代码行数:48,代码来源:g2o_anonymize_observations.cpp
示例14: connectedSubset
void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited,
HyperGraph::VertexSet& startingSet,
HyperGraph* g, HyperGraph::Vertex* v,
HyperDijkstra::CostFunction* cost, double distance,
double comparisonConditioner, double maxEdgeCost)
{
typedef std::queue<HyperGraph::Vertex*> VertexDeque;
visited.clear();
connected.clear();
VertexDeque frontier;
HyperDijkstra dv(g);
connected.insert(v);
frontier.push(v);
while (! frontier.empty()) {
HyperGraph::Vertex* v0=frontier.front();
frontier.pop();
dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it) {
visited.insert(*it);
if (startingSet.find(*it)==startingSet.end())
continue;
std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
if (insertOutcome.second) { // the node was not in the connectedSet;
frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
}
}
}
}
开发者ID:CHItA,项目名称:ORB_SLAM2,代码行数:28,代码来源:hyper_dijkstra.cpp
示例15: buildIndexMapping
bool SparseOptimizer::initializeOptimization
(HyperGraph::VertexSet& vset, int level)
{
// Recorre todos los vertices introducidos en el optimizador.
// Para cada vertice 'V' obtiene los edges de los que forma parte.
// Para cada uno de esos edges, se mira si todos sus vertices estan en el
// optimizador. Si lo estan, el edge se aniade a _activeEdges.
// Si el vertice 'V' tiene algun edge con todos los demas vertices en el
// optimizador, se aniade 'V' a _activeVertices
// Al final se asignan unos indices internos para los vertices:
// -1: vertices fijos
// 0..n: vertices no fijos y NO marginalizables
// n+1..m: vertices no fijos y marginalizables
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator
it = vset.begin();
it != vset.end();
it++)
{
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator
it = vEdges.begin();
it != vEdges.end();
it++)
{
OptimizableGraph::Edge* e =
reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level)
{
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator
vit = e->vertices().begin();
vit != e->vertices().end();
++vit)
{
if (vset.find(*vit) == vset.end())
{
allVerticesOK = false;
break;
}
}
if (allVerticesOK)
{
auxEdgeSet.insert(reinterpret_cast<OptimizableGraph::Edge*>(*it));
levelEdges++;
}
}
}
if (levelEdges) _activeVertices.push_back(v);
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator
it = auxEdgeSet.begin();
it != auxEdgeSet.end();
++it)
_activeEdges.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
开发者ID:RoboWGT,项目名称:robo_groovy,代码行数:73,代码来源:graph_optimizer_sparse.cpp
示例16: computeSimpleStars
void computeSimpleStars(StarSet& stars,
SparseOptimizer* optimizer,
EdgeLabeler* labeler,
EdgeCreator* creator,
OptimizableGraph::Vertex* gauge_,
std::string edgeTag,
std::string vertexTag,
int level,
int step,
int backboneIterations,
int starIterations,
double rejectionThreshold,
bool debug){
cerr << "preforming the tree actions" << endl;
HyperDijkstra d(optimizer);
// compute a spanning tree based on the types of edges and vertices in the pool
EdgeTypesCostFunction f(edgeTag, vertexTag, level);
d.shortestPaths(gauge_,
&f,
std::numeric_limits< double >::max(),
1e-6,
false,
std::numeric_limits< double >::max()/2);
HyperDijkstra::computeTree(d.adjacencyMap());
// constructs the stars on the backbone
BackBoneTreeAction bact(optimizer, vertexTag, level, step);
bact.init();
cerr << "free edges size " << bact.freeEdges().size() << endl;
// perform breadth-first visit of the visit tree and create the stars on the backbone
d.visitAdjacencyMap(d.adjacencyMap(),&bact,true);
stars.clear();
for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin();
it!=bact.vertexStarMultiMap().end(); it++){
stars.insert(it->second);
}
cerr << "stars.size: " << stars.size() << endl;
cerr << "size: " << bact.vertexStarMultiMap().size() << endl;
// for each star
// for all vertices in the backbone, select all edges leading/leaving from that vertex
// that are contained in freeEdges.
// mark the corresponding "open" vertices and add them to a multimap (vertex->star)
// select a gauge in the backbone
// push all vertices on the backbone
// compute an initial guess on the backbone
// one round of optimization backbone
// lock all vertices in the backbone
// push all "open" vertices
// for each open vertex,
// compute an initial guess given the backbone
// do some rounds of solveDirect
// if (fail)
// - remove the vertex and the edges in that vertex from the star
// - make the structures consistent
// pop all "open" vertices
// pop all "vertices" in the backbone
// unfix the vertices in the backbone
int starNum=0;
for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
Star* s =*it;
HyperGraph::VertexSet backboneVertices = s->_lowLevelVertices;
HyperGraph::EdgeSet backboneEdges = s->_lowLevelEdges;
if (backboneEdges.empty())
continue;
// cerr << "optimizing backbone" << endl;
// one of these should be the gauge, to be simple we select the fisrt one in the backbone
OptimizableGraph::VertexSet gauge;
gauge.insert(*backboneVertices.begin());
s->gauge()=gauge;
s->optimizer()->push(backboneVertices);
s->optimizer()->setFixed(gauge,true);
s->optimizer()->initializeOptimization(backboneEdges);
s->optimizer()->computeInitialGuess();
s->optimizer()->optimize(backboneIterations);
s->optimizer()->setFixed(backboneVertices, true);
// cerr << "assignind edges.vertices not in bbone" << endl;
HyperGraph::EdgeSet otherEdges;
HyperGraph::VertexSet otherVertices;
std::multimap<HyperGraph::Vertex*, HyperGraph::Edge*> vemap;
//.........这里部分代码省略.........
开发者ID:MichaelRuhnke,项目名称:g2o,代码行数:101,代码来源:simple_star_ops.cpp
示例17: assignHierarchicalEdges
void assignHierarchicalEdges(StarSet& stars, EdgeStarMap& esmap, EdgeLabeler* labeler, EdgeCreator* creator, SparseOptimizer* optimizer, int minNumEdges, int maxIterations){
// now construct the hierarchical edges for all the stars
int starNum=0;
for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
cerr << "STAR# " << starNum << endl;
Star* s=*it;
std::vector<OptimizableGraph::Vertex*> vertices(2);
vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();
cerr << "eIs" << endl;
HyperGraph::VertexSet vNew =s->lowLevelVertices();
for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit;
vertices[1]=v;
if (v==vertices[0])
continue;
HyperGraph::EdgeSet eInSt;
int numEdges = vertexEdgesInStar(eInSt, v, s, esmap);
if (Factory::instance()->tag(v)==Factory::instance()->tag(vertices[0]) || numEdges>minNumEdges) {
OptimizableGraph::Edge* e=creator->createEdge(vertices);
//cerr << "creating edge" << e << endl;
if (e) {
e->setLevel(1);
optimizer->addEdge(e);
s->_starEdges.insert(e);
} else {
cerr << "THERE" << endl;
cerr << "FATAL, cannot create edge" << endl;
}
} else {
vNew.erase(v);
// cerr << numEdges << " ";
// cerr << "r " << v-> id() << endl;
// remove from the star all edges that are not sufficiently connected
for (HyperGraph::EdgeSet::iterator it=eInSt.begin(); it!=eInSt.end(); it++){
HyperGraph::Edge* e=*it;
s->lowLevelEdges().erase(e);
}
}
}
s->lowLevelVertices()=vNew;
//cerr << endl;
cerr << "gauge: " << (*s->_gauge.begin())->id()
<< " edges:" << s->_lowLevelEdges.size()
<< " hedges" << s->_starEdges.size() << endl;
const bool debug = false;
if (debug){
char starLowName[100];
sprintf(starLowName, "star-%04d-low.g2o", starNum);
ofstream starLowStream(starLowName);
optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
}
bool labelOk=s->labelStarEdges(maxIterations, labeler);
if (labelOk) {
if (debug) {
char starHighName[100];
sprintf(starHighName, "star-%04d-high.g2o", starNum);
ofstream starHighStream(starHighName);
optimizer->saveSubset(starHighStream, s->_starEdges);
}
} else {
cerr << "FAILURE" << endl;
}
starNum++;
}
}
开发者ID:MichaelRuhnke,项目名称:g2o,代码行数:66,代码来源:simple_star_ops.cpp
示例18: transf
bool G2oSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information)
{
(void) tag;
(void) id;
size_t oldEdgesSize = _optimizer->edges().size();
if (dimension == 3) {
SE2 transf(measurement[0], measurement[1], measurement[2]);
Eigen::Matrix3d infMat;
int idx = 0;
for (int r = 0; r < 3; ++r)
for (int c = r; c < 3; ++c, ++idx) {
assert(idx < (int)information.size());
infMat(r,c) = infMat(c,r) = information[idx];
}
//cerr << PVAR(infMat) << endl;
int doInit = 0;
SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
if (! v1) {
OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
_verticesAdded.insert(v);
doInit = 1;
++_nodesAdded;
}
if (! v2) {
OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
_verticesAdded.insert(v);
doInit = 2;
++_nodesAdded;
}
if (_optimizer->edges().size() == 0) {
cerr << "FIRST EDGE ";
if (v1->id() < v2->id()) {
cerr << "fixing " << v1->id() << endl;
v1->setFixed(true);
}
else {
cerr << "fixing " << v2->id() << endl;
v2->setFixed(true);
}
}
OnlineEdgeSE2* e = new OnlineEdgeSE2;
e->vertices()[0] = v1;
e->vertices()[1] = v2;
e->setMeasurement(transf);
e->setInformation(infMat);
_optimizer->addEdge(e);
_edgesAdded.insert(e);
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
e->initialEstimate(toSet, from);
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
e->initialEstimate(fromSet, to);
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
else if (dimension == 6) {
Eigen::Isometry3d transf;
Matrix<double, 6, 6> infMat;
if (measurement.size() == 7) { // measurement is a Quaternion
Vector7d meas;
for (int i=0; i<7; ++i)
meas(i) = measurement[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
transf = internal::fromVectorQT(meas);
for (int i = 0, idx = 0; i < infMat.rows(); ++i)
for (int j = i; j < infMat.cols(); ++j){
infMat(i,j) = information[idx++];
if (i != j)
infMat(j,i)=infMat(i,j);
//.........这里部分代码省略.........
开发者ID:CreativeCimmons,项目名称:ORB-SLAM-Android-app,代码行数:101,代码来源:g2o_slam_interface.cpp
示例19: main
//.........这里部分代码省略.........
incIterations = 1;
}
int updateDisplayEveryN = updateGraphEachN;
int maxDim = 0;
cerr << "# incremental settings" << endl;
cerr << "#\t solve every " << updateGraphEachN << endl;
cerr << "#\t iterations " << incIterations << endl;
SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
maxDim = max(maxDim, v->dimension());
}
vector<SparseOptimizer::Edge*> edges;
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
edges.push_back(e);
}
optimizer.edges().clear();
optimizer.vertices().clear();
optimizer.setVerbose(false);
// sort the edges in a way that inserting them makes sense
sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
double cumTime = 0.;
int vertexCount=0;
int lastOptimizedVertexCount = 0;
int lastVisUpdateVertexCount = 0;
bool freshlyOptimized=false;
bool firstRound = true;
HyperGraph::VertexSet verticesAdded;
HyperGraph::EdgeSet edgesAdded;
for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
SparseOptimizer::Edge* e = *it;
int doInit = 0;
SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
if (! v1) {
SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
bool v1Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v1Added);
if (! v1Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 1;
if (v->dimension() == maxDim)
vertexCount++;
}
if (! v2) {
SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
bool v2Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v2Added);
if (! v2Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 2;
开发者ID:PennPanda,项目名称:g2o,代码行数:67,代码来源:g2o.cpp
示例20: initializeOptimization
bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
if (edges().size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
return false;
}
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level) {
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
if (vset.find(*vit) == vset.end()) {
allVerticesOK = false;
break;
}
}
if (allVerticesOK && !e->allVerticesFixed()) {
auxEdgeSet.insert(e);
levelEdges++;
}
}
}
if (levelEdges){
_activeVertices.push_back(v);
// test for NANs in the current estimate if we are debugging
# ifndef NDEBUG
int estimateDim = v->estimateDimension();
if (estimateDim > 0) {
Eigen::VectorXd estimateData(estimateDim);
if (v->getEstimateData(estimateData.data()) == true) {
int k;
bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
}
}
# endif
}
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
_activeEdges.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
开发者ID:skarlsson,项目名称:tmp-android,代码行数:62,代码来源:sparse_optimizer.cpp
注:本文中的hypergraph::VertexSet类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论