Пример #1
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;
      }
    }
  }
}
Пример #2
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);
  }
}
Пример #3
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);
 
  }
  
}
Пример #4
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());
    }
  }
}
Пример #5
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;

}
Пример #6
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;
}
Пример #7
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

  }