Example #1
0
// records regions of contiguous identity in the alignment
void CColorspaceUtilities::FindIndenticalRegions(char* pReference, char* pQuery, const unsigned short pairwiseLen, RegionVector& rv) {

    for(unsigned short i = 0; i < pairwiseLen; i++) {
        if(pReference[i] == pQuery[i]) {
            RegionT r(i);
            unsigned short end = i;
            while((end < pairwiseLen) && (pReference[end] == pQuery[end])) {
                ++r.Length;
                ++end;
            }
            rv.push_back(r);
            i = end;
        }
    }
}
Example #2
0
bool ScanMatcher::scanMatchingLChierarchical(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* currentVertex=dynamic_cast<VertexSE2*>(_currentVertex);
  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);
  //cerr << "subsampling: " << scansInCurVertex.size() << " -> " << reducedScans.size() << endl;

  SE2 delta = referenceVertex->estimate().inverse() * currentVertex->estimate();

  Vector3d initGuess(delta.translation().x(), delta.translation().y(), delta.rotation().angle());

  Vector3f lower(-2.+initGuess.x(), -2.+initGuess.y(), -1.+initGuess.z());
  Vector3f upper(+2.+initGuess.x(),  2.+initGuess.y(),  1.+initGuess.z()); 

  RegionVector regions;
  Region reg;
  reg.lowerLeft  = lower;
  reg.upperRight = upper;
  regions.push_back(reg);

  std::vector<MatcherResult> mresvec;
  double thetaRes = 0.025; // was 0.0125*.5
 
  // clock_t t_ini, t_fin;
  // double secs;
  
  // t_ini = clock();
  _grid.hierarchicalSearch(mresvec, reducedScans, regions, thetaRes, maxScore, 0.5, 0.5, 0.2, 3);
  // 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()){
    Vector3d adj=mresvec[0].transformation;
    SE2 transf;
    transf.setTranslation(Vector2d(adj.x(), adj.y()));
    transf.setRotation(adj.z());
    //    cerr <<  " bestScore = " << mresvec[0].score << endl; 
    //cerr << "Found Loop Closure Edge. Transf: " << adj.x() << " " << adj.y() << " " << adj.z() << endl << endl;

    trel.push_back(transf);
  }

  if (trel.size())
    return true;

  return false;
}
Example #3
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;
}