Beispiel #1
0
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);

}
Beispiel #2
0
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);
}
Beispiel #3
0
void GraphSLAM::checkHaveLaser(OptimizableGraph::VertexSet& vset){
  OptimizableGraph::VertexSet tmpvset = vset;
  for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
    OptimizableGraph::Vertex *vertex = (OptimizableGraph::Vertex*) *it;
    if (!findLaserData(vertex))
      vset.erase(*it);
  }
}
Beispiel #4
0
 void EdgePointXYCovPointXYCov::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to)
 {
   assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXYCov");
   VertexPointXYCov* vi     = static_cast<VertexPointXYCov*>(_vertices[0]);
   VertexPointXYCov* vj = static_cast<VertexPointXYCov*>(_vertices[1]);
   if (from.count(vi) > 0 && to == vj) {
     vj->estimate() = vi->estimate(); // * _measurement;
   }
 }
Beispiel #5
0
 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);
 }
Beispiel #6
0
  void EdgeSE2PointXYCalib::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY");

    if (from.count(_vertices[0]) != 1)
      return;
    VertexSE2* vi     = static_cast<VertexSE2*>(_vertices[0]);
    VertexPointXY* vj = static_cast<VertexPointXY*>(_vertices[1]);
    vj->setEstimate(vi->estimate() * _measurement);
  }
Beispiel #7
0
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;
      }
    }
  }
}
Beispiel #8
0
  void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
    Eigen::Vector3d p;
    p(2) = _measurement(2);
    p.head<2>() = _measurement.head<2>()*p(2);
    p=invKcam*p;
    point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
  }
  void EdgeSE2PointXYBearing::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY");

    if (from.count(_vertices[0]) != 1)
      return;
    double r=2.;
    const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]);
    VertexPointXY* l2 = static_cast<VertexPointXY*>(_vertices[1]);
    SE2 t=v1->estimate();
    t.setRotation(t.rotation().angle()+_measurement);
    Vector2d vr;
    vr[0]=r; vr[1]=0;
    l2->setEstimate(t*vr);
  }
  void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    // SE3OffsetCache* vcache = (SE3OffsetCache* ) cam->getCache(_cacheIds[0]);
    // if (! vcache){
    //   cerr << "fatal error in retrieving cache" << endl;
    // }
    // SE3OffsetParameters* params=vcache->params;
    Eigen::Vector3d p=_measurement;
    point->setEstimate(cam->estimate() * (offsetParam->offset() * p));
  }
Beispiel #11
0
 void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /* to */)
 {
   VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
   VertexSE2* toEdge   = static_cast<VertexSE2*>(_vertices[1]);
   if (from.count(fromEdge) > 0)
     toEdge->setEstimate(fromEdge->estimate() * _measurement);
   else
     fromEdge->setEstimate(toEdge->estimate() * _inverseMeasurement);
 }
Beispiel #12
0
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
  ///////////////////////////////////
  // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one

  CovarianceEstimator ce(_graph);

  ce.setVertices(vset);
  ce.setGauge(_lastVertex);
  
  ce.compute();

  assert(!_lastVertex->fixed() && "last Vertex is fixed");
  assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
  
  OptimizableGraph::VertexSet tmpvset = vset;
  for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    
    MatrixXd Pv = ce.getCovariance(vertex);
    Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
    SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();	
    Vector2d hxy (delta.translation().x(), delta.translation().y());
    double perceptionRange =1;
    if (hxy.x()-perceptionRange>0) 
      hxy.x() -= perceptionRange;
    else if (hxy.x()+perceptionRange<0)
      hxy.x() += perceptionRange;
    else
      hxy.x() = 0;

    if (hxy.y()-perceptionRange>0) 
      hxy.y() -= perceptionRange;
    else if (hxy.y()+perceptionRange<0)
      hxy.y() += perceptionRange;
    else
      hxy.y() = 0;
    
    double d2 = hxy.transpose() * Pxy.inverse() * hxy;
    if (d2 > 5.99)
      vset.erase(*it);
 
  }
  
}
Beispiel #13
0
void EdgeSE3::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
    VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);

    if (from_.count(from) > 0) {
        to->setEstimate(from->estimate() * _measurement);
    } else
        from->setEstimate(to->estimate() * _measurement.inverse());
    //cerr << "IE" << endl;
}
void EdgeSE2PlaceVicinity::initialEstimate(
		const OptimizableGraph::VertexSet& from,
		OptimizableGraph::Vertex* /*to*/) {
	VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
	VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);

	if (from.count(fromEdge) > 0)
		toEdge->setEstimate(fromEdge->estimate());
	else
		fromEdge->setEstimate(toEdge->estimate());
  }
