// 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; } } }
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; }
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; }