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