Beispiel #15
0
  void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
    VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);

    Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
    
    if (from_.count(from) > 0) {
      to->setEstimate(from->estimate() * virtualMeasurement);
    } else
      from->setEstimate(to->estimate() * virtualMeasurement.inverse());
  }
  void EdgeSE3PointXYZDisparity::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);

    // VertexCameraCache* vcache = (VertexCameraCache* ) cam->getCache(_cacheIds[0]);
    // if (! vcache){
    //   cerr << "fatal error in retrieving cache" << endl;
    // }
    //ParameterCamera* params=vcache->params;
    const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
    Eigen::Vector3d p;
    double w=1./_measurement(2);
    p.head<2>() = _measurement.head<2>()*w;
    p(2) = w;
    p = invKcam * p;
    p = cam->estimate() * (params->offset() * p);
    point->setEstimate(p);
  }
Beispiel #17
0
 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);
 }
Beispiel #18
0
void transformPointsFromVSet(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _referenceVertex, RawLaser::Point2DVector& scansInRefVertex){

  VertexSE2* referenceVertex=dynamic_cast<VertexSE2*>(_referenceVertex);
  scansInRefVertex.clear();
  for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
    if (laserv){
      RawLaser::Point2DVector vscan = laserv->cartesian();
      SE2 trl = laserv->laserParams().laserPose;
      RawLaser::Point2DVector scanInRefVertex;
      if (vertex->id() == referenceVertex->id()){
	ScanMatcher::applyTransfToScan(trl, vscan, scanInRefVertex);
      }else{
	SE2 trel = referenceVertex->estimate().inverse() * vertex->estimate();
	SE2 transf = trel * trl;
	ScanMatcher::applyTransfToScan(transf, vscan, scanInRefVertex);
      }
      scansInRefVertex.insert(scansInRefVertex.end(), scanInRefVertex.begin(), scanInRefVertex.end());
    }
  }
}
void EdgeSE2DistanceOrientation::initialEstimate(
		const OptimizableGraph::VertexSet& from,
		OptimizableGraph::Vertex* /*to*/) {
	VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]);
	VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]);


	double dist = sqrt(_measurement[0] * _measurement[0] + _measurement[1] * _measurement[1]);
	double theta = _measurement[2];
	double x = dist * cos(theta), y = dist * sin(theta);
	SE2 tmpMeasurement(x,y,theta);
	SE2 inverseTmpMeasurement  = tmpMeasurement.inverse();
	if (from.count(fromEdge) > 0)
		toEdge->setEstimate(fromEdge->estimate() * tmpMeasurement);
	else
		fromEdge->setEstimate(toEdge->estimate() * inverseTmpMeasurement);
  }
