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"
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
00040 }
00041 for (int i = 1; i <= n-r; i++){
00042 denominator *= (double)i;
00043
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
00053 int numKeys = (int)keyPairs.size();
00054
00055
00056
00057 int Arows = 2 * numKeys;
00058 int Acols = 4;
00059
00060
00061
00062 int brows = 2 * numKeys;
00063 int bcols = 1;
00064
00065
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
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
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
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
00139 if (tempX < -1.0) tempX = -1.0;
00140 if (tempX > 1.0) tempX = 1.0;
00141 double theta = acos(tempX);
00142
00143 if (solution(2,1) / scale < 0.0) theta = -theta;
00144 delete inliers;
00145 inliers = new std::vector<keypointPair*>;
00146
00147
00148
00149
00150
00151
00152
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
00170
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
00177
00178
00179
00180 if (xyError >= xyDist
00181 && scaleError >= scaleDist
00182 && rotationError >= rotationDist
00183 ){
00184 inliers->push_back(allPairs[j]);
00185 allPairs[j]->getKey1()->isInlier = true;
00186 }else{
00187 allPairs[j]->getKey1()->isInlier = false;
00188 }
00189 }
00190
00191 if (inliers->size() < 4) break;
00192
00193 }
00194
00195 if (inliers->size() < 4) break;
00196
00197
00198
00199 xyError = maxXYError / (i+2);
00200 scaleError = maxScaleError / (i+2);
00201 rotationError = maxRotationError / (i+2);
00202
00203
00204
00205
00206 }
00207
00208 NEWMAT::Matrix finalSolution;
00209
00210 if (inliers->size() >= 4)
00211 finalSolution = leastSquareSolution(*inliers, error);
00212 else{
00213
00214
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
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
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
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
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
00309
00310
00311
00312
00313
00314 }
00315 }
00316 }
00317 }
00318 }
00319
00320
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
00329 NEWMAT::Matrix solution = getBestTransform2(allPairs, *hashData[i], &err, &in);
00330
00331
00332 size_t numInliers = in->size();
00333
00334 if (numInliers >= 4){
00335
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
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
00362
00363 transforms->insert(transformsLocation, solution);
00364 error->insert(errorLocation, err);
00365
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
00375
00376
00377
00378
00379
00380 }else{
00381 delete in;
00382 }
00383 }
00384
00385
00386
00387 for (int i = 0; i < (int)(*inliersToReturn).size(); i++){
00388
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
00408
00409
00410
00411
00412
00413
00414
00415
00416
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(¶mVals);
00469 paramHash.retrieveAllKeys(¶mNames);
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
00483 int maxSearches = 20;
00484 int n = 10;
00485 myTree->getBestNKeypointMatch(*(*newKeys)[i], maxSearches, n);
00486 if ((*newKeys)[i]->bestMatch[0] == NULL){
00487
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();
00492
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
00497 dist1 = sqrt((*newKeys)[i]->bestDist[j]);
00498 break;
00499 }
00500 }
00501
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
00507
00508
00509
00510 matches[objectID][modelID].push_back((*newKeys)[i]->bestMatch[0]);
00511 imageKey[objectID][modelID].push_back((*newKeys)[i]);
00512
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
00517
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
00526 }
00527
00528 void KnowledgeBase::modelMatching(size_t numNewKeys, vector< vector< vector<keypoint*> > >& matches, vector< vector< vector<keypoint*> > >& imageKey, vector<modelMatchingInfo*>& mminfo){
00529
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
00537 for (int j = 0; j < (int)objects[i]->models.size(); j++){
00538 int modelID = objects[i]->models[j]->getID();
00539
00540 if (imageKey[objectID][modelID].size() > 4){
00541 double error;
00542
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
00549 if (transforms.size() == 0){
00550 continue;
00551 }
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585 for (int t = 0; t < (int)transforms.size(); t++){
00586
00587
00588
00589
00590
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;
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
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);
00614
00615 }
00616
00617 double PM_f = PM / (PM + Pf_notM);
00618
00619
00620
00621
00622
00623 if (PM_f >= probOfMatch && numInliers >= 4 && error <= threshold){
00624
00625
00626
00627
00628
00629
00630
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
00640 for (int tt = 0; tt < (int)in[t]->size(); tt++){
00641 delete (*(in[t]))[tt];
00642 }
00643 delete in[t];
00644 }
00645 }
00646
00647
00648
00649 }
00650 }
00651 }
00652
00653
00654
00655
00656
00657
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
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
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
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
00701
00702 M->O = O;
00703 O->models.push_back(M);
00704 if (maxNumModels < O->models.size()) maxNumModels = O->models.size();
00705
00706
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
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
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
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
00750
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
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
00780
00781
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
00798 for (int i = 0; i < (int)(*newKeys).size(); i++){
00799
00800 keygroup *G = new keygroup();
00801
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
00810
00811
00812
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
00828
00829 if((*keygroups.at(j)).compareTo(G) == 0) {
00830 keygroups.erase(keygroups.begin()+j);
00831
00832 }
00833 }
00834
00835
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
00848
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
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
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
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
00921
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;
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
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
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
01014
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
01039
01040 matchingGroup->keypts.push_back((*newKeys)[i]);
01041 (*newKeys)[i]->G = matchingGroup;
01042 }else{
01043
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
01075 vector<keypoint*>* newKeys;
01076
01077 newKeys = &K;
01078
01079
01080
01081
01082
01083
01084 size_t objectsSize = objects.size();
01085 size_t maxNumModels1 = maxNumModels+1;
01086
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
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
01101 vector<modelMatchingInfo*> mminfo;
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123 modelMatching((*newKeys).size(), matches, imageKey, mminfo);
01124
01125
01126
01127
01128
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");
01140 if (bestModel == NULL){
01141
01142
01143
01144
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
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
01181 mInfo.O = mInfo.matchedO = bestModel->O;
01182 mInfo.matchedM = bestModel;
01183
01184 if (bestError > threshold){
01185
01186
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
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
01214
01215 if (!toIntegrate){
01216
01217 mInfo.M = bestModel;
01218
01219
01220
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
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
01248
01249
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
01266
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
01298 mInfo.M = bestModel;
01299
01300
01301
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
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
01317
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
01361
01362 rebuildKDTree();
01363 }else{
01364 for (int i = 0; i < (int)((*newKeys).size()); i++){
01365 delete (*newKeys)[i];
01366 }
01367 }
01368
01369
01370
01371
01372
01373
01374
01375
01376
01377
01378
01379
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
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
01425 outfile << keypoint::getKeypointID() << endl;
01426
01427
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
01435 outfile << objects.size() << endl;
01436 for (unsigned int i = 0; i < objects.size(); i++){
01437 objects[i]->writeToFile(outfile);
01438 }
01439
01440
01441 outfile << models.size() << endl;
01442 for (unsigned int i = 0; i < models.size(); i++){
01443 models[i]->writeToFile(outfile);
01444 }
01445
01446
01447 outfile << keygroups.size() << endl;
01448 for (unsigned int i = 0; i < keygroups.size(); i++){
01449 keygroups[i]->writeToFile(outfile);
01450 }
01451
01452
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
01463 cleanUpMemory();
01464
01465
01466 int keypointID;
01467 infile >> keypointID;
01468 keypoint::setKeypointID(keypointID);
01469
01470
01471 int nParams;
01472 infile >> nParams;
01473
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
01482 }
01483
01484
01485 unsigned int numObjects;
01486 infile >> numObjects;
01487
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
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
01507 }
01508
01509 objectModelID.push_back(modelIDs);
01510
01511 object* O = new object(objectID);
01512 O->setName(objectNameStr);
01513 objects.push_back(O);
01514 }
01515
01516
01517 unsigned int numModels;
01518 infile >> numModels;
01519
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
01527 infile.getline(modelName, 2048);
01528 infile.getline(modelName, 2048);
01529 string modelNameStr(modelName);
01530
01531 infile >> numKeygroups;
01532
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
01539 }
01540
01541 modelKeygroupID.push_back(keygroupIDs);
01542
01543 model* M = new model(modelID);
01544 M->generation = generation;
01545 M->setName(modelNameStr);
01546
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
01553 break;
01554 }
01555 }
01556
01557 }
01558
01559
01560 unsigned int numKeygroups;
01561 infile >> numKeygroups;
01562
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
01570 infile >> numNeighbors;
01571
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
01580 }
01581
01582 keygroupNeighborObjectID.push_back(neighborObjectIDs);
01583 keygroupNeighborModelID.push_back(neighborModelIDs);
01584 keygroupNeighborID.push_back(neighborIDs);
01585 infile >> numKeys;
01586
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
01593 }
01594
01595 keygroupKeyID.push_back(keyIDs);
01596
01597 keygroup* G = new keygroup(keygroupID);
01598
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
01607 break;
01608 }
01609 }
01610 break;
01611 }
01612 }
01613
01614 }
01615
01616 for (unsigned int i = 0; i < keygroupNeighborID.size(); i++){
01617 for (unsigned int j = 0; j < keygroupNeighborID[i].size(); j++){
01618
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
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
01637 }
01638 }
01639
01640
01641 unsigned int numKeypoints;
01642 vector<int> finalMatches;
01643 infile >> numKeypoints;
01644
01645
01646 for (unsigned int i = 0; i < numKeypoints; i++){
01647 int keypointID1, objectID, modelID, keygroupID;
01648
01649 infile >> keypointID1 >> objectID >> modelID >> keygroupID;
01650
01651
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
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
01696 int numMatches;
01697 infile >> numMatches;
01698 for (int m = 0; m < numMatches; m++){
01699 int matchID;
01700 infile >> matchID;
01701 }
01702
01703 unsigned int descSize;
01704 infile >> descSize;
01705
01706 for (unsigned int di = 0; di < descSize; di++){
01707 double d;
01708 infile >> d;
01709 K->desc.push_back(d);
01710 }
01711
01712
01713
01714
01715
01716
01717
01718
01719
01720
01721
01722
01723
01724
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