obvious::Matrix RandomNormalMatching::match(obvious::Matrix* M, const bool* maskM, obvious::Matrix* NM, obvious::Matrix* S, const bool* maskS, double phiMax, const double transMax, const double resolution) { obvious::Matrix TBest(3, 3); TBest.setIdentity(); const int pointsInM = M->getRows(); const int pointsInS = S->getRows(); if(pointsInM != pointsInS) { LOGMSG(DBG_ERROR, "Model and scene need to be of same size, size of M: " << pointsInM << ", size of S: " << pointsInS); return TBest; } if(pointsInM < 3) { LOGMSG(DBG_ERROR, "Model and scene contain too less points, size of M: " << pointsInM << ", size of S: " << pointsInS); return TBest; } // ----------------- Model ------------------ obvious::Matrix* NMpca = new Matrix(pointsInM, 2); // Normals for model double* phiM = new double[pointsInM]; // Orientation of model points bool* maskMpca = new bool[pointsInM]; // Validity mask of model points memcpy(maskMpca, maskM, pointsInM*sizeof(bool)); if(NM) { calcPhi(NM, maskM, phiM); } else // if normals are not supplied { calcNormals(M, NMpca, maskM, maskMpca, _pcaSearchRange/2); calcPhi(NMpca, maskMpca, phiM); } vector<unsigned int> idxMValid = extractSamples(M, maskMpca, _pcaSearchRange/2); #if USEKNN initKDTree(M, idxMValid); #endif // ------------------------------------------- // ----------------- Scene ------------------- obvious::Matrix* NSpca = new Matrix(pointsInS, 2); // Normals for scene double* phiS = new double[pointsInS]; // Orientation of scene points bool* maskSpca = new bool[pointsInS]; // Validity mask of scene points memcpy(maskSpca, maskS, pointsInS*sizeof(bool)); // Determine number of valid samples in local scene neighborhood // only from these points a valid orientation is computable unsigned int validPoints = 0; for(int i=0; i<pointsInS; i++) if(maskSpca[i]) validPoints++; // Probability of point masking double probability = 180.0/(double)validPoints; if(probability<0.99) subsampleMask(maskSpca, pointsInS, probability); calcNormals(S, NSpca, maskS, maskSpca, _pcaSearchRange/2); calcPhi(NSpca, maskSpca, phiS); vector<unsigned int> idxSValid = extractSamples(S, maskSpca, _pcaSearchRange/2); // ------------------------------------------- // --------------- Control set --------------- vector<unsigned int> idxControl; //represents the indices of points used for Control in S. obvious::Matrix* Control = pickControlSet(S, idxSValid, idxControl); obvious::Matrix* NControl = new obvious::Matrix(idxControl.size(), 2); for(unsigned int i=0; i<Control->getCols(); i++) { (*NControl)(i, 0) = (*NSpca)(idxControl[i], 0); (*NControl)(i, 1) = (*NSpca)(idxControl[i], 1); } unsigned int pointsInC = Control->getCols(); unsigned int cntMatchThresh = pointsInC / 3; // TODO: Determine meaningful parameter double* phiControl = new double[pointsInC]; // Orientation of control points calcPhi(NControl, NULL, phiControl); // -------------------------------------------// // Determine frustum, i.e., direction of leftmost and rightmost model point double thetaBoundMin = atan2((*M)(idxMValid.front(),1), (*M)(idxMValid.front(),0)); // real bounding double thetaBoundMax = atan2((*M)(idxMValid.back(),1), (*M)(idxMValid.back(),0)); // real bounding LOGMSG(DBG_DEBUG, "Valid points in scene: " << idxSValid.size() << ", valid points in model: " << idxMValid.size() << ", Control set: " << Control->getCols()); LOGMSG(DBG_DEBUG, "Model phi min:: " << rad2deg(thetaBoundMin) << ", Model phi max: " << rad2deg(thetaBoundMax)); if(idxSValid.size() < 3) { LOGMSG(DBG_ERROR, "Too less valid points in scene, matchable size: " << idxSValid.size()); return TBest; } if(idxMValid.size() < 3) { LOGMSG(DBG_ERROR, "Too less valid points in model, matchable size: " << idxMValid.size()); return TBest; } // Check for maximum meaningful trials unsigned int trials = _trials; if(idxMValid.size()<_trials) trials = idxMValid.size(); if(_trace) { _trace->reset(); _trace->setModel(M, idxMValid); _trace->setScene(S, idxSValid); } // Calculate search "radius", i.e., maximum difference in polar indices because of rotation phiMax = min(phiMax, M_PI * 0.5); int span; if(resolution > 1e-6) { span = floor(phiMax / resolution); if(span > (int)pointsInM) span = (int)pointsInM; } else { LOGMSG(DBG_ERROR, "Resolution not properly set: resolution = " << resolution); return TBest; } srand (time(NULL)); double bestRatio = 0.0; unsigned int bestCnt = 0; double bestErr = 1e12; #ifndef DEBUG // trace is only possible for single threaded execution if(_trace) { omp_set_num_threads(1); LOGMSG(DBG_WARN, "Configured single-threaded execution due to application of trace module"); } #endif //Timer t; //t.start(); vector<unsigned int> idxTrials = idxMValid; #pragma omp parallel { bool* maskControl = new bool[pointsInC]; double* thetaControl = new double[pointsInC]; #pragma omp for for(unsigned int trial = 0; trial < trials; trial++) { int idx; #pragma omp critical { const int randIdx = rand() % (idxTrials.size()); idx = idxTrials[randIdx]; // remove chosen element to avoid picking same index a second time idxTrials.erase(idxTrials.begin() + randIdx); } // leftmost scene point const int iMin = max(idx-span, _pcaSearchRange/2); // rightmost scene point const int iMax = min(idx+span, pointsInS-_pcaSearchRange/2); for(int i=iMin; i<iMax; i++) { if(maskSpca[i]) { double phi = phiM[idx] - phiS[i]; if(phi>M_PI) phi -= 2.0*M_PI; else if(phi<-M_PI) phi += 2.0*M_PI; if(fabs(phi) < phiMax) { obvious::Matrix T = obvious::MatrixFactory::TransformationMatrix33(phi, 0, 0); // Calculate translation const double sx = (*S)(i,0); const double sy = (*S)(i,1); T(0, 2) = (*M)(idx,0) - (T(0, 0) * sx + T(0, 1) * sy); T(1, 2) = (*M)(idx,1) - (T(1, 0) * sx + T(1, 1) * sy); // Transform control set obvious::Matrix STemp = T * (*Control); unsigned int pointsInControl = STemp.getCols(); // Determine number of control points in field of view unsigned int maxCntMatch = 0; for(unsigned int j=0; j<pointsInControl; j++) { thetaControl[j] = atan2(STemp(1, j), STemp(0, j)); if(thetaControl[j]>thetaBoundMax || thetaControl[j]<thetaBoundMin) { maskControl[j] = false; } else { maskControl[j] = true; maxCntMatch++; } } // Determine how many nearest neighbors (model <-> scene) are close enough unsigned int cntMatch = 0; flann::Matrix<int> indices(new int[1], 1, 1); flann::Matrix<double> dists(new double[1], 1, 1); double errSum = 0; //double scoreSum = 0.0; for(unsigned int s = 0; s < pointsInControl; s++) { // clip points outside of model frustum if(maskControl[s]) { #if USEKNN // find nearest neighbor of control point double q[2]; q[0] = STemp(0, s); q[1] = STemp(1, s); flann::Matrix<double> query(q, 1, 2); flann::SearchParams p(-1, 0.0); _index->knnSearch(query, indices, dists, 1, p); const int idxQuery = idxMValid[indices[0][0]]; double distConsensus = dists[0][0]; #else // speeded-up NN search through back projection const int idxQuery = round((thetaControl[s]-thetaMin) / resolution); if(!maskM[idxQuery]) continue; double distX = (*M)(idxQuery, 0) - STemp(0, s); double distY = (*M)(idxQuery, 1) - STemp(1, s); double distConsensus = distX*distX + distY*distY; #endif #if NORMALCONSENSUS // Experimental idea: rate matching results additionally with normal consensus // consensus score is in range [0, 1] -> perfect match = 0 double normalConsensus = (1.0 - cos(phiM[idxQuery] - phiControl[s] - phi))/2.0; // Normalized error (weight distance and normal consensus) double err = distConsensus*_scaleDistance + normalConsensus*_scaleOrientation; #else double err = distConsensus*_scaleDistance; #endif errSum += err; if(err<1.0) cntMatch++; } } delete[] indices.ptr(); delete[] dists.ptr(); if(cntMatch <= cntMatchThresh) continue; // Experimental rating double ratio = (double)cntMatch / (double) maxCntMatch; #pragma omp critical { // Rating from Markus Kuehn double equalThres = 1e-5; bool rateCondition = ((ratio-bestRatio) > equalThres) && (cntMatch > bestCnt); bool similarityCondition = fabs( (ratio-bestRatio) < equalThres ) && (cntMatch == bestCnt) && errSum < bestErr; bool goodMatch = rateCondition ||similarityCondition; if(goodMatch) { bestRatio = ratio; bestCnt = cntMatch; bestErr = errSum; TBest = T; } } if(_trace) { //trace is only possible for single threaded execution vector<unsigned int> idxM; idxM.push_back(idx); vector<unsigned int> idxS; idxS.push_back(i); _trace->addAssignment(M, idxM, S, idxS, &STemp, errSum, trial); } }// if phiMax } // if maskS } // for i } // for trials delete [] maskControl; } // OMP //cout << "elapsed: " << t.elapsed() << endl; //t.reset(); delete NMpca; delete NSpca; delete [] phiM; delete [] phiS; delete [] phiControl; delete [] maskMpca; delete [] maskSpca; delete Control; return TBest; }
obvious::Matrix TSD_PDFMatching::match( const obvious::Matrix TSensor, const obvious::Matrix* M, const bool* maskM, const obvious::Matrix* NM, const obvious::Matrix* S, const bool* maskS, double phiMax, const double transMax, const double resolution) { obvious::Matrix TBest(3, 3); TBest.setIdentity(); const int pointsInM = M->getRows(); const int pointsInS = S->getRows(); if(pointsInM != pointsInS) { LOGMSG(DBG_ERROR, "Model and scene need to be of same size, size of M: " << pointsInM << ", size of S: " << pointsInS); return TBest; } if(pointsInM < 3) { LOGMSG(DBG_ERROR, "Model and scene contain too less points, size of M: " << pointsInM << ", size of S: " << pointsInS); return TBest; } // ----------------- Model ------------------ obvious::Matrix* NMpca = new Matrix(pointsInM, 2); // Normals for model double* phiM = new double[pointsInM]; // Orientation of model points bool* maskMpca = new bool[pointsInM]; // Validity mask of model points memcpy(maskMpca, maskM, pointsInM * sizeof(bool)); if(NM) { calcPhi(NM, maskM, phiM); } else // if normals are not supplied { calcNormals(M, NMpca, maskM, maskMpca, _pcaSearchRange/2); calcPhi(NMpca, maskMpca, phiM); } vector<unsigned int> idxMValid = extractSamples(M, maskMpca, _pcaSearchRange / 2); // ------------------------------------------- // ----------------- Scene ------------------- obvious::Matrix* NSpca = new Matrix(pointsInS, 2); // Normals for scene double* phiS = new double[pointsInS]; // Orientation of scene points bool* maskSpca = new bool[pointsInS]; // Validity mask of scene points memcpy(maskSpca, maskS, pointsInS * sizeof(bool)); // Determine number of valid samples in local scene neighborhood // only from these points a valid orientation is computable unsigned int validPoints = 0; for(int i = 0; i < pointsInS; i++) if(maskSpca[i]) validPoints++; // Probability of point masking double probability = 180.0 / (double)validPoints; if(probability < 0.99) subsampleMask(maskSpca, pointsInS, probability); calcNormals(S, NSpca, maskS, maskSpca, _pcaSearchRange/2); calcPhi(NSpca, maskSpca, phiS); vector<unsigned int> idxSValid = extractSamples(S, maskSpca, _pcaSearchRange / 2); // ------------------------------------------- // --------------- Control set --------------- vector<unsigned int> idxControl; //represents the indices of points used for Control in S. obvious::Matrix* Control = pickControlSet(S, idxSValid, idxControl); obvious::Matrix* NControl = new obvious::Matrix(idxControl.size(), 2); for(unsigned int i = 0; i < Control->getCols(); i++) { (*NControl)(i, 0) = (*NSpca)(idxControl[i], 0); (*NControl)(i, 1) = (*NSpca)(idxControl[i], 1); } unsigned int pointsInC = Control->getCols(); double* phiControl = new double[pointsInC]; // Orientation of control points calcPhi(NControl, NULL, phiControl); // -------------------------------------------// // Determine frustum, i.e., direction of leftmost and rightmost model point //double thetaMin = -((double)pointsInM - 1.0) / 2.0 * resolution; // theoretical bounding double thetaBoundMin = atan2((*M)(idxMValid.front(), 1), (*M)(idxMValid.front(), 0)); // real bounding double thetaBoundMax = atan2((*M)(idxMValid.back(), 1), (*M)(idxMValid.back(), 0)); // real bounding LOGMSG(DBG_DEBUG, "Valid points in scene: " << idxSValid.size() << ", valid points in model: " << idxMValid.size() << ", Control set: " << Control->getCols()); LOGMSG(DBG_DEBUG, "Model phi min:: " << rad2deg(thetaBoundMin) << ", Model phi max: " << rad2deg(thetaBoundMax)); if(idxSValid.size() < 3) { LOGMSG(DBG_ERROR, "Too less valid points in scene, matchable size: " << idxSValid.size()); return TBest; } if(idxMValid.size() < 3) { LOGMSG(DBG_ERROR, "Too less valid points in model, matchable size: " << idxMValid.size()); return TBest; } // Check for maximum meaningful trials unsigned int trials = _trials; if(idxMValid.size() < _trials) trials = idxMValid.size(); if(_trace) { _trace->reset(); _trace->setModel(M, idxMValid); _trace->setScene(S, idxSValid); } // Calculate search "radius", i.e., maximum difference in polar indices because of rotation phiMax = min(phiMax, M_PI * 0.5); int span; if(resolution > 1e-6) { span = floor(phiMax / resolution); if(span > (int)pointsInM) span = (int)pointsInM; } else { LOGMSG(DBG_ERROR, "Resolution not properly set: resolution = " << resolution); return TBest; } srand(time(NULL)); double bestProb = 0.0; #ifndef DEBUG // trace is only possible for single threaded execution if(_trace) { omp_set_num_threads(1); LOGMSG(DBG_WARN, "Configured single-threaded execution due to application of trace module"); } #endif //Timer t; //t.start(); vector<unsigned int> idxTrials = idxMValid; bool* maskControl = new bool[pointsInC]; #pragma omp parallel for for(unsigned int trial = 0; trial < trials; trial++) { int idx; #pragma omp critical { const int randIdx = rand() % (idxTrials.size()); idx = idxTrials[randIdx]; // remove chosen element to avoid picking same index a second time idxTrials.erase(idxTrials.begin() + randIdx); } // leftmost scene point const int iMin = max(idx - span, _pcaSearchRange / 2); // rightmost scene point const int iMax = min(idx + span, pointsInS - _pcaSearchRange / 2); for(int i = iMin; i < iMax; i++) { if(maskSpca[i]) { double phi = phiM[idx] - phiS[i]; if(phi > M_PI) phi -= 2.0 * M_PI; else if(phi < -M_PI) phi += 2.0 * M_PI; if(fabs(phi) < phiMax) { obvious::Matrix T = obvious::MatrixFactory::TransformationMatrix33(phi, 0, 0); // Calculate translation const double sx = (*S)(i, 0); const double sy = (*S)(i, 1); T(0, 2) = (*M)(idx, 0) - (T(0, 0) * sx + T(0, 1) * sy); T(1, 2) = (*M)(idx, 1) - (T(1, 0) * sx + T(1, 1) * sy); obvious::Matrix TMap = TSensor * T; // Transform control set obvious::Matrix STemp = TMap * (*Control); unsigned int pointsInControl = STemp.getCols(); // Rating Daniel Ammon & Tobias Fink std::vector<double> probOfAllScans; // vector for probabilities of single scans in one measurement double probOfActualMeasurement = 1.0; for (unsigned int s = 0; s < pointsInControl; s++) // whole control set { obfloat coord[2]; coord[0] = STemp(0, s); coord[1] = STemp(1, s); // todo: magic numbers 0.05 / 0.95 obfloat tsd; if( !_grid.interpolateBilinear(coord, &tsd) ) { // rating function: clipped probability --> avoid prob of 0 // multiply all probabilities for probability of whole scan probOfActualMeasurement *= (1.0 - (1.0 - _zrand) * fabs(tsd)); } else { probOfActualMeasurement *= _zrand; } } // whole control set #pragma omp critical { // update T and bestProb if better than last iteration if(probOfActualMeasurement > bestProb) { TBest = T; bestProb = probOfActualMeasurement; #ifndef DEBUG if(_trace) { //trace is only possible for single threaded execution vector<unsigned int> idxM; idxM.push_back(idx); vector<unsigned int> idxS; idxS.push_back(i); _trace->addAssignment(M, idxM, S, idxS, &STemp, 10 * probOfActualMeasurement, trial); } #endif } } } // if(fabs(phi) < phiMax) } // if(maskSpca[i]) } // for i } // for trials //cout << "elapsed: " << t.elapsed() << endl; //t.reset(); delete [] maskControl; delete NMpca; delete NSpca; delete [] phiM; delete [] phiS; delete [] phiControl; delete [] maskMpca; delete [] maskSpca; delete Control; return TBest; }