Beispiel #20
0
  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;
      for (HyperGraph::VertexSet::iterator bit=backboneVertices.begin(); bit!=backboneVertices.end(); bit++){
	HyperGraph::Vertex* v=*bit;
	for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
	  OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
	  HyperGraph::EdgeSet::iterator feit=bact.freeEdges().find(e);
	  if (feit!=bact.freeEdges().end()){ // edge is admissible
	    otherEdges.insert(e);
	    bact.freeEdges().erase(feit);
	    for (size_t i=0; i<e->vertices().size(); i++){
	      OptimizableGraph::Vertex* ve= (OptimizableGraph::Vertex*)e->vertices()[i];
	      if (backboneVertices.find(ve)==backboneVertices.end()){
		otherVertices.insert(ve);
		vemap.insert(make_pair(ve,e));
	      }
	    }
	  }
	}
      }

      // RAINER TODO maybe need a better solution than dynamic casting here??
      OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*>(s->optimizer()->solver());
      if (solverWithHessian) {
        s->optimizer()->push(otherVertices);
        // cerr << "optimizing vertices out of bbone" << endl;
        // cerr << "push" << endl;
        // cerr << "init" << endl;
        s->optimizer()->initializeOptimization(otherEdges);
        // cerr << "guess" << endl;
        s->optimizer()->computeInitialGuess();
        // cerr << "solver init" << endl;
        s->optimizer()->solver()->init();
        // cerr << "structure" << endl;
        if (!solverWithHessian->buildLinearStructure())
          cerr << "FATAL: failure while building linear structure" << endl;
        // cerr << "errors" << endl;
        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        solverWithHessian->updateLinearSystem();
        // cerr << "directSolove" << endl;
      } else {
        cerr << "FATAL: hierarchical thing cannot be used with a solver that does not support the system structure construction" << endl;
      }


      // // then optimize the vertices one at a time to check if a solution is good
      for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){
        OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
        v->solveDirect();
        // cerr << " " << d;
        // if  a solution is found, add a vertex and all the edges in
        //othervertices that are pointing to that edge to the star
        s->_lowLevelVertices.insert(v);
        for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
          OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
          if (otherEdges.find(e)!=otherEdges.end())
            s->_lowLevelEdges.insert(e);
        }
      }
      //cerr <<  endl;

      // relax the backbone and optimize it all
      // cerr << "relax bbone" << endl;
      s->optimizer()->setFixed(backboneVertices, false);
      //cerr << "fox gauge bbone" << endl;
      s->optimizer()->setFixed(s->gauge(),true);

      //cerr << "opt init" << endl;
      s->optimizer()->initializeOptimization(s->_lowLevelEdges);
      optimizer->computeActiveErrors();
      double initialChi = optimizer->activeChi2();
      int starOptResult = s->optimizer()->optimize(starIterations);
      //cerr << starOptResult << "(" << starIterations << ")  " << endl;
      double finalchi=-1.;

      cerr <<  "computing star: " << starNum << endl;

      int vKept=0, vDropped=0;
      if (!starIterations || starOptResult > 0  ){
	optimizer->computeActiveErrors();
	finalchi = optimizer->activeChi2();

#if 1

        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        if (solverWithHessian)
          solverWithHessian->updateLinearSystem();
        HyperGraph::EdgeSet prunedStarEdges = backboneEdges;
        HyperGraph::VertexSet prunedStarVertices = backboneVertices;
        for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){

	  //discard the vertices whose error is too big


          OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
          MatrixXd h(v->dimension(), v->dimension());
          for (int i=0; i<v->dimension(); i++){
            for (int j=0; j<v->dimension(); j++)
              h(i,j)=v->hessian(i,j);
          }
          EigenSolver<Eigen::MatrixXd> esolver;
          esolver.compute(h);
          VectorXcd ev= esolver.eigenvalues();
          double emin = std::numeric_limits<double>::max();
          double emax = -std::numeric_limits<double>::max();
          for (int i=0; i<ev.size(); i++){
            emin = ev(i).real()>emin ? emin : ev(i).real();
            emax = ev(i).real()<emax ? emax : ev(i).real();
          }

          double d=emin/emax;


          // cerr << " " << d;
          if (d>rejectionThreshold){
	  // if  a solution is found, add a vertex and all the edges in
	  //othervertices that are pointing to that edge to the star
            prunedStarVertices.insert(v);
            for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
              OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
              if (otherEdges.find(e)!=otherEdges.end())
                prunedStarEdges.insert(e);
            }
            //cerr << "K( " << v->id() << "," << d << ")" ;
            vKept ++;
          } else {
            vDropped++;
            //cerr << "R( " << v->id() << "," << d << ")" ;
          }
        }
        s->_lowLevelEdges=prunedStarEdges;
        s->_lowLevelVertices=prunedStarVertices;

