C++ VertexSet::insert方法代码示例

void GraphSLAM::addNeighboringVertices(OptimizableGraph::VertexSet& vset, int gap){
  OptimizableGraph::VertexSet temp = vset;
  for (OptimizableGraph::VertexSet::iterator it = temp.begin(); it!=temp.end(); it++){
    OptimizableGraph::Vertex* vertex = (OptimizableGraph::Vertex*) *it;
    for (int i = 1; i <= gap; i++){
      OptimizableGraph::Vertex *v = (OptimizableGraph::Vertex *) _graph->vertex(vertex->id()+i);
      if (v && v->id() != _lastVertex->id()){
	OptimizableGraph::VertexSet::iterator itv = vset.find(v);
	if (itv == vset.end())
	  vset.insert(v);
	else
	  break;
      }
    }

    for (int i = 1; i <= gap; i++){
      OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) _graph->vertex(vertex->id()-i);
      if (v && v->id() != _lastVertex->id()){
	OptimizableGraph::VertexSet::iterator itv = vset.find(v);
	if (itv == vset.end())
	  vset.insert(v);
	else
	  break;
      }
    }
  }
}

bool ScanMatcher::globalMatching(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::Vertex* _currentVertex,  SE2 *trel, double maxScore){
 
  OptimizableGraph::VertexSet vset;
  vset.insert(_currentVertex);
  return globalMatching(referenceVset, _referenceVertex,  vset, _currentVertex, trel, maxScore);

}

bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset,  OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::Vertex* _currentVertex,  std::vector<SE2>& trel, double maxScore){ 

  OptimizableGraph::VertexSet  currvset;
  currvset.insert(_currentVertex);

  return scanMatchingLC(referenceVset, _referenceVertex, currvset, _currentVertex, trel, maxScore);
  //return scanMatchingLChierarchical(referenceVset, _originVertex, currvset, _currentVertex, trel, maxScore);
}

 void EstimatePropagator::propagate(OptimizableGraph::Vertex* v, 
     const EstimatePropagator::PropagateCost& cost, 
      const EstimatePropagator::PropagateAction& action,
      double maxDistance, 
      double maxEdgeCost)
 {
   OptimizableGraph::VertexSet vset;
   vset.insert(v);
   propagate(vset, cost, action, maxDistance, maxEdgeCost);
 }

  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;
//.........这里部分代码省略.........

void GraphSLAM::addDataSM(SE2 currentOdom, RobotLaser* laser){
  boost::mutex::scoped_lock lockg(graphMutex);

  //Add current vertex
  VertexSE2 *v = new VertexSE2;

  SE2 displacement = _lastOdom.inverse() * currentOdom;
  SE2 currEst = _lastVertex->estimate() * displacement;

  v->setEstimate(currEst);
  v->setId(++_runningVertexId + idRobot() * baseId());
  //Add covariance information
  //VertexEllipse *ellipse = new VertexEllipse;
  //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
  //ellipse->setCovariance(cov);
  //v->setUserData(ellipse);
  v->addUserData(laser);

  std::cout << endl << 
    "Current vertex: " << v->id() << 
    " Estimate: "<< v->estimate().translation().x() << 
    " " << v->estimate().translation().y() << 
    " " << v->estimate().rotation().angle() << std::endl;

  _graph->addVertex(v);

  //Add current odometry edge
  EdgeSE2 *e = new EdgeSE2;
  e->setId(++_runningEdgeId + idRobot() * baseId());
  e->vertices()[0] = _lastVertex;
  e->vertices()[1] = v;
      

  OptimizableGraph::VertexSet vset;
  vset.insert(_lastVertex);
  int j = 1;
  int gap = 5;
  while (j <= gap){
    VertexSE2 *vj =  dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
    if (vj)
      vset.insert(vj);
    else
      break;
    j++;
  }

  SE2 transf;
  bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v,  &transf, maxScore);

  if (shouldIAdd){
    e->setMeasurement(transf);
    e->setInformation(_SMinf);
  }else{ //Trust the odometry
    e->setMeasurement(displacement);
    // Vector3d dis = displacement.toVector();
    // dis.x() = fabs(dis.x());
    // dis.y() = fabs(dis.y());
    // dis.z() = fabs(dis.z());
    // dis += Vector3d(1e-3,1e-3,1e-2);  
    // Matrix3d dis2 = dis*dis.transpose();
    // Matrix3d newcov = dis2.cwiseProduct(_odomK);
    // e->setInformation(newcov.inverse());

    e->setInformation(_odominf);
  }

  _graph->addEdge(e);

  _lastOdom = currentOdom;
  _lastVertex = v;
}