Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

KnowledgeBase.cc

Go to the documentation of this file.
00001 #include "KnowledgeBase.h"
00002 
00003 #include "keypoint.h"
00004 #include "keygroup.h"
00005 #include "keypointpair.h"
00006 #include "model.h"
00007 #include "object.h"
00008 #include "matchinfo.h"
00009 
00010 #include "HoughHash.h"
00011 #include "KDTree.h"
00012 #include "Shared/newmat/newmat.h"
00013 
00014 #include <limits>
00015 #include "Shared/mathutils.h" // for isnan fix
00016 
00017 using namespace std;
00018 
00019 const int numParams = 2;
00020 const char* paramList[numParams] = {"probOfMatch", "errorThreshold"};
00021 
00022 int getNumParams(){
00023   return numParams;
00024 }
00025 
00026 const char* getParamList(int i){
00027   return paramList[i];
00028 }
00029 
00030 double choose(int n, int r){
00031   if (n < 1 || r < 0) return -0.0;
00032   if (r < n-r) r = n-r;
00033   
00034   double numerator = 1;
00035   double denominator = 1;
00036   
00037   for (int i = r+1; i <= n; i++){
00038     numerator *= (double)i;
00039     //    cout << "numerator " << numerator << endl;
00040   }
00041   for (int i = 1; i <= n-r; i++){
00042     denominator *= (double)i;
00043     //    cout << "denominator " << denominator << endl;
00044   }
00045   
00046   if (numerator == std::numeric_limits<double>::infinity()) return numerator;
00047   
00048   return numerator/denominator;
00049 }
00050 
00051 NEWMAT::Matrix leastSquareSolution(std::vector<keypointPair*> keyPairs, double *error){
00052   // make assumption that #keys = #matches
00053   int numKeys = (int)keyPairs.size();
00054   //  cout << "numKeys = " << numKeys << endl;
00055   
00056   // Create A, b
00057   int Arows = 2 * numKeys;
00058   int Acols = 4;
00059   //  double Avals[Arows * Acols];
00060   //  int indexA = 0;
00061   
00062   int brows = 2 * numKeys;
00063   int bcols = 1;
00064   //  double bvals[brows * bcols];
00065   //  int indexb = 0;
00066   
00067   NEWMAT::Matrix m_A(Arows, Acols);
00068   NEWMAT::Matrix m_b(brows, bcols);
00069   
00070   for (int i = 0; i < Arows; i+=2){
00071     m_A(i+1, 1) = static_cast<NEWMAT::Real>( keyPairs[i/2]->getKey2()->modelX );
00072     m_A(i+1, 2) = static_cast<NEWMAT::Real>( -(keyPairs[i/2]->getKey2()->modelY) );
00073     m_A(i+1, 3) = static_cast<NEWMAT::Real>( 1 );
00074     m_A(i+1, 4) = static_cast<NEWMAT::Real>( 0 );
00075     m_A(i+2, 1) = static_cast<NEWMAT::Real>( keyPairs[i/2]->getKey2()->modelY );
00076     m_A(i+2, 2) = static_cast<NEWMAT::Real>( keyPairs[i/2]->getKey2()->modelX );
00077     m_A(i+2, 3) = static_cast<NEWMAT::Real>( 0 );
00078     m_A(i+2, 4) = static_cast<NEWMAT::Real>( 1 );
00079     
00080     m_b(i+1, 1) = static_cast<NEWMAT::Real>( keyPairs[i/2]->getKey1()->imageX );
00081     m_b(i+2, 1) = static_cast<NEWMAT::Real>( keyPairs[i/2]->getKey1()->imageY );
00082   }
00083   
00084   NEWMAT::Matrix m_ATran = m_A.t();
00085   
00086   NEWMAT::Matrix m_ATranA = m_ATran * m_A;
00087   
00088   NEWMAT::Matrix m_x = (m_ATranA.i() * m_ATran) * m_b;
00089   
00090   NEWMAT::Matrix m_Axb = (m_A * m_x) - m_b;
00091   
00092   *error = 0.0;
00093   
00094   for (int i = 1; i <= brows; i++){
00095     *error += (m_Axb(i,1) * m_Axb(i,1));
00096   }
00097   *error *= 2.0;
00098   *error /= (Arows - 4);
00099   *error /= (Arows);
00100   *error = std::sqrt(*error);
00101   
00102   return m_x;
00103 }
00104 
00105 
00106 NEWMAT::Matrix getBestTransform2(std::vector<keypointPair*> allPairs, std::vector<keypointPair*> seedPairs, double *error, std::vector<keypointPair*>** inliersToReturn){
00107   int maxIterations = 3;
00108   
00109   const double maxXYError = 0.25 * MAXIMAGEDIM;
00110   const double maxScaleError = 4.0;
00111   const double maxRotationError = 30.0/180.0 * M_PI;
00112   
00113   std::vector<keypointPair*>* inliers = new std::vector<keypointPair*>;
00114   
00115   // Everything belongs to inliers for now
00116   for (int i = 0; i < (int)seedPairs.size(); i++){
00117     inliers->push_back(seedPairs[i]);
00118   }
00119   
00120   
00121   double xyError = maxXYError;
00122   double scaleError = maxScaleError;
00123   double rotationError = maxRotationError;
00124   for (int i = 0; i < maxIterations; i++){
00125     double oldSolution[4] = {0,0,0,0};
00126     for (int ii = 0; ii < 100; ii++){
00127       //    cout << "\tIteration " << i << " with " << inliers->size() << " inliers" << endl;
00128       NEWMAT::Matrix solution = leastSquareSolution(*inliers, error);
00129       bool hasConverged = true;
00130       for (int j = 0; j < 4; j++){
00131         hasConverged &= (solution(j+1,1) == oldSolution[j]);
00132         oldSolution[j] = solution(j+1,1);
00133       }
00134       if (hasConverged) break;
00135       //    cout << "\tIteration " << i << " with " << inliers->size() << " inliers" << endl;
00136       double scale = sqrt(solution(1,1) * solution(1,1) + solution(2,1) * solution(2,1));
00137       double tempX = (double)(solution(1,1)) / scale;
00138       /// This part is weird.. for some reason, sometimes tempX is outside of range ///
00139       if (tempX < -1.0) tempX = -1.0;
00140       if (tempX >  1.0) tempX =  1.0;
00141       double theta = acos(tempX);
00142       //    cout << "\t\tin range? " << (tempX >= -1.0) << (tempX <= 1.0) << (tempX == 1.0) <<  " " << tempX << endl;
00143       if (solution(2,1) / scale < 0.0) theta = -theta;
00144       delete inliers;
00145       inliers = new std::vector<keypointPair*>;
00146       //    cout << "\t\t(" << solution(1,1) << "," << solution(2,1) << "," << solution(3,1) << "," << solution(4,1) << ")'" << endl;
00147       //    cout << "\t\tscale = " <<  scale << endl;
00148       //    cout.precision(10);
00149       //    cout << "\t\t" << (solution(1,1) / scale) << endl;
00150       //    cout << "\t\ttheta = " <<  theta << endl;
00151       //    cout << "\t\ttx = " <<  solution(3,1) << endl;
00152       //    cout << "\t\tty = " <<  solution(4,1) << endl;
00153       for (int j = 0; j < (int)allPairs.size(); j++){
00154         double xTranslate, yTranslate, zoom, rotation;
00155         allPairs[j]->getBackwardTransform(&xTranslate, &yTranslate, &zoom, &rotation);
00156         
00157         double imageX = allPairs[j]->getKey1()->imageX;
00158         double imageY = allPairs[j]->getKey1()->imageY;
00159         double modelX = allPairs[j]->getKey2()->modelX;
00160         double modelY = allPairs[j]->getKey2()->modelY;
00161         double expectedX = modelX * solution(1,1) - modelY * solution(2,1) + solution(3,1);
00162         double expectedY = modelX * solution(2,1) + modelY * solution(1,1) + solution(4,1);
00163         double xDist = imageX - expectedX;
00164         double yDist = imageY - expectedY;
00165         double xyDist = sqrt(xDist * xDist + yDist * yDist);
00166         
00167         double scaleDist = scale / zoom;
00168         if (scaleDist < 1.0) scaleDist = 1.0 / scaleDist;
00169         //      double scaleDist = scale - zoom;
00170         //      if (scaleDist < 0.0) scaleDist = -scaleDist;
00171         
00172         double rotationDist = theta - rotation;
00173         while (rotationDist > M_PI) rotationDist -= (2 * M_PI);
00174         while (rotationDist <= -M_PI) rotationDist += (2 * M_PI);
00175         
00176         //    cout << "image: (" << imageX << "," << imageY << ") model: (" << modelX << "," << modelY << ") expected: (" << expectedX << "," << expectedY << ")\n";
00177         //    cout << "zoom: " << zoom << " rotation: " << rotation << " rotationError: " << rotationError << " rotationDist: " << rotationDist;
00178         //    cout << "\t" << (xyError       >= xyDist) << (scaleError    >= scaleDist) << (rotationError >= rotationDist) << endl;
00179         
00180         if (xyError       >= xyDist
00181           && scaleError    >= scaleDist
00182           && rotationError >= rotationDist
00183           ){
00184           inliers->push_back(allPairs[j]);
00185           allPairs[j]->getKey1()->isInlier = true;      // NOTE: isInlier works as an indicator only because each keypoint is called by a getBestTransform ONCE
00186         }else{
00187           allPairs[j]->getKey1()->isInlier = false;     // NOTE: isInlier works as an indicator only because each keypoint is called by a getBestTransform ONCE
00188         }
00189       }
00190       
00191       if (inliers->size() < 4) break;
00192       
00193     }
00194     
00195     if (inliers->size() < 4) break;
00196     
00197     //    cout << inliers->size() << " inliers left, previous error: " << *error << "\n";
00198     
00199     xyError = maxXYError / (i+2);
00200     scaleError = maxScaleError / (i+2);
00201     rotationError = maxRotationError / (i+2);
00202     
00203     //    xyError /= 2;
00204     //    scaleError /= 2;
00205     //    rotationError /= 2;
00206   }
00207   
00208   NEWMAT::Matrix finalSolution;
00209   
00210   if (inliers->size() >= 4)
00211     finalSolution = leastSquareSolution(*inliers, error);
00212   else{
00213     // double array[4] = {0.0, 0.0, 0.0, 0.0};
00214     // finalSolution = m_Matrix(4, 1, array);
00215     finalSolution = NEWMAT::Matrix(4, 1);
00216     finalSolution(1,1) = 0;
00217     finalSolution(2,1) = 0;
00218     finalSolution(3,1) = 0;
00219     finalSolution(4,1) = 0;
00220     *error = -1;
00221   }
00222   
00223   //  cout << "inliers->size() = " << inliers->size() << endl;
00224   
00225   *inliersToReturn = inliers;
00226   
00227   for (int i = 0; i < (int)inliers->size(); i++){
00228     (*inliers)[i]->getKey1()->modelMatches.push_back((*inliers)[i]->getKey2());
00229     (*inliers)[i]->getKey1()->modelMatchErrors.push_back(*error);
00230   }
00231   
00232   return finalSolution;
00233   
00234 }
00235 
00236 void getBestHoughTransforms(std::vector<NEWMAT::Matrix>* transforms, std::vector<keypoint*> keys, std::vector<keypoint*> matches, std::vector<double>* error, std::vector<std::vector<keypointPair*>*>* inliersToReturn){
00237   
00238   //  cout << "getBestHoughTransforms\n";
00239   
00240   Hashtable<vector<keypointPair*>, HoughKey, hashHoughKey, HoughKeyEquals> HoughHash;
00241   vector<HoughKey*> allKeys;
00242   vector< vector<keypointPair*>* > allHashPairs;
00243   
00244   vector<keypointPair*> allPairs;
00245   
00246   transforms->clear();
00247   error->clear();
00248   inliersToReturn->clear();
00249   
00250   // Each match must also vote for all matches in the same cluster
00251   for (int i = 0; i < (int)keys.size(); i++){
00252     keypointPair* newpair = new keypointPair(keys[i], matches[i]);
00253     allPairs.push_back(newpair);
00254     
00255     double xTranslate, yTranslate, zoom, rotation;
00256     newpair->getBackwardTransform(&xTranslate, &yTranslate, &zoom, &rotation);
00257     
00258     // Compute best 2 bins for each dimension
00259     double bestX[2], bestY[2], bestZoom[2], bestRotation[2];
00260     
00261     if (zoom  < 1.0){
00262       double tempZoom = 1.0;
00263       while (tempZoom > zoom) tempZoom /= 2.0;
00264       bestZoom[0] = tempZoom;
00265       bestZoom[1] = tempZoom * 2.0;
00266     }else{
00267       double tempZoom = 1.0;
00268       while (tempZoom < zoom) tempZoom *= 2.0;
00269       bestZoom[0] = tempZoom / 2.0;
00270       bestZoom[1] = tempZoom;
00271     }
00272     
00273     double degrees30 = 30.0 / 180.0 * M_PI;
00274     int rotationBin = (int)(rotation / degrees30);
00275     bestRotation[0] = (double)rotationBin * degrees30;
00276     bestRotation[1] = bestRotation[0] + degrees30;
00277     
00278     for (int ii = 0; ii <= 1; ii++){
00279       double maxImageDim = bestZoom[ii] * MAXIMAGEDIM;
00280       double partitionImageDim = maxImageDim / 4.0;
00281       int xBin = (int)(xTranslate / partitionImageDim);
00282       bestX[0] = partitionImageDim * xBin;
00283       bestX[1] = bestX[0] + partitionImageDim;
00284       int yBin = (int)(yTranslate / partitionImageDim);
00285       bestY[0] = partitionImageDim * yBin;
00286       bestY[1] = bestY[0] + partitionImageDim;
00287       for (int jj = 0; jj <= 1; jj++){
00288         for (int xk = 0; xk <= 1; xk++){
00289           for (int yk = 0; yk <= 1; yk++){
00290             HoughKey* hKey = new HoughKey();
00291             allKeys.push_back(hKey);
00292             hKey->M = newpair->getKey2()->G->M;
00293             hKey->x = bestX[xk];
00294             hKey->y = bestY[yk];
00295             hKey->scale = bestZoom[ii];
00296             hKey->orientation = bestRotation[jj];
00297             
00298             vector<keypointPair*>* hashPairs = HoughHash.retrieve(hKey);
00299             if (hashPairs == NULL){
00300               hashPairs = new vector<keypointPair*>();
00301               allHashPairs.push_back(hashPairs);
00302               hashPairs->push_back(newpair);
00303               HoughHash.insert(hashPairs, hKey);
00304             }else{
00305               hashPairs->push_back(newpair);
00306             }
00307             
00308             //            cout << "Adding to hash: ("
00309             //                 << hashPairs->size() << ",\t"
00310             //                 << bestX[xk] << ",\t"
00311             //                 << bestY[yk] << ",\t"
00312             //                 << bestZoom[ii] << ",\t"
00313             //                 << bestRotation[jj] << ")\n";
00314           }
00315         }
00316       }
00317     }
00318   }
00319   
00320   // Iterate through results from hash
00321   vector<vector<keypointPair*>*> hashData;
00322   HoughHash.retrieveAllData(&hashData);
00323   
00324   for (int i = 0; i < (int)hashData.size(); i++){
00325     if (hashData[i]->size() < 3) continue;
00326     double err;
00327     vector<keypointPair*>* in;
00328     //    cout << "# in hash bin: " << hashData[i]->size() << endl << flush;
00329     NEWMAT::Matrix solution = getBestTransform2(allPairs, *hashData[i], &err, &in);
00330     
00331     // Check that a solution is found
00332     size_t numInliers = in->size();
00333     //    cout << numInliers << endl;
00334     if (numInliers >= 4){
00335       // Check for duplicity
00336       bool isDuplicate = false;
00337       for (int j = 0; j < (int)(transforms->size()); j++){
00338         if ((*transforms)[j] == solution){
00339           isDuplicate = true;
00340           break;
00341         }
00342       }
00343       
00344       if (isDuplicate){
00345         delete in;
00346         continue;
00347       }
00348       
00349       // Find location to do insertion (sort)
00350       vector< vector<keypointPair*>* >::iterator location = (*inliersToReturn).begin();
00351       vector< NEWMAT::Matrix >::iterator transformsLocation = (*transforms).begin();
00352       vector< double >::iterator errorLocation = (*error).begin();
00353       for (vector< vector<keypointPair*>* >::iterator j = (*inliersToReturn).begin(); j < (*inliersToReturn).end(); j++){
00354         if (in->size() >= (*j)->size()){
00355           break;
00356         }
00357         location++;
00358         transformsLocation++;
00359         errorLocation++;
00360       }
00361       //      cout << "# inliers " << in->size() << ", error: " << err << endl;
00362       
00363       transforms->insert(transformsLocation, solution);
00364       error->insert(errorLocation, err);
00365       // Make copy of in
00366       vector<keypointPair*>* in2;
00367       in2 = new vector<keypointPair*>();
00368       for (int j = 0; j < (int)in->size(); j++){
00369         keypointPair* newPair = new keypointPair((*in)[j]->getKey1(), (*in)[j]->getKey2());
00370         in2->push_back(newPair);
00371       }
00372       inliersToReturn->insert(location, in2);
00373       delete in;
00374       //      double scale = sqrt(solution(1) * solution(1) + solution(2) * solution(2));
00375       //      double theta = acos(solution(1) / scale);
00376       //      cout << "(" << (*hashData[i])[0]->getKey2()->G->M->O 
00377       //          << "," << (*hashData[i])[0]->getKey2()->G->M->id << ")"
00378       //          << "  " << hashData[i]->size() << " \t"
00379       //          << error << " \t" << scale << " \t" << theta << " \t" << solution(3) << " \t" << solution(4) << endl;
00380     }else{
00381       delete in;
00382     }
00383   }
00384   
00385   //  cout << endl << endl << endl;
00386   
00387   for (int i = 0; i < (int)(*inliersToReturn).size(); i++){
00388     //    cout << "# inliers " << (*inliersToReturn)[i]->size() << ", error: " << (*error)[i] << endl;
00389   }
00390   
00391   for (int i = 0; i < (int)allPairs.size(); i++){
00392     delete allPairs[i];
00393   }
00394   for (int i = 0; i < (int)allHashPairs.size(); i++){
00395     delete allHashPairs[i];
00396   }
00397   for (int i = 0; i < (int)allKeys.size(); i++){
00398     delete allKeys[i];
00399   }
00400 }
00401 
00402 KnowledgeBase::modelMatchingInfo::modelMatchingInfo()
00403 : matchedModel(NULL), solution(NULL), inliers(NULL), 
00404 error(numeric_limits<double>::infinity()),
00405 probOfMatch(numeric_limits<double>::infinity()) {}
00406 
00407 // KnowledgeBase::modelMatchingInfo::modelMatchingInfo(const KnowledgeBase::modelMatchingInfo& other) : //copy constructor for modelMatchingInfo
00408 //   matchedModel(other.matchedModel), solution(other.solution), inliers(other.inliers), error(other.error) {}
00409 
00410 // KnowledgeBase::modelMatchingInfo::operator=(const KnowledgeBase::modelMatchingInfo& other) //Operator overloading for modelMatchingInfo
00411 // {
00412 //   matchedModel = other.matchedModel;
00413 //   solution = other.solution;
00414 //   inliers = other.inliers;
00415 //   error = other.error;
00416 //   return *this;
00417 //     }
00418 
00419 KnowledgeBase::modelMatchingInfo::~modelMatchingInfo(){
00420   if (solution != NULL) delete solution;
00421   if (inliers != NULL){
00422     for (int i = 0; i < (int)inliers->size(); i++)
00423       delete (*inliers)[i];
00424     delete inliers;
00425   }
00426 }
00427 
00428 KnowledgeBase::KnowledgeBase()
00429 : maxNumModels(0), keys(), keygroups(), models(), objects(), myTree(new KDTree(keys)), paramHash() {
00430   setParameter("probOfMatch", 0.9);
00431   setParameter("errorThreshold", 0.05*MAXIMAGEDIM);
00432 }
00433 
00434 KnowledgeBase::~KnowledgeBase(){
00435   cleanUpMemory();
00436 }
00437 
00438 void KnowledgeBase::cleanUpMemory(){
00439   maxNumModels = 0;
00440   
00441   if (myTree){
00442     delete myTree;
00443     myTree = NULL;
00444   }
00445   
00446   for (int i = 0; i < (int)keys.size(); i++){
00447     delete keys[i];
00448   }
00449   keys.clear();
00450   
00451   for (int i = 0; i < (int)keygroups.size(); i++){
00452     delete keygroups[i];
00453   }
00454   keygroups.clear();
00455   
00456   for (int i = 0; i < (int)models.size(); i++){
00457     delete models[i];
00458   }
00459   models.clear();
00460   
00461   for (int i = 0; i < (int)objects.size(); i++){
00462     delete objects[i];
00463   }
00464   objects.clear();
00465   
00466   vector<double*> paramVals;
00467   vector<CharPtrKey*> paramNames;
00468   paramHash.retrieveAllData(&paramVals);
00469   paramHash.retrieveAllKeys(&paramNames);
00470   for (int i = 0; i < (int)paramNames.size(); i++){
00471     CharPtrKey* tempKey;
00472     paramHash.deleteData(paramNames[i], &tempKey);
00473     delete paramNames[i];
00474   }
00475   for (int i = 0; i < (int)paramVals.size(); i++){
00476     free(paramVals[i]);
00477   }
00478 }
00479 
00480 void KnowledgeBase::keypointMatching(vector<keypoint*>* newKeys, vector< vector< vector<keypoint*> > >& matches, vector< vector< vector<keypoint*> > >& imageKey, bool objectSpecified, int wantedObjectID){
00481   for (int i = 0; i < (int)(*newKeys).size(); i++){
00482     //    vector<keypoint*> bestKeypoint;
00483     int maxSearches = 20;
00484     int n = 10;
00485     myTree->getBestNKeypointMatch(*(*newKeys)[i], maxSearches, n);
00486     if ((*newKeys)[i]->bestMatch[0] == NULL){
00487       //      cout << i << "!\n";
00488       continue;
00489     }
00490     double dist0 = ((*newKeys)[i]->bestMatch[0] == NULL) ? numeric_limits<double>::infinity() : sqrt((*newKeys)[i]->sqDist(*((*newKeys)[i]->bestMatch[0])));
00491     double dist1 = numeric_limits<double>::infinity(); //((*newKeys)[i]->bestMatch[1] == NULL) ? numeric_limits<double>::infinity() : sqrt((*newKeys)[i]->sqDist(*((*newKeys)[i]->bestMatch[1])));
00492     // Find next nearest feature from non-neighbor group
00493     for (int j = 1; j < n; j++){
00494       if ((*newKeys)[i]->bestMatch[j] == NULL) break;
00495       if (!((*newKeys)[i]->bestMatch[0]->G->isNeighbor((*newKeys)[i]->bestMatch[j]->G))){
00496         //        cout << "Found at bestMatch[" << j << "]\n";
00497         dist1 = sqrt((*newKeys)[i]->bestDist[j]);
00498         break;
00499       }
00500     }
00501     //  cout << i << endl;
00502     if (dist0 <= 0.85 * dist1){
00503       int objectID = (*newKeys)[i]->bestMatch[0]->G->M->O->getID();
00504       if (objectSpecified && objectID != wantedObjectID) continue;
00505       int modelID  = (*newKeys)[i]->bestMatch[0]->G->M->getID();
00506       //      cout << (*newKeys)[i]->bestMatch[0] << " " << objectID << " " << modelID << endl;
00507       //      cout << "(" << (*newKeys)[i]->x <<               "," << (*newKeys)[i]->y << ") - "
00508       //           << "(" << (*newKeys)[i]->bestMatch[0]->x << "," << (*newKeys)[i]->bestMatch[0]->y << ")"
00509       //           << "(" << (*newKeys)[i]->x - (*newKeys)[i]->bestMatch[0]->x << "," << (*newKeys)[i]->y - (*newKeys)[i]->bestMatch[0]->y << ")\n";
00510       matches[objectID][modelID].push_back((*newKeys)[i]->bestMatch[0]);
00511       imageKey[objectID][modelID].push_back((*newKeys)[i]);
00512       // Also add best points in neighbor groups
00513       for (int j = 0; j < (int)((*newKeys)[i]->bestMatch[0]->G->neighbors.size()); j++){
00514         double e;
00515         keypoint* bestMatchInGroup = (*newKeys)[i]->bestMatch[0]->G->neighbors[j]->bestMatchInGroup((*newKeys)[i], &e);
00516         // Make assumption that a group contains at least one keypoint
00517         // Make assumption that groups' neighbors must be from same object
00518         modelID = bestMatchInGroup->G->M->getID();
00519         matches[objectID][modelID].push_back(bestMatchInGroup);
00520         imageKey[objectID][modelID].push_back((*newKeys)[i]);
00521       }
00522     }
00523   }
00524   
00525   //  cout << "Finished matching keypoints\n";
00526 }
00527 
00528 void KnowledgeBase::modelMatching(size_t numNewKeys, vector< vector< vector<keypoint*> > >& matches, vector< vector< vector<keypoint*> > >& imageKey, vector<modelMatchingInfo*>& mminfo/*, vector<model*>& acceptableModels, vector<double>& modelConfidence, vector<double>& modelError, vector<int>& solutionIndex, vector< vector< vector<NEWMAT::Matrix*> > >& solutions, vector< vector< vector< vector<keypointPair*>* > > >& inliers*/){
00529   //    cout << "checkpoint1 " << objects.size() << " " << models.size() << endl;
00530   
00531   double probOfMatch = getParameter("probOfMatch");
00532   double threshold = getParameter("errorThreshold");
00533   
00534   for (int i = 0; i < (int)objects.size(); i++){
00535     int objectID = objects[i]->getID();
00536     //    cout << objectID << endl;
00537     for (int j = 0; j < (int)objects[i]->models.size(); j++){
00538       int modelID = objects[i]->models[j]->getID();
00539       //      cout << "\t" << modelID << endl;
00540       if (imageKey[objectID][modelID].size() > 4){
00541         double error;
00542         //        cout << "(" << objectID << "," << modelID << ") [" << imageKey[objectID][modelID].size() <<  "]\t";
00543         
00544         vector<vector<keypointPair*>*> in;
00545         vector<double> errors;
00546         vector<NEWMAT::Matrix> transforms;
00547         getBestHoughTransforms(&transforms, imageKey[objectID][modelID], matches[objectID][modelID], &errors, &in);
00548         //        cout << "#transforms=" << transforms.size() << "\n";
00549         if (transforms.size() == 0){
00550           continue;
00551         }
00552         /*
00553          int bestTransformIndex = -1;
00554          int maxNumInliers = 0;
00555          for (int k = 0; k < (int)transforms.size(); k++){
00556          //           cout << (int)(in[k])->size() << endl;
00557          if (maxNumInliers < (int)(in[k])->size()){
00558          bestTransformIndex = k;
00559          maxNumInliers = (int)(in[k])->size();
00560          }
00561          }
00562          cout << maxNumInliers << endl;
00563          solutions[objectID][modelID] = new NEWMAT::Matrix(transforms[bestTransformIndex]);
00564          inliers[objectID][modelID] = in[bestTransformIndex];
00565          error = errors[bestTransformIndex];
00566          for (int k = 0; k < bestTransformIndex; k++){
00567          for (int kk = 0; kk < (int)(in[k]->size()); kk++)
00568          delete (*(in[k]))[kk];
00569          delete in[k];
00570          }
00571          for (int k = bestTransformIndex+1; k < (int)transforms.size(); k++){
00572          for (int kk = 0; kk < (int)(in[k]->size()); kk++)
00573          delete (*(in[k]))[kk];
00574          delete in[k];
00575          }
00576          int numInliers = (inliers[objectID][modelID])->size();
00577          //         if (numInliers < 4){
00578          //           cout << endl;
00579          //           continue;
00580          //         }
00581          //         double scale = sqrt((*(solutions[objectID][modelID]))(1,1) * (*(solutions[objectID][modelID]))(1,1) + (*(solutions[objectID][modelID]))(2,1) * (*(solutions[objectID][modelID]))(2,1));
00582          //         double theta = acos((*(solutions[objectID][modelID]))(1,1) / scale);
00583          */
00584         
00585         for (int t = 0; t < (int)transforms.size(); t++){
00586           //          solutions[objectID][modelID].push_back(new NEWMAT::Matrix(transforms[t]));
00587           //          inliers[objectID][modelID].push_back(in[t]);
00588           
00589           //          double scale = sqrt((*(solutions[objectID][modelID][t]))(1,1) * (*(solutions[objectID][modelID][t]))(1,1) + (*(solutions[objectID][modelID][t]))(2,1) * (*(solutions[objectID][modelID][t]))(2,1));
00590           //          double theta = acos((*(solutions[objectID][modelID][t]))(1,1) / scale);
00591           error = errors[t];
00592           
00593           size_t numInliers = in[t]->size();
00594           
00595           size_t numKeypointsInModel = 0;
00596           for (int jj = 0; jj < (int)objects[i]->models[j]->keygroups.size(); jj++){
00597             numKeypointsInModel += objects[i]->models[j]->keygroups[jj]->keypts.size();
00598           }
00599           
00600           double d = (double)numKeypointsInModel / (double)(keys.size());
00601           double l = 0.125 * 0.125;
00602           double p = d*l;
00603           double q = (1.0-p);
00604           
00605           double PM = 0.01;
00606           double nCjj = 1.0; // n C n = 1.0
00607           double Pf_notM = (numInliers < numNewKeys) ? nCjj * std::pow(p, (int)numNewKeys) : 0.0;
00608           cout.precision(6);
00609           for (int jj = (int)numNewKeys - 1; jj >= (int)numInliers; jj--){
00610             //          nCjj = nCjj  * (double)(jj+1) / (double)((*newKeys).size() - jj);
00611             nCjj = choose((int)numNewKeys, jj);
00612             if (nCjj == numeric_limits<double>::infinity()) continue; 
00613             Pf_notM += nCjj * std::pow(p, jj) * std::pow(q, (int)numNewKeys - jj);//prob;
00614             //          cout << Pf_notM << " " << nCjj << " " << pow(p, jj) << " " << pow(q, (*newKeys).size() - jj) << "\n";
00615           }
00616           
00617           double PM_f = PM / (PM + Pf_notM);
00618           
00619           //        cout << PM_f << " " << scientific << PM << " " << Pf_notM << " " << p << fixed << "\t"
00620           //            << numInliers << "/" << numKeypointsInModel << "=" << (double)numInliers / (double)numKeypointsInModel
00621           //            << "\t" << error << " \t" << scale << " \t" << theta << " \t" << (*(solutions[objectID][modelID]))(3,1) << " \t" << (*(solutions[objectID][modelID]))(4,1) << endl;
00622           
00623           if (PM_f >= probOfMatch && numInliers >= 4 && error <= threshold){
00624             //            acceptableModels.push_back(objects[i]->models[j]);
00625             //            modelConfidence.push_back(PM_f);
00626             //            modelError.push_back(error);
00627             //            solutionIndex.push_back(solutions[objectID][modelID].size()-1);
00628             //            cout << "[qq] " << numInliers << "/" << numKeypointsInModel << ", error: " << error << 
00629             //                ", scale: " << scale << ", theta: " << theta << 
00630             //                endl;
00631             modelMatchingInfo *newmminfo = new modelMatchingInfo();
00632             newmminfo->matchedModel = objects[i]->models[j];
00633             newmminfo->solution = new NEWMAT::Matrix(transforms[t]);
00634             newmminfo->inliers = in[t];
00635             newmminfo->error = error;
00636             newmminfo->probOfMatch = PM_f;
00637             mminfo.push_back(newmminfo);
00638           }else{
00639             // Clean up in[t]
00640             for (int tt = 0; tt < (int)in[t]->size(); tt++){
00641               delete (*(in[t]))[tt];
00642             }
00643             delete in[t];
00644           }
00645         }
00646         //        if (mminfo.size() == 0) cout << endl;
00647         //      }else{
00648         //        solutions[objectID][modelID] = NULL;
00649       }
00650     }
00651   }
00652   
00653   //  cout << "checkpoint2\n";
00654   
00655   // Sort models by lowest error
00656   // Bubble sort
00657   // NOTE: This is really unncessary if we only want top 3 models! Simply do insertion sort for 3 elements
00658   bool isDone = false;
00659   for (int i = (int)mminfo.size()-1; !isDone && i > 0; i--){
00660     isDone = true;
00661     for (int j = 0; j < i; j++){
00662       if (mminfo[j]->error > mminfo[j+1]->error){
00663         //        model* tempModel = acceptableModels[j];
00664         //        acceptableModels[j] = acceptableModels[j+1];
00665         //        acceptableModels[j+1] = tempModel;
00666         
00667         //        double tempDouble = modelConfidence[j];
00668         //        modelConfidence[j] = modelConfidence[j+1];
00669         //        modelConfidence[j+1] = tempDouble;
00670         
00671         //        tempDouble = modelError[j];
00672         //        modelError[j] = modelError[j+1];
00673         //        modelError[j+1] = tempDouble;
00674         
00675         //        int tempInt = solutionIndex[j];
00676         //        solutionIndex[j] = solutionIndex[j+1];
00677         //        solutionIndex[j+1] = tempInt;
00678         
00679         modelMatchingInfo* tempmminfo = mminfo[j];
00680         mminfo[j] = mminfo[j+1];
00681         mminfo[j+1] = tempmminfo;
00682         
00683         isDone = false;
00684         
00685       }
00686     }
00687   }
00688   //  cout << "Model matching complete\n";
00689 }
00690 
00691 void KnowledgeBase::rebuildKDTree(){
00692   if (myTree) delete myTree;
00693   cout << "Rebuilding with " << keys.size() << " keys..\n";
00694   myTree = new KDTree(keys);
00695   cout << "Rebuilding complete\n";
00696 }
00697 object* KnowledgeBase::unlearn_Object(vector<keypoint*>* newKeys, int oID){
00698   object *O = new object((int)objects.size());
00699   model  *M = new model(0);
00700   //  M->filename.clear();
00701   //  M->filename.append(filename);
00702   M->O = O;
00703   O->models.push_back(M);
00704   if (maxNumModels < O->models.size()) maxNumModels = O->models.size();
00705   //models.push_back(M);
00706   //objects.push_back(O);
00707   for(int a = 0; objects.size(); a++) {
00708     if(objects.at(a)->id == oID) objects.erase(objects.begin()+a);
00709   }
00710   for (int i = 0; i < (int)(*newKeys).size(); i++){
00711     
00712     keygroup *G = new keygroup();
00713     G->M = M;
00714     M->keygroups.push_back(G);
00715     keygroups.push_back(G);
00716     
00717     (*newKeys)[i]->G = G;
00718     (*newKeys)[i]->generation = M->generation;
00719     /*
00720      int spot = 0;    
00721      for(int j = 0; j > (int)(G->keypts.size()); j++) {
00722      if(G->keypts.at(j) == (*newKeys)[i]) spot = j;
00723      }
00724      G->keypts.erase(G->keypts.begin()+spot-1, G->keypts.begin()+spot);
00725      */   
00726     /*for(int j = 0; j < (int)(keygroups.size()); j++) {
00727      //if(keygroups.at(j) == G) spot = j;
00728      //found = matchingGroup->keypts.at(j) == (*newKeys)[i];
00729      if((*keygroups.at(j)).compareTo(G) == 0) {
00730      keygroups.erase(keygroups.begin()+j);
00731      //cout << "Keygroups erased... \n";
00732      }    
00733      }*/
00734     for(int j = 0; j > (int)(keys.size()); j++) {
00735       if(keys.at(j) == (*newKeys)[i]) keys.erase(keys.begin()+j);
00736     }   
00737     
00738     //    ((*newKeys)[i])->id = getNewKeypointID();
00739   }
00740   
00741   M->generation--;
00742   rebuildKDTree();
00743   return O;
00744 }
00745 object* KnowledgeBase::learn_newObject(vector<keypoint*>* newKeys){
00746   object *O = new object((int)objects.size());
00747   objects.push_back(O);
00748   model  *M = new model(0);
00749   //  M->filename.clear();
00750   //  M->filename.append(filename);
00751   M->O = O;
00752   O->models.push_back(M);
00753   if (maxNumModels < O->models.size()) maxNumModels = O->models.size();
00754   models.push_back(M);
00755   
00756   for (int i = 0; i < (int)(*newKeys).size(); i++){
00757     
00758     keygroup *G = new keygroup();
00759     G->M = M;
00760     M->keygroups.push_back(G);
00761     keygroups.push_back(G);
00762     
00763     (*newKeys)[i]->G = G;
00764     (*newKeys)[i]->generation = M->generation;
00765     G->keypts.push_back((*newKeys)[i]);
00766     keys.push_back((*newKeys)[i]);
00767     //    ((*newKeys)[i])->id = getNewKeypointID();
00768   }
00769   
00770   M->generation++;
00771   
00772   rebuildKDTree();
00773   
00774   return O;
00775 }
00776 model* KnowledgeBase::unlearn_Model( vector<keypoint*>* newKeys, object* O, vector<modelMatchingInfo*> mminfo, int mID){
00777   model* M = new model((int)O->models.size());
00778   M->O = O;
00779   //  M->filename.clear();
00780   //  M->filename.append(filename);
00781   //O->models.push_back(M);
00782   for(int j = 0; j < (int)(models.size()); j++) {
00783     cout << "checking..." << models.at(j)->name << " " << mID;
00784     if(models.at(j)->id == mID) {
00785       models.erase(models.begin()+j);
00786       cout << "Found and removed from models.";
00787     }
00788   }
00789   for(int j = 0; j < (int)(O->models.size()); j++) {
00790     cout << "checking..." << O->models.at(j)->name << " " << mID;
00791     if(O->models.at(j)->id == mID) {
00792       O->models.erase(O->models.begin()+j);
00793       cout << "Found and removed from O's models.";
00794     }
00795   } 
00796   if (maxNumModels < O->models.size()) maxNumModels = O->models.size();
00797   //models.push_back(M);
00798   for (int i = 0; i < (int)(*newKeys).size(); i++){
00799     // Create keygroup for each keypoint
00800     keygroup *G = new keygroup();
00801     //keygroups.push_back(G);
00802     
00803     G->M = M;
00804     M->keygroups.push_back(G);
00805     
00806     G->keypts.push_back((*newKeys)[i]);
00807     (*newKeys)[i]->G = G;
00808     (*newKeys)[i]->generation = M->generation;
00809     // int spot = 0;
00810     // bool found = false;
00811     
00812     // Link to best matches
00813     int groupCount = 0;
00814     for (int j = 0; j < (int)(*newKeys)[i]->modelMatches.size() && groupCount < 3; j++){
00815       keygroup *grp = (*newKeys)[i]->modelMatches[j]->G;
00816       model *modelMatch = grp->M;
00817       if ((mminfo.size() > 0 && modelMatch == mminfo[0]->matchedModel) ||
00818         (mminfo.size() > 1 && modelMatch == mminfo[1]->matchedModel) ||
00819         (mminfo.size() > 2 && modelMatch == mminfo[2]->matchedModel))
00820       {
00821         grp->neighbors.push_back(G);
00822         G->neighbors.push_back(grp);
00823         groupCount++;
00824       }
00825     }
00826     for(int j = 0; j < (int)(keygroups.size()); j++) {
00827       //if(keygroups.at(j) == G) spot = j;
00828       //found = matchingGroup->keypts.at(j) == (*newKeys)[i];
00829       if((*keygroups.at(j)).compareTo(G) == 0) {
00830         keygroups.erase(keygroups.begin()+j);
00831         //cout << "Keygroups erased... \n";
00832       }   
00833     }
00834     //keys.push_back((*newKeys)[i]);
00835     //    ((*newKeys)[i])->id = getNewKeypointID();
00836   }
00837   
00838   M->generation++;
00839   
00840   rebuildKDTree();
00841   
00842   return M;
00843 }
00844 model* KnowledgeBase::learn_newModel( vector<keypoint*>* newKeys, object* O, vector<modelMatchingInfo*> mminfo){
00845   model* M = new model((int)O->models.size());
00846   M->O = O;
00847   //  M->filename.clear();
00848   //  M->filename.append(filename);
00849   O->models.push_back(M);
00850   if (maxNumModels < O->models.size()) maxNumModels = O->models.size();
00851   models.push_back(M);
00852   for (int i = 0; i < (int)(*newKeys).size(); i++){
00853     // Create keygroup for each keypoint
00854     keygroup *G = new keygroup();
00855     keygroups.push_back(G);
00856     
00857     G->M = M;
00858     M->keygroups.push_back(G);
00859     
00860     G->keypts.push_back((*newKeys)[i]);
00861     (*newKeys)[i]->G = G;
00862     (*newKeys)[i]->generation = M->generation;
00863     
00864     // Link to best matches
00865     int groupCount = 0;
00866     for (int j = 0; j < (int)(*newKeys)[i]->modelMatches.size() && groupCount < 3; j++){
00867       keygroup *grp = (*newKeys)[i]->modelMatches[j]->G;
00868       model *modelMatch = grp->M;
00869       if ((mminfo.size() > 0 && modelMatch == mminfo[0]->matchedModel) ||
00870         (mminfo.size() > 1 && modelMatch == mminfo[1]->matchedModel) ||
00871         (mminfo.size() > 2 && modelMatch == mminfo[2]->matchedModel))
00872       {
00873         grp->neighbors.push_back(G);
00874         G->neighbors.push_back(grp);
00875         groupCount++;
00876       }
00877     }
00878     
00879     keys.push_back((*newKeys)[i]);
00880     //    ((*newKeys)[i])->id = getNewKeypointID();
00881   }
00882   
00883   M->generation++;
00884   
00885   rebuildKDTree();
00886   
00887   return M;
00888 }
00889 void KnowledgeBase::unlearn_fromModel(vector<keypoint*>* newKeys, model* bestModel, NEWMAT::Matrix* transform, double& s, double& theta, double& tx, double& ty){
00890   cout << "Locating data in database... \n";
00891   s = sqrt((*transform)(1,1) * (*transform)(1,1) + (*transform)(2,1) * (*transform)(2,1));
00892   double temp = (*transform)(1,1) / s;
00893   if (temp > 1.0) temp = 1.0;
00894   if (temp < -1.0) temp = -1.0;
00895   theta = acos(temp);
00896   double m  = (*transform)(1,1);
00897   double n  = (*transform)(2,1);
00898   tx = (*transform)(3,1);
00899   ty = (*transform)(4,1);
00900   
00901   if (isnan(theta)) theta = 0.0;
00902   if ((*transform)(2,1) / s < 0.0) theta = -theta;
00903   
00904   for (int i = 0; i < (int)(*newKeys).size(); i++){
00905     int spot = 0;
00906     for(int j = 0; j > (int)(keys.size()); j++) {
00907       if(keys.at(j) == (*newKeys)[i]) spot = j;
00908     }   
00909     keys.erase(keys.begin()+spot-1, keys.begin()+spot);   
00910     
00911     double u = (*newKeys)[i]->imageX;
00912     double v = (*newKeys)[i]->imageY;
00913     double newy = (m*(v-ty)-n*(u-tx)) / (m*m + n*n);
00914     double newx = (m*(u-tx)+n*(v-ty)) / (m*m + n*n);
00915     double newscale = (*newKeys)[i]->imageScale / s;
00916     double neworientation = (*newKeys)[i]->imageOrientation - theta;
00917     while (neworientation > M_PI)   neworientation -= (2 * M_PI);
00918     while (neworientation <= -M_PI) neworientation += (2 * M_PI);
00919     
00920     //        cout << "(" << u << "," << v << "," << (*newKeys)[i]->orientation << "," << (*newKeys)[i]->scale << ") --> ("
00921     //            << "(" << newx << "," << newy << "," << neworientation << "," << newscale << ")\n";
00922     
00923     (*newKeys)[i]->modelX           = newx;
00924     (*newKeys)[i]->modelY           = newy;
00925     (*newKeys)[i]->modelScale       = newscale;
00926     (*newKeys)[i]->modelOrientation = neworientation;
00927     (*newKeys)[i]->generation       = bestModel->generation;
00928     
00929     bool hasMatchingGroup = false;
00930     keygroup* matchingGroup = NULL;
00931     keypoint* modelMatch = NULL;
00932     modelMatch = (keypoint*)matchingGroup; // hack to suppress compiler warning due to commented out code below
00933     
00934     if ((*newKeys)[i]->bestMatch[0] != NULL){
00935       for (int j = 0; j < (int)((*newKeys)[i]->modelMatches.size()); j++){
00936         if ((*newKeys)[i]->modelMatches[j]->G->M == bestModel){
00937           hasMatchingGroup = true;
00938           modelMatch = (*newKeys)[i]->modelMatches[j];
00939           (*newKeys)[i]->modelMatches[j] = NULL;
00940           matchingGroup = modelMatch->G;
00941           break;
00942         }
00943       }
00944     }
00945      if (hasMatchingGroup){
00946     /*          
00947      // For now, add indiscriminately
00948      // According to Lowe's paper, should only link if keypoint adds information
00949      cout << "Found matching group! \n";
00950      spot = 0;
00951      bool found = false;
00952      for(int j = 0; j < (int)(matchingGroup->keypts.size()); j++) {
00953      if(matchingGroup->keypts.at(j) == (*newKeys)[i]) spot = j;
00954      found = matchingGroup->keypts.at(j) == (*newKeys)[i];
00955      cout << "CompareTo says:" << matchingGroup->compareTo(newKeys) << "\n";
00956      }
00957      cout << "Found matching keypts, at " << found << " " << spot << " of " << (int) matchingGroup->keypts.size() << " \n";   
00958      matchingGroup->keypts.erase(keys.begin()+spot);
00959      cout << "Erased all the stuff? \n";      
00960      //(*newKeys)[i]->G = matchingGroup;
00961      }else{
00962      // Create new cluster
00963      cout << "Not Found? \n";
00964      keygroup *G = new keygroup();
00965      keygroups.push_back(G);
00966      
00967      G->M = bestModel;
00968      spot = 0;
00969      for(int j = 0; j > (int)(bestModel->keygroups.size()); j++) {
00970      if(bestModel->keygroups.at(j) == G) spot = j;
00971      }    
00972      bestModel->keygroups.erase((bestModel->keygroups).begin()+spot-1, (bestModel->keygroups).begin()+spot);
00973      
00974      G->keypts.push_back((*newKeys)[i]);
00975      (*newKeys)[i]->G = G;
00976      
00977      */ 
00978      }
00979   }
00980   cout << "Done \n";
00981   bestModel->generation--;
00982   
00983   rebuildKDTree();
00984   
00985   
00986 }
00987 void KnowledgeBase::learn_toModel(vector<keypoint*>* newKeys, model* bestModel, NEWMAT::Matrix* transform, double& s, double& theta, double& tx, double& ty){
00988   s = sqrt((*transform)(1,1) * (*transform)(1,1) + (*transform)(2,1) * (*transform)(2,1));
00989   double temp = (*transform)(1,1) / s;
00990   if (temp > 1.0) temp = 1.0;
00991   if (temp < -1.0) temp = -1.0;
00992   theta = acos(temp);
00993   double m  = (*transform)(1,1);
00994   double n  = (*transform)(2,1);
00995   tx = (*transform)(3,1);
00996   ty = (*transform)(4,1);
00997   
00998   if (isnan(theta)) theta = 0.0;
00999   if ((*transform)(2,1) / s < 0.0) theta = -theta;
01000   
01001   for (int i = 0; i < (int)(*newKeys).size(); i++){
01002     keys.push_back((*newKeys)[i]);
01003     
01004     double u = (*newKeys)[i]->imageX;
01005     double v = (*newKeys)[i]->imageY;
01006     double newy = (m*(v-ty)-n*(u-tx)) / (m*m + n*n);
01007     double newx = (m*(u-tx)+n*(v-ty)) / (m*m + n*n);
01008     double newscale = (*newKeys)[i]->imageScale / s;
01009     double neworientation = (*newKeys)[i]->imageOrientation - theta;
01010     while (neworientation > M_PI)   neworientation -= (2 * M_PI);
01011     while (neworientation <= -M_PI) neworientation += (2 * M_PI);
01012     
01013     //        cout << "(" << u << "," << v << "," << (*newKeys)[i]->orientation << "," << (*newKeys)[i]->scale << ") --> ("
01014     //            << "(" << newx << "," << newy << "," << neworientation << "," << newscale << ")\n";
01015     
01016     (*newKeys)[i]->modelX           = newx;
01017     (*newKeys)[i]->modelY           = newy;
01018     (*newKeys)[i]->modelScale       = newscale;
01019     (*newKeys)[i]->modelOrientation = neworientation;
01020     (*newKeys)[i]->generation       = bestModel->generation;
01021     
01022     bool hasMatchingGroup = false;
01023     keygroup* matchingGroup = NULL;
01024     keypoint* modelMatch = NULL;
01025     
01026     if ((*newKeys)[i]->bestMatch[0] != NULL){
01027       for (int j = 0; j < (int)((*newKeys)[i]->modelMatches.size()); j++){
01028         if ((*newKeys)[i]->modelMatches[j]->G->M == bestModel){
01029           hasMatchingGroup = true;
01030           modelMatch = (*newKeys)[i]->modelMatches[j];
01031           matchingGroup = modelMatch->G;
01032           break;
01033         }
01034       }
01035     }
01036     
01037     if (hasMatchingGroup){
01038       // For now, add indiscriminately
01039       // According to Lowe's paper, should only link if keypoint adds information
01040       matchingGroup->keypts.push_back((*newKeys)[i]);
01041       (*newKeys)[i]->G = matchingGroup;
01042     }else{
01043       // Create new cluster
01044       keygroup *G = new keygroup();
01045       keygroups.push_back(G);
01046       
01047       G->M = bestModel;
01048       bestModel->keygroups.push_back(G);
01049       
01050       G->keypts.push_back((*newKeys)[i]);
01051       (*newKeys)[i]->G = G;
01052       
01053     }
01054   }
01055   
01056   bestModel->generation++;
01057   
01058   rebuildKDTree();
01059   
01060 }
01061 
01062 void KnowledgeBase::learn_toModel(vector<keypoint*>* newKeys, model* bestModel, NEWMAT::Matrix* transform){
01063   double s, theta, tx, ty;
01064   learn_toModel(newKeys, bestModel, transform, s, theta, tx, ty);
01065 }
01066 void KnowledgeBase::unlearn_fromModel(vector<keypoint*>* newKeys, model* bestModel, NEWMAT::Matrix* transform){
01067   double s, theta, tx, ty;
01068   unlearn_fromModel(newKeys, bestModel, transform, s, theta, tx, ty);
01069 }
01070 
01071 
01072 void KnowledgeBase::learn(vector<keypoint*>& K, matchInfo& mInfo, bool toIntegrate){
01073   
01074   /// Read keypoints
01075   vector<keypoint*>* newKeys;
01076   
01077   newKeys = &K;
01078   // Assign IDs
01079   //  for (int i = 0; i < (int)K.size(); i++){
01080   //    (*newKeys)[i]->id = getNewKeypointID();
01081   //  }
01082   
01083   /// Match keypoints
01084     size_t objectsSize = objects.size();
01085     size_t maxNumModels1 = maxNumModels+1;
01086     //  vector<keypoint*> matches[objectsSize][maxNumModels1];
01087     vector< vector< vector<keypoint*> > > matches;
01088     matches.resize(objectsSize);
01089     for (size_t i = 0; i < objectsSize; i++){
01090     matches[i].resize(maxNumModels1);
01091     }
01092     //  vector<keypoint*> imageKey[objectsSize][maxNumModels1];
01093     vector< vector< vector<keypoint*> > > imageKey;
01094     imageKey.resize(objectsSize);
01095     for (size_t i = 0; i < objectsSize; i++){
01096     imageKey[i].resize(maxNumModels1);
01097     }
01098     keypointMatching(newKeys, matches, imageKey, false, -1);
01099   
01100     /// Match model
01101   vector<modelMatchingInfo*> mminfo;
01102   //  vector<model*> acceptableModels;
01103   //  vector<double> modelConfidence;
01104   //  vector<double> modelError;
01105   //  vector<int> solutionIndex;
01106   //  NEWMAT::Matrix* solutions[objects.size()][maxNumModels+1];
01107   //  vector<keypointPair*>* inliers[objects.size()][maxNumModels+1];
01108   //  for (int i = 0; i < (int)objects.size(); i++){
01109   //    for (int j = 0; j < maxNumModels+1; j++){
01110   //      solutions[i][j] = NULL;
01111   //      inliers[i][j] = NULL;
01112   //    }
01113   //  }
01114   //  vector< vector< vector<NEWMAT::Matrix*> > > solutions;
01115   //  vector< vector< vector< vector<keypointPair*>* > > > inliers;
01116   //  solutions.resize(objectsSize);
01117   //  inliers.resize(objectsSize);
01118   //  for (int i = 0; i < (int)objects.size(); i++){
01119   //    solutions[i].resize(maxNumModels1);
01120   //    inliers[i].resize(maxNumModels1);
01121   //  }
01122   
01123   modelMatching((*newKeys).size(), matches, imageKey, mminfo/*, acceptableModels, modelConfidence, modelError, solutionIndex, solutions, inliers*/);
01124   
01125   //  int oldNumOfObjects = (int)objects.size();
01126   //  int oldMaxNumModels = maxNumModels;
01127   
01128   /// Integrate
01129   model* bestModel;
01130   double bestError;
01131   if (mminfo.size() > 0){
01132     bestModel = mminfo[0]->matchedModel;
01133     bestError = mminfo[0]->error;
01134   }else{
01135     bestModel = NULL;
01136     bestError = numeric_limits<double>::infinity();
01137   }
01138   
01139   double threshold = getParameter("errorThreshold"); // 0.001 * MAXIMAGEDIM;
01140   if (bestModel == NULL){
01141     // No matching objects!
01142     // Create new object
01143     //    cout << "No matches; creating new object\n";
01144     //    cout << "[qq] 0/" << endl;
01145     if (toIntegrate){
01146       object *O = learn_newObject(newKeys);
01147       model* M = O->models[0];
01148       
01149       mInfo.M = M;
01150       mInfo.matchedM = NULL;
01151       mInfo.O = O;
01152       mInfo.matchedO = NULL;
01153       mInfo.s = mInfo.theta = mInfo.tx = mInfo.ty = 0;
01154     }else{
01155       mInfo.M = mInfo.matchedM = NULL;
01156       mInfo.O = mInfo.matchedO = NULL;
01157       mInfo.s = mInfo.theta = mInfo.tx = mInfo.ty = 0;
01158     }
01159     for (int i = 0; i < (int)(*newKeys).size(); i++){
01160       // Set values for mInfo
01161       mInfo.initialVals.push_back((*newKeys)[i]->getID());
01162       mInfo.initialVals.push_back((*newKeys)[i]->imageX);
01163       mInfo.initialVals.push_back((*newKeys)[i]->imageY);
01164       mInfo.initialVals.push_back((*newKeys)[i]->imageScale);
01165       mInfo.initialVals.push_back((*newKeys)[i]->imageOrientation);
01166       mInfo.newVals.push_back((*newKeys)[i]->getID());
01167       mInfo.newVals.push_back((*newKeys)[i]->modelX);
01168       mInfo.newVals.push_back((*newKeys)[i]->modelY);
01169       mInfo.newVals.push_back((*newKeys)[i]->modelScale);
01170       mInfo.newVals.push_back((*newKeys)[i]->modelOrientation);
01171       mInfo.hasMatch.push_back(false);
01172       mInfo.matchedVals.push_back(-1);
01173       mInfo.matchedVals.push_back(-1);
01174       mInfo.matchedVals.push_back(-1);
01175       mInfo.matchedVals.push_back(-1);
01176       mInfo.matchedVals.push_back(-1);
01177     }
01178   }else{
01179     
01180     // Copy relevant information to mInfo
01181     mInfo.O = mInfo.matchedO = bestModel->O;
01182     mInfo.matchedM = bestModel;
01183     
01184     if (bestError > threshold){
01185       // Over threshold; create new model
01186       //      cout << "Matched to object " << bestModel->O->id << ", but not to model" << endl;
01187       if (toIntegrate){
01188         mInfo.M = learn_newModel(newKeys, bestModel->O, mminfo);
01189       }else{
01190         mInfo.M = NULL;
01191       }
01192       mInfo.s = mInfo.theta = mInfo.tx = mInfo.ty = 0;
01193       for (int i = 0; i < (int)(*newKeys).size(); i++){
01194         // Set values for mInfo
01195         mInfo.initialVals.push_back((*newKeys)[i]->getID());
01196         mInfo.initialVals.push_back((*newKeys)[i]->imageX);
01197         mInfo.initialVals.push_back((*newKeys)[i]->imageY);
01198         mInfo.initialVals.push_back((*newKeys)[i]->imageScale);
01199         mInfo.initialVals.push_back((*newKeys)[i]->imageOrientation);
01200         mInfo.newVals.push_back((*newKeys)[i]->getID());
01201         mInfo.newVals.push_back((*newKeys)[i]->modelX);
01202         mInfo.newVals.push_back((*newKeys)[i]->modelY);
01203         mInfo.newVals.push_back((*newKeys)[i]->modelScale);
01204         mInfo.newVals.push_back((*newKeys)[i]->modelOrientation);
01205         mInfo.hasMatch.push_back(false);
01206         mInfo.matchedVals.push_back(-1);
01207         mInfo.matchedVals.push_back(-1);
01208         mInfo.matchedVals.push_back(-1);
01209         mInfo.matchedVals.push_back(-1);
01210         mInfo.matchedVals.push_back(-1);
01211       }
01212     }else{
01213       // Add to old model view
01214       
01215       if (!toIntegrate){
01216         //      cout << "Matched to object " << bestModel->O->id << ", model " << bestModel->id << " (" << bestModel->filename << ") (error=" << bestError << "<=" << threshold << "=threshold)" << endl;
01217         mInfo.M = bestModel;
01218         
01219         //    for (int i = 0; i < (int)(*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]])).size(); i++){
01220         //      (*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]]))[i]->getKey1()->finalMatch = (*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]]))[i]->getKey2();
01221         //    }
01222         for (int i = 0; i < (int)mminfo[0]->inliers->size(); i++){
01223           (*(mminfo[0]->inliers))[i]->getKey1()->finalMatch = (*(mminfo[0]->inliers))[i]->getKey2();
01224         }
01225         
01226         //        model *oldModel = bestModel;
01227         NEWMAT::Matrix* transform = mminfo[0]->solution;
01228         double s = sqrt((*transform)(1,1) * (*transform)(1,1) + (*transform)(2,1) * (*transform)(2,1));
01229         double temp = (*transform)(1,1) / s;
01230         if (temp > 1.0) temp = 1.0;
01231         if (temp < -1.0) temp = -1.0;
01232         double theta = acos(temp);
01233         double m  = (*transform)(1,1);
01234         double n  = (*transform)(2,1);
01235         double tx = (*transform)(3,1);
01236         double ty = (*transform)(4,1);
01237         
01238         if (isnan(theta)) theta = 0.0;
01239         if ((*transform)(2,1) / s < 0.0) theta = -theta;
01240         
01241         mInfo.s = s;
01242         mInfo.theta = theta;
01243         mInfo.tx = tx;
01244         mInfo.ty = ty;
01245         
01246         for (int i = 0; i < (int)(*newKeys).size(); i++){
01247           //          ((*newKeys)[i])->id = getNewKeypointID();
01248           
01249           // Set values for mInfo
01250           mInfo.initialVals.push_back((*newKeys)[i]->getID());
01251           mInfo.initialVals.push_back((*newKeys)[i]->imageX);
01252           mInfo.initialVals.push_back((*newKeys)[i]->imageY);
01253           mInfo.initialVals.push_back((*newKeys)[i]->imageScale);
01254           mInfo.initialVals.push_back((*newKeys)[i]->imageOrientation);
01255           
01256           double u = (*newKeys)[i]->imageX;
01257           double v = (*newKeys)[i]->imageY;
01258           double newy = (m*(v-ty)-n*(u-tx)) / (m*m + n*n);
01259           double newx = (m*(u-tx)+n*(v-ty)) / (m*m + n*n);
01260           double newscale = (*newKeys)[i]->imageScale / s;
01261           double neworientation = (*newKeys)[i]->imageOrientation - theta;
01262           while (neworientation > M_PI)   neworientation -= (2 * M_PI);
01263           while (neworientation <= -M_PI) neworientation += (2 * M_PI);
01264           
01265           //        cout << "(" << u << "," << v << "," << (*newKeys)[i]->orientation << "," << (*newKeys)[i]->scale << ") --> ("
01266           //            << "(" << newx << "," << newy << "," << neworientation << "," << newscale << ")\n";
01267           
01268           (*newKeys)[i]->modelX           = newx;
01269           (*newKeys)[i]->modelY           = newy;
01270           (*newKeys)[i]->modelScale       = newscale;
01271           (*newKeys)[i]->modelOrientation = neworientation;
01272           
01273           mInfo.newVals.push_back((*newKeys)[i]->getID());
01274           mInfo.newVals.push_back((*newKeys)[i]->modelX);
01275           mInfo.newVals.push_back((*newKeys)[i]->modelY);
01276           mInfo.newVals.push_back((*newKeys)[i]->modelScale);
01277           mInfo.newVals.push_back((*newKeys)[i]->modelOrientation);
01278           
01279           keypoint* k = (*newKeys)[i]->finalMatch;
01280           if (k != NULL){
01281             mInfo.hasMatch.push_back(true);
01282             mInfo.matchedVals.push_back(k->getID());
01283             mInfo.matchedVals.push_back(k->modelX);
01284             mInfo.matchedVals.push_back(k->modelY);
01285             mInfo.matchedVals.push_back(k->modelScale);
01286             mInfo.matchedVals.push_back(k->modelOrientation);
01287           }else{
01288             mInfo.hasMatch.push_back(false);
01289             mInfo.matchedVals.push_back(-1);
01290             mInfo.matchedVals.push_back(-1);
01291             mInfo.matchedVals.push_back(-1);
01292             mInfo.matchedVals.push_back(-1);
01293             mInfo.matchedVals.push_back(-1);
01294           }
01295         }
01296       }else{
01297         //        cout << "Matched to object " << bestModel->O->id << ", model " << bestModel->id << " (" << bestModel->filename << ") (error=" << bestError << "<=" << threshold << "=threshold)" << endl;
01298         mInfo.M = bestModel;
01299         
01300         //        for (int i = 0; i < (int)(*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]])).size(); i++){
01301         //          (*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]]))[i]->getKey1()->finalMatch = (*(inliers[bestModel->O->id][bestModel->id][solutionIndex[0]]))[i]->getKey2();
01302         //        }
01303         for (int i = 0; i < (int)mminfo[0]->inliers->size(); i++){
01304           (*(mminfo[0]->inliers))[i]->getKey1()->finalMatch = (*(mminfo[0]->inliers))[i]->getKey2();
01305         }
01306         
01307         for (int i = 0; i < (int)(*newKeys).size(); i++){
01308           // Set values for mInfo
01309           mInfo.initialVals.push_back((*newKeys)[i]->getID());
01310           mInfo.initialVals.push_back((*newKeys)[i]->imageX);
01311           mInfo.initialVals.push_back((*newKeys)[i]->imageY);
01312           mInfo.initialVals.push_back((*newKeys)[i]->imageScale);
01313           mInfo.initialVals.push_back((*newKeys)[i]->imageOrientation);
01314         }
01315         
01316         // For now, add all keypoints
01317         //        model *oldModel = bestModel;
01318         NEWMAT::Matrix* transform = mminfo[0]->solution;
01319         
01320         double s, theta, tx, ty;
01321         
01322         learn_toModel(newKeys, bestModel,transform, s, theta, tx, ty);
01323         
01324         
01325         mInfo.s = s;
01326         mInfo.theta = theta;
01327         mInfo.tx = tx;
01328         mInfo.ty = ty;
01329         
01330         for (int i = 0; i < (int)(*newKeys).size(); i++){
01331           mInfo.newVals.push_back((*newKeys)[i]->getID());
01332           mInfo.newVals.push_back((*newKeys)[i]->modelX);
01333           mInfo.newVals.push_back((*newKeys)[i]->modelY);
01334           mInfo.newVals.push_back((*newKeys)[i]->modelScale);
01335           mInfo.newVals.push_back((*newKeys)[i]->modelOrientation);
01336           
01337           keypoint* k = (*newKeys)[i]->finalMatch;
01338           if (k != NULL){
01339             mInfo.hasMatch.push_back(true);
01340             mInfo.matchedVals.push_back(k->getID());
01341             mInfo.matchedVals.push_back(k->modelX);
01342             mInfo.matchedVals.push_back(k->modelY);
01343             mInfo.matchedVals.push_back(k->modelScale);
01344             mInfo.matchedVals.push_back(k->modelOrientation);
01345           }else{
01346             mInfo.hasMatch.push_back(false);
01347             mInfo.matchedVals.push_back(-1);
01348             mInfo.matchedVals.push_back(-1);
01349             mInfo.matchedVals.push_back(-1);
01350             mInfo.matchedVals.push_back(-1);
01351             mInfo.matchedVals.push_back(-1);
01352           }
01353         }
01354         
01355       }
01356     }
01357   }
01358   
01359   if (toIntegrate){
01360     /// Rebuild KDTree
01361     // KDTree rebuild should be done in the functions learn_*
01362     rebuildKDTree();
01363   }else{
01364     for (int i = 0; i < (int)((*newKeys).size()); i++){
01365       delete (*newKeys)[i];
01366     }
01367   }
01368   
01369   /// Clean up
01370   //  for (int i = 0; i < oldNumOfObjects; i++){
01371   //    for (int j = 0; j < oldMaxNumModels+1; j++){
01372   //      for (int t = 0; t < (int)solutions[i][j].size(); t++){
01373   // //       if (solutions[i][j]) 
01374   //        delete solutions[i][j][t];
01375   // //       if (inliers[i][j]){
01376   //        for (int k = 0; k < (int)(inliers[i][j][t]->size()); k++){
01377   //          delete (*(inliers[i][j][t]))[k];
01378   //        }
01379   //        delete inliers[i][j][t];
01380   // //       }
01381   //      }
01382   //    }
01383   //  }
01384   for (int i = 0; i < (int)mminfo.size(); i++){
01385       delete mminfo[i];
01386   }
01387 }
01388 
01389 object* KnowledgeBase::objectExists(int objectID){
01390   // Check if the object exists in kb
01391   for (unsigned int i = 0; i < objects.size(); i++){
01392     if (objects[i]->getID() == objectID){
01393       return objects[i];
01394     }
01395   }
01396   return NULL;
01397 }
01398 
01399 void KnowledgeBase::setParameter(const char* paramName, double paramVal){
01400   CharPtrKey* key = new CharPtrKey(paramName);
01401   double* paramPtr = paramHash.retrieve(key);
01402   if (paramPtr == NULL){
01403     paramPtr = (double*)malloc(sizeof(double));
01404     *paramPtr = paramVal;
01405     paramHash.insert(paramPtr, key);
01406   }else{
01407     *paramPtr = paramVal;
01408     delete key;
01409   }
01410 }
01411 
01412 double KnowledgeBase::getParameter(const char* paramName){
01413   CharPtrKey* key = new CharPtrKey(paramName);
01414   double* paramPtr = paramHash.retrieve(key);
01415   delete key;
01416   if (paramPtr == NULL){
01417     return 0.0;
01418   }else{
01419     return *paramPtr;
01420   }
01421 }
01422 
01423 void KnowledgeBase::saveToFile(ofstream& outfile){
01424   // Write out keypointID
01425   outfile << keypoint::getKeypointID() << endl;
01426   
01427   // Write out params
01428   outfile << numParams << endl;
01429   for (int i = 0; i < numParams; i++){
01430     outfile << paramList[i] << endl;
01431     outfile << getParameter(paramList[i]) << endl;
01432   }
01433   
01434   // Write out objects
01435   outfile << objects.size() << endl;
01436   for (unsigned int i = 0; i < objects.size(); i++){
01437     objects[i]->writeToFile(outfile);
01438   }
01439   
01440   // Write out models
01441   outfile << models.size() << endl;
01442   for (unsigned int i = 0; i < models.size(); i++){
01443     models[i]->writeToFile(outfile);
01444   }
01445   
01446   // Write out keygroups
01447   outfile << keygroups.size() << endl;
01448   for (unsigned int i = 0; i < keygroups.size(); i++){
01449     keygroups[i]->writeToFile(outfile);
01450   }
01451   
01452   // Write out keypoints
01453   outfile << keys.size() << endl;
01454   for (unsigned int i = 0; i < keys.size(); i++){
01455     keys[i]->writeToFile(outfile);
01456   }
01457   
01458   outfile << maxNumModels << endl;
01459 }
01460 
01461 void KnowledgeBase::readFromFile(ifstream& infile){
01462   // Start from clean state
01463   cleanUpMemory();
01464   
01465   // Set keypointID
01466   int keypointID;
01467   infile >> keypointID;
01468   keypoint::setKeypointID(keypointID);
01469   
01470   // Read and store params
01471   int nParams;
01472   infile >> nParams;
01473   //  cout << nParams << endl;
01474   for (int i = 0; i < nParams; i++){
01475     char paramName[2048];
01476     double paramVal;
01477     infile.getline(paramName, 2048);
01478     infile.getline(paramName, 2048);
01479     infile >> paramVal;
01480     setParameter(paramName, paramVal);
01481     //    cout << paramName << ": " << paramVal << endl;
01482   }
01483   
01484   // Read objects
01485   unsigned int numObjects;
01486   infile >> numObjects;
01487   //  cout << numObjects << endl;
01488   vector< vector<int> > objectModelID;
01489   for (unsigned int i = 0; i < numObjects; i++){
01490     int objectID;
01491     unsigned int numModels;
01492     char objectName[2048];
01493     infile >> objectID;
01494     cout << objectID << endl;
01495     infile.getline(objectName, 2048);
01496     infile.getline(objectName, 2048);
01497     string objectNameStr(objectName);
01498     cout << objectName << endl;
01499     infile >> numModels;
01500     //    cout << numModels << endl;
01501     vector<int> modelIDs;
01502     for (unsigned int j = 0; j < numModels; j++){
01503       int id;
01504       infile >> id;
01505       modelIDs.push_back(id);
01506       //      cout << id << "\t";
01507     }
01508     //    cout << endl;
01509     objectModelID.push_back(modelIDs);
01510     //Create object
01511     object* O = new object(objectID);
01512     O->setName(objectNameStr);
01513     objects.push_back(O);
01514   }
01515   
01516   // Read models
01517   unsigned int numModels;
01518   infile >> numModels;
01519   //  cout << "#models: " << numModels << endl;
01520   vector< vector<int> > modelKeygroupID;
01521   for (unsigned int i = 0; i < numModels; i++){
01522     int objectID, modelID, generation;
01523     unsigned int numKeygroups;
01524     char modelName[2048];
01525     infile >> modelID >> objectID >> generation;
01526     //    cout << "ID: " << objectID << "->" << modelID << endl;
01527     infile.getline(modelName, 2048);
01528     infile.getline(modelName, 2048);
01529     string modelNameStr(modelName);
01530     //    cout << "Name: " << modelNameStr << endl;
01531     infile >> numKeygroups;
01532     //    cout << "#keygroups: " << numKeygroups << endl;
01533     vector<int> keygroupIDs;
01534     for (unsigned int j = 0; j < numKeygroups; j++){
01535       int id;
01536       infile >> id;
01537       keygroupIDs.push_back(id);
01538       //      cout << id << "\t";
01539     }
01540     //    cout << endl;
01541     modelKeygroupID.push_back(keygroupIDs);
01542     // Create model
01543     model* M = new model(modelID);
01544     M->generation = generation;
01545     M->setName(modelNameStr);
01546     //    bool found = false;
01547     for (unsigned int j = 0; j < objects.size(); j++){
01548       if (objects[j]->getID() == objectID){
01549         M->O = objects[j];
01550         objects[j]->models.push_back(M);
01551         models.push_back(M);
01552         //        found = true;
01553         break;
01554       }
01555     }
01556     //    if (!found) cout << "!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n";
01557   }
01558   
01559   // Read keygroups
01560   unsigned int numKeygroups;
01561   infile >> numKeygroups;
01562   //  cout << "#keygroups: " << numKeygroups << endl;
01563   vector< vector<int> > keygroupKeyID;
01564   vector< vector<int> > keygroupNeighborObjectID, keygroupNeighborModelID, keygroupNeighborID;
01565   for (unsigned int i = 0; i < numKeygroups; i++){
01566     int objectID, modelID, keygroupID;
01567     unsigned int numKeys, numNeighbors;
01568     infile >> keygroupID >> objectID >> modelID;
01569     //    cout << "ID: " << objectID << "->" << modelID << "->" << keygroupID << endl;
01570     infile >> numNeighbors;
01571     //    cout << "#neighbors: " << numNeighbors << endl;
01572     vector<int> neighborObjectIDs, neighborModelIDs, neighborIDs;
01573     for (unsigned int j = 0; j < numNeighbors; j++){
01574       int Oid, Mid, id;
01575       infile >> Oid >> Mid >> id;
01576       neighborObjectIDs.push_back(Oid);
01577       neighborModelIDs.push_back(Mid);
01578       neighborIDs.push_back(id);
01579       //      cout << Oid << "->" << Mid << "->" << id << "\t";
01580     }
01581     //    cout << endl;
01582     keygroupNeighborObjectID.push_back(neighborObjectIDs);
01583     keygroupNeighborModelID.push_back(neighborModelIDs);
01584     keygroupNeighborID.push_back(neighborIDs);
01585     infile >> numKeys;
01586     //    cout << "#keys: " << numKeys << endl;
01587     vector<int> keyIDs;
01588     for (unsigned int j = 0; j < numKeys; j++){
01589       int id;
01590       infile >> id;
01591       keyIDs.push_back(id);
01592       //      cout << id << "\t";
01593     }
01594     //    cout << endl;
01595     keygroupKeyID.push_back(keyIDs);
01596     // Create keygroup
01597     keygroup* G = new keygroup(keygroupID);
01598     //    bool found = false;
01599     for (unsigned int j = 0; j < objects.size(); j++){
01600       if (objects[j]->getID() == objectID){
01601         for (unsigned int k = 0; k < objects[j]->models.size(); k++){
01602           if (objects[j]->models[k]->getID() == modelID){
01603             G->M = objects[j]->models[k];
01604             objects[j]->models[k]->keygroups.push_back(G);
01605             keygroups.push_back(G);
01606             //            found = true;
01607             break;
01608           }
01609         }
01610         break;
01611       }
01612     }
01613     //    if (!found) cout << "!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n";
01614   }
01615   // Connect the keygroups' neighbors
01616   for (unsigned int i = 0; i < keygroupNeighborID.size(); i++){
01617     for (unsigned int j = 0; j < keygroupNeighborID[i].size(); j++){
01618       //      bool found = false;
01619       for (unsigned int oi = 0; oi < objects.size(); oi++){
01620         if (objects[oi]->getID() == keygroupNeighborObjectID[i][j]){
01621           for (unsigned int mi = 0; mi < objects[oi]->models.size(); mi++){
01622             if (objects[oi]->models[mi]->getID() == keygroupNeighborModelID[i][j]){
01623               for (unsigned gi = 0; gi < objects[oi]->models[mi]->keygroups.size(); gi++){
01624                 if (objects[oi]->models[mi]->keygroups[gi]->getID() == keygroupNeighborID[i][j]){
01625                   //                  found = true;
01626                   keygroups[i]->neighbors.push_back(objects[oi]->models[mi]->keygroups[gi]);
01627                   break;
01628                 }
01629               }
01630               break;
01631             }
01632           }
01633           break;
01634         }
01635       }
01636       //      if (!found) cout << "!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n";
01637     }
01638   }
01639   
01640   // Read keypoints
01641   unsigned int numKeypoints;
01642   vector<int> finalMatches;
01643   infile >> numKeypoints;
01644   //  vector< vector<int>
01645   //  cout << "#keys: " << numKeypoints << endl;
01646   for (unsigned int i = 0; i < numKeypoints; i++){
01647     int keypointID1, objectID, modelID, keygroupID;
01648     //    int finalMatchObjectID, finalMatchModelID, finalMatchKeygroupID, finalMatchID;
01649     infile >> keypointID1 >> objectID >> modelID >> keygroupID;
01650     //    cout << objectID << "->" << modelID << "->" << keygroupID << "->" << keypointID1 << endl;
01651     // Create keypoint and link to keygroup
01652     keypoint* K = new keypoint(keypointID1);
01653     for (unsigned int oi = 0; oi < objects.size(); oi++){
01654       if (objects[oi]->getID() == objectID){
01655         for (unsigned int mi = 0; mi < objects[oi]->models.size(); mi++){
01656           if (objects[oi]->models[mi]->getID() == modelID){
01657             for (unsigned int gi = 0; gi < objects[oi]->models[mi]->keygroups.size(); gi++){
01658               if (objects[oi]->models[mi]->keygroups[gi]->getID() == keygroupID){
01659                 K->G = objects[oi]->models[mi]->keygroups[gi];
01660                 objects[oi]->models[mi]->keygroups[gi]->keypts.push_back(K);
01661                 keys.push_back(K);
01662                 break;
01663               }
01664             }
01665             break;
01666           }
01667         }
01668         break;
01669       }
01670     }
01671     infile
01672     //        >> K->valid
01673     >> K->imageX
01674     >> K->imageY
01675     >> K->imageScale
01676     >> K->imageOrientation
01677     >> K->imageIntScale
01678     >> K->imageIntOctave
01679     >> K->modelX
01680     >> K->modelY
01681     >> K->modelScale
01682     >> K->modelOrientation
01683     >> K->generation
01684     ;
01685     bool finalMatchValid;
01686     infile >> finalMatchValid;
01687     if (finalMatchValid){
01688       int finalMatchID;
01689       infile >> finalMatchID;
01690       finalMatches.push_back(finalMatchID);
01691     }else{
01692       K->finalMatch = NULL;
01693       finalMatches.push_back(-1);
01694     }
01695     // Discard matches as we reconstruct from finalMatches
01696     int numMatches;
01697     infile >> numMatches;
01698     for (int m = 0; m < numMatches; m++){
01699       int matchID;
01700       infile >> matchID;
01701     }
01702     //    cout << "valid:" << K->valid << endl;
01703     unsigned int descSize;
01704     infile >> descSize;
01705     //    cout << "desc size = " << descSize << endl;
01706     for (unsigned int di = 0; di < descSize; di++){
01707       double d;
01708       infile >> d;
01709       K->desc.push_back(d);
01710     }
01711     //    unsigned int bestDistSize;
01712     //    infile >> bestDistSize;
01713     //    cout << "bestDist size = " << bestDistSize << endl;
01714     //    for (unsigned int di = 0; di < bestDistSize; di++){
01715     //      double d;
01716     //      infile >> d;
01717     //      K->bestDist.push_back(d);
01718     //    }
01719     //    bool finalMatchValid;
01720     //    infile >> finalMatchValid;
01721     //    if (finalMatchValid){
01722     //      infile >> finalMatchObjectID >> finalMatchModelID >> finalMatchKeygroupID >> finalMatchID;
01723     //    }
01724     //    return;
01725   }
01726   
01727   for (unsigned int i = 0; i < keys.size(); i++){
01728     if (finalMatches[i] == -1) continue;
01729     bool foundFinalMatch = false;
01730     for (unsigned int j = 0; j < keys.size(); j++){
01731       if (keys[j]->getID() == finalMatches[i]){
01732         foundFinalMatch = true;
01733         keys[i]->finalMatch = keys[j];
01734         keys[j]->matches.push_back(keys[i]);
01735       }
01736     }
01737     if (!foundFinalMatch) cout << "Error!\n";
01738   }
01739   
01740   keypoint::setKeypointID(keypointID);
01741   
01742   infile >> maxNumModels;
01743   
01744   rebuildKDTree();
01745   
01746 }
01747 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:42 2016 by Doxygen 1.6.3