#endif
	//cerr << "addHedges" << endl;
	//now add to the star the hierarchical edges
	std::vector<OptimizableGraph::Vertex*> vertices(2);
	vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();

	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;
	  OptimizableGraph::Edge* e=creator->createEdge(vertices);
	  //rr << "creating edge" << e <<  Factory::instance()->tag(vertices[0]) << "->" <<  Factory::instance()->tag(v) <endl;
	  if (e) {
	    e->setLevel(level+1);
	    optimizer->addEdge(e);
	    s->_starEdges.insert(e);
	  } else {
            cerr << "HERE" << endl;
	    cerr << "FATAL, cannot create edge" << endl;
	  }
	}
      }

      cerr << " gauge: " << (*s->_gauge.begin())->id()
           << " kept: " << vKept
           << " dropped: " << vDropped
	   << " edges:" << s->_lowLevelEdges.size()
	   << " hedges" << s->_starEdges.size()
	   << " initial chi " << initialChi
	   << " final chi " << finalchi << endl;

      if (debug) {
	char starLowName[100];
	sprintf(starLowName, "star-%04d-low.g2o", starNum);
	ofstream starLowStream(starLowName);
	optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
      }
      bool labelOk=false;
      if (!starIterations || starOptResult > 0)
        labelOk = s->labelStarEdges(0, 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: " << starOptResult << endl;
      }
      starNum++;

      //label each hierarchical edge
      s->optimizer()->pop(otherVertices);
      s->optimizer()->pop(backboneVertices);
      s->optimizer()->setFixed(s->gauge(),false);
    }


    StarSet stars2;
    // now erase the stars that have 0 edges. They r useless
    for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
      Star* s=*it;
      if (s->lowLevelEdges().size()==0) {
        delete s;
      } else
        stars2.insert(s);
    }
    stars=stars2;
  }
Beispiel #21
0
  void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, 
      const EstimatePropagator::PropagateCost& cost, 
       const EstimatePropagator::PropagateAction& action,
       double maxDistance, 
       double maxEdgeCost)
  {
    reset();

    PriorityQueue frontier;
    for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
      AdjacencyMap::iterator it = _adjacencyMap.find(v);
      assert(it != _adjacencyMap.end());
      it->second._distance = 0.;
      it->second._parent.clear();
      it->second._frontierLevel = 0;
      frontier.push(&it->second);
    }

    while(! frontier.empty()){
      AdjacencyMapEntry* entry = frontier.pop();
      OptimizableGraph::Vertex* u = entry->child();
      double uDistance = entry->distance();
      //cerr << "uDistance " << uDistance << endl;

      // initialize the vertex
      if (entry->_frontierLevel > 0) {
        action(entry->edge(), entry->parent(), u);
      }

      /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
      OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
      while (et != u->edges().end()){
        OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
        ++et;

        int maxFrontier = -1;
        OptimizableGraph::VertexSet initializedVertices;
        for (size_t i = 0; i < edge->vertices().size(); ++i) {
          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
          AdjacencyMap::iterator ot = _adjacencyMap.find(z);
          if (ot->second._distance != numeric_limits<double>::max()) {
            initializedVertices.insert(z);
            maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
          }
        }
        assert(maxFrontier >= 0);

        for (size_t i = 0; i < edge->vertices().size(); ++i) {
          OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
          if (z == u)
            continue;

          size_t wasInitialized = initializedVertices.erase(z);

          double edgeDistance = cost(edge, initializedVertices, z);
          if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
            double zDistance = uDistance + edgeDistance;
            //cerr << z->id() << " " << zDistance << endl;

            AdjacencyMap::iterator ot = _adjacencyMap.find(z);
            assert(ot!=_adjacencyMap.end());

            if (zDistance < ot->second.distance() && zDistance < maxDistance){
              //if (ot->second.inQueue)
                //cerr << "Updating" << endl;
              ot->second._distance = zDistance;
              ot->second._parent = initializedVertices;
              ot->second._edge = edge;
              ot->second._frontierLevel = maxFrontier + 1;
              frontier.push(&ot->second);
            }
          }

          if (wasInitialized > 0)
            initializedVertices.insert(z);

        }
      }
    }

    // writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
    cerr << "Writing cost.dat" << endl;
    ofstream costStream("cost.dat");
    for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
      HyperGraph::Vertex* u = it->second.child();
      costStream << "vertex " << u->id() << "  cost " << it->second._distance << endl;
    }
    cerr << "Writing init.dat" << endl;
    ofstream initStream("init.dat");
    vector<AdjacencyMapEntry*> frontierLevels;
    for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
      if (it->second._frontierLevel > 0)
        frontierLevels.push_back(&it->second);
    }
    sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
    for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
      AdjacencyMapEntry* entry       = *it;
      OptimizableGraph::Vertex* to   = entry->child();

      initStream << "calling init level = " << entry->_frontierLevel << "\t (";
      for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
        initStream << " " << (*pit)->id();
      }
      initStream << " ) -> " << to->id() << endl;
    }
