double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const
 {
   OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
   OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
   SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
   if (it == _graph->activeEdges().end()) // it has to be an active edge
     return std::numeric_limits<double>::max();
   return e->initialEstimatePossible(from, to);
 }
 double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
 {
   OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
   OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
   OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
   if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
     return std::numeric_limits<double>::max();
   SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
   if (it == _graph->activeEdges().end()) // it has to be an active edge
     return std::numeric_limits<double>::max();
   return e->initialEstimatePossible(from_, to);
 }
Exemple #3
0
  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;
    }
  }