#endif

  }
Beispiel #22
0
bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset,  OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex,  std::vector<SE2>& trel, double maxScore){ 
  cerr << "Loop Closing Scan Matching" << endl;
  //cerr << "Size of Vset " << referenceVset.size() << endl;
  VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex);

  resetGrid();
  trel.clear();
  
  RawLaser::Point2DVector scansInRefVertex;
  transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex);
  _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel);

  RawLaser::Point2DVector scansInCurVertex;
  transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex);

  Vector2dVector reducedScans;
  CharGrid::subsample(reducedScans, scansInCurVertex, 0.1);

  RegionVector regions;
  RegionVector regionspi;
  for (OptimizableGraph::VertexSet::iterator it = referenceVset.begin(); it != referenceVset.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;

    Region reg;
    SE2 relposv(.0, .0, .0);
    if (vertex->id() != referenceVertex->id())
      relposv = referenceVertex->estimate().inverse() * vertex->estimate();
    
    Vector3f lower(-.5+relposv.translation().x(), -2.+relposv.translation().y(), -1.+relposv.rotation().angle());
    Vector3f upper( .5+relposv.translation().x(),  2.+relposv.translation().y(),  1.+relposv.rotation().angle());
    reg.lowerLeft  = lower;
    reg.upperRight = upper;
    regions.push_back(reg);
  
    lower[2] += M_PI;
    upper[2] += M_PI; 
    reg.lowerLeft  = lower;
    reg.upperRight = upper;
    regionspi.push_back(reg);
  }

  std::vector<MatcherResult> mresvec;
  double thetaRes = 0.025; // was 0.0125*.5
  //Results discretization
  double dx = 0.5, dy = 0.5, dth = 0.2;
  
  std::map<DiscreteTriplet, MatcherResult> resultsMap;
  
  clock_t t_ini, t_fin;
  double secs;
  
  t_ini = clock();
  _grid.greedySearch(mresvec, reducedScans, regions, thetaRes, maxScore, dx, dy, dth);
  t_fin = clock();
  secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
  printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
  
  if (mresvec.size()){
    mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
    cerr << "Found Loop Closure Edge. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;

    CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
  }

  t_ini = clock();
  _grid.greedySearch(mresvec, reducedScans, regionspi, thetaRes, maxScore, dx, dy, dth);
  t_fin = clock();
  secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
  printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());

  if (mresvec.size()){
    mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
    cerr << "Found Loop Closure Edge PI. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;

    CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
  }

  for (std::map<DiscreteTriplet, MatcherResult>::iterator it = resultsMap.begin(); it!= resultsMap.end(); it++){
    MatcherResult res = it->second;
    Vector3d adj=res.transformation;
    SE2 transf;
    transf.setTranslation(Vector2d(adj.x(), adj.y()));
    transf.setRotation(normalize_theta(adj.z()));
    trel.push_back(transf);
    
    std::cerr << "Final result: " << transf.translation().x() << " " << transf.translation().y() << " " << transf.rotation().angle() << std::endl;
  }

  if (trel.size())
    return true;

  return false;
}
Beispiel #23
0
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;
}
Beispiel #24
0
bool ScanMatcher::verifyMatching(OptimizableGraph::VertexSet& vset1, OptimizableGraph::Vertex* _referenceVertex1, 
				 OptimizableGraph::VertexSet& vset2, OptimizableGraph::Vertex* _referenceVertex2, 
				 SE2 trel12, double *score){ 

  VertexSE2* referenceVertex2=dynamic_cast<VertexSE2*>(_referenceVertex2);

  resetGrid();
  CharGrid auxGrid = _grid;

  //Transform points from vset2 in the reference of referenceVertex1 using trel12
  RawLaser::Point2DVector scansvset2inref1;
  for (OptimizableGraph::VertexSet::iterator it = vset2.begin(); it != vset2.end(); it++){
    VertexSE2 *vertex = (VertexSE2*) *it;
    RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
    RawLaser::Point2DVector vscan = laserv->cartesian();
    SE2 trl = laserv->laserParams().laserPose;

    RawLaser::Point2DVector scanInRefVertex1;
    if (vertex->id() == referenceVertex2->id()){
      applyTransfToScan(trel12 * trl, vscan, scanInRefVertex1);
    }else{
      //Transform scans to the referenceVertex2 coordinates
      SE2 tref2_v = referenceVertex2->estimate().inverse() * vertex->estimate();
      applyTransfToScan(trel12 * tref2_v * trl, vscan, scanInRefVertex1);
    }
    scansvset2inref1.insert(scansvset2inref1.end(), scanInRefVertex1.begin(), scanInRefVertex1.end());
  }

  //Scans in vset1
  RawLaser::Point2DVector scansvset1;
  transformPointsFromVSet(vset1, _referenceVertex1, scansvset1);

  //Add local map from vset2 into the grid
  _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset2inref1.begin(), scansvset2inref1.end(), _kernel);
  //Find points from vset1 not explained by map vset2
  RawLaser::Point2DVector nonmatchedpoints;
  _grid.searchNonMatchedPoints(scansvset1, nonmatchedpoints, .3);

  //Add those points to a grid to count them
  auxGrid.addAndConvolvePoints<RawLaser::Point2DVector>(nonmatchedpoints.begin(), nonmatchedpoints.end(), _kernel);

  // ofstream image1;
  // std::stringstream filename1;
  // filename1 << "map2.ppm";
  // image1.open(filename1.str().c_str());
  // _LCGrid.grid().saveAsPPM(image1, false);

  // ofstream image2;
  // std::stringstream filename2;
  // filename2 << "mapnonmatched.ppm";
  // image2.open(filename2.str().c_str());
  // auxGrid.grid().saveAsPPM(image2, false);

  // //Just for saving the image
  // resetLCGrid();
  // _LCGrid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset1.begin(), scansvset1.end(), _LCKernel);
  // ofstream image3;
  // std::stringstream filename3;
  // filename3 << "map1.ppm";
  // image3.open(filename3.str().c_str());
  // _LCGrid.grid().saveAsPPM(image3, false);


  //Counting points around trel12
  Vector2f lower(-.3+trel12.translation().x(), -.3+trel12.translation().y());
  Vector2f upper(+.3+trel12.translation().x(), +.3+trel12.translation().y()); 
  
  auxGrid.countPoints(lower, upper, score);
  cerr << "Score: " << *score << endl;
  double threshold = 40.0;
  if (*score <= threshold)
    return true;
  
  return false;

}