Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

agmMain.cc

Go to the documentation of this file.
00001 /*
00002  * agmMain.cc -- defines the main class for interfacing with the AIBO
00003  *   Global (height) Map. See agmMain.h for details.
00004  *
00005  * Started 1-21-2002, tss
00006  */
00007 
00008 #include "Configuration.h"
00009 #include "agmMain.h"
00010 #include "almStructures.h"
00011 #include "almUtility.h"
00012 
00013 #include "../WorldModel2.h"
00014 
00015 #include <cmath>
00016 #include <iostream>
00017 
00018 #ifdef UNIT_TEST_AGM_MA
00019 #warning Compiling unit test code for agmMain.cc
00020 #include <fstream>
00021 #include <cstdlib>
00022 #include <ctime>
00023 #endif
00024 
00025   // Here we keep the map data itself, which is declared as a global because
00026   // the AIBO has a buggy malloc, and anyway we only need one map. I know this
00027   // is bad, but I'll try to write the code so that it doesn't matter so much.
00028   //   Here is the big global map.
00029 hm_cell GM[GM_CELL_COUNT];
00030 
00031   // This method initializes the static table used to hold the map data.
00032   // Yes, you have to manually run an initializer. I apologize!
00033 void AGM::init(void)
00034 {
00035   int i;
00036   hm_cell gmc;
00037 
00038   gmc.height = 0.0;
00039   gmc.trav = 0.5;   // maybe traversable? maybe...
00040   gmc.confidence = 0.0;
00041 #ifndef UNIT_TEST
00042   gmc.color = COLOR_GREEN;  // assumption: grass as far as the eye can see
00043 #else
00044   gmc.color = GREEN;    // assumption: grass as far as the eye can see
00045 #endif
00046   gmc.cluster = 0;    // just set cluster to 0
00047 
00048   for(i=0; i<GM_CELL_COUNT; ++i) GM[i] = gmc;
00049 }
00050 
00051   // Carry over information from the local height map. More literally,
00052   // see about placing height map cell at x,y. We may not do it if data
00053   // there are from a measurement we're more confident about.
00054 void AGM::carryOver(double x, double y, hm_cell &cell)
00055 {
00056   int i;
00057 
00058     // Get an index into the global height map.
00059     // Skip if we're out of bounds
00060   if(!xy2gm_index(x, y, i)) return;
00061 
00062     // yeppers. Go with the higher confidence cell
00063   if(cell.confidence > GM[i].confidence) GM[i] = cell;
00064 }
00065 
00066   // Tax the certainty of the global map after a move
00067 void AGM::decay()
00068 {
00069   for(int i=0; i<GM_CELL_COUNT; ++i) GM[i].confidence *= ALM_GM_TAX;
00070 }
00071 
00072   // This method generates motion requests for WorldModel2. It's the reason
00073   // WorldModel2 is our friend.
00074   // For the time being, the method used here is K-means clustering.
00075   // Hope you like loops!
00076 void AGM::genRequests(MRvector &requests)
00077 {
00078     // useful storage space
00079   MotionRequest request;
00080     // find handy constants
00081   static const double d_x = AGM_LEFT - AGM_RIGHT;
00082   static const double d_y = AGM_TOP  - AGM_BOTTOM;
00083     // Store the cluster centers
00084   double gm_centers[AGM_NUMCLUSTERS][2]; // would use new, but bad malloc
00085 
00086     // Randomly distribute centers through GM
00087   for(int i=0; i<AGM_NUMCLUSTERS; ++i) { 
00088     gm_centers[i][0] = AGM_LEFT + (double)rand() * d_x / (double) RAND_MAX;
00089     gm_centers[i][1] = AGM_TOP  + (double)rand() * d_y / (double) RAND_MAX;
00090   }
00091 
00092     // The actual K-means bit
00093   for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00094       // Array keeps track of how popular each cluster center is in terms
00095       // of member uncertainty
00096     double popularity[AGM_NUMCLUSTERS];
00097     for(int i=0; i<AGM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00098 
00099       // Tag all of the points as belonging to a specific cluster
00100     for(int i=0; i<GM_CELL_COUNT; ++i) {
00101       double x, y, bestdist = HUGE_VAL;
00102       gm_index2xy(i, x, y);     // get x and y for this index
00103 
00104         // Find out which cluster center is best for this point
00105       for(int cindex=0; cindex<AGM_NUMCLUSTERS; cindex++) {
00106   double rx, ry, dist;
00107 
00108   rx   = x - gm_centers[cindex][0]; // get relative x value
00109   ry   = y - gm_centers[cindex][1]; // get relative y value
00110   dist = rx*rx + ry*ry;             // get relative distance squared
00111 
00112     // is this point the best seen yet?
00113   if(dist < bestdist) { bestdist = dist; GM[i].cluster = cindex; }
00114       }
00115   // increment chosen cluster center popularity--more uncertain
00116   // members mean greater popularity!
00117       popularity[GM[i].cluster] += 1 - GM[i].confidence;
00118     }
00119 
00120       // Now shift the cluster centers to the weighted center of their clusters.      // Start by resetting everything to 0
00121     for(int cindex=0; cindex<AGM_NUMCLUSTERS; cindex++) {
00122       gm_centers[cindex][0] = 0.0;
00123       gm_centers[cindex][1] = 0.0;
00124     }
00125       // Now add the influence of all the members of the GM
00126     for(int i=0; i<GM_CELL_COUNT; ++i) {
00127       double x, y, uncertainty;
00128       gm_index2xy(i, x, y);     // get x and y for this index
00129       uncertainty = 1 - GM[i].confidence; // get uncertainty for this index
00130 
00131       gm_centers[GM[i].cluster][0] +=
00132   x * uncertainty / popularity[GM[i].cluster];
00133       gm_centers[GM[i].cluster][1] +=
00134   y * uncertainty / popularity[GM[i].cluster];
00135     }
00136   }
00137 
00138     // K-means is finished for the GM. Store results in the requests vector
00139   request.type = MotionRequest::GO_TO;
00140   for(int cindex=0; cindex<AGM_NUMCLUSTERS; cindex++) {
00141     request.xy.x = gm_centers[cindex][0];
00142     request.xy.y = gm_centers[cindex][1];
00143     requests.push_back(request);
00144   }
00145 }
00146 
00147 void AGM::dump(hmPicker &p, std::ostream &out)
00148 {
00149   for(int y=0; y<AGM_V_SIZE; ++y) {
00150     int yi = y*AGM_H_SIZE;
00151     out << p(GM[yi]);
00152     for(int x=1; x<AGM_H_SIZE; ++x) out << '\t' << p(GM[++yi]);
00153     out << std::endl;
00154   }
00155 }
00156 
00157   // This method provides direct access to the global map data array. You
00158   // should use GM_CELL_COUNT to index the array. This method is PRIVATE
00159   // and is intended only for use by WorldModel2 objects.
00160 hm_cell *AGM::getGM() { return GM; }
00161 
00162 
00163 #ifdef UNIT_TEST_AGM_MA
00164 int main(int argc, char **argv)
00165 {
00166   using namespace std;
00167 
00168   // 10 RANDOMIZE TIMER
00169   srand(time(NULL));
00170   srand48(time(NULL));
00171 
00172   // initialize AGM
00173   AGM::init();
00174 
00175   // OK, now randomly spatter around some uncertainty
00176   hm_cell gmc;
00177   for(int i=0; i<3000000; ++i) {
00178     float x = drand48()*4000 - 2000;
00179     float y = drand48()*4000 - 2000;
00180 
00181     gmc.height = drand48()*4;
00182     gmc.trav = 1 - drand48()*0.2;
00183     gmc.confidence = drand48()*0.2 + 0.2;
00184     gmc.color = ORANGE;
00185 
00186     AGM::carryOver(x, y, gmc);
00187   }
00188 
00189   // OK, put certainty in select spots
00190   for(int i=0; i<800000; ++i) {
00191     float x = drand48()*1333 - 2000;
00192     float y = drand48()*1333 - 2000;
00193     gmc.confidence = drand48()*0.2 + 0.8;
00194     AGM::carryOver(x, y, gmc);
00195   }
00196   for(int i=0; i<800000; ++i) {
00197     float x = drand48()*1333 + 666;
00198     float y = drand48()*1333 - 2000;
00199     gmc.confidence = drand48()*0.2 + 0.8;
00200     AGM::carryOver(x, y, gmc);
00201   }
00202   for(int i=0; i<800000; ++i) {
00203     float x = drand48()*1333 - 2000;
00204     float y = drand48()*1333 + 666;
00205     gmc.confidence = drand48()*0.2 + 0.8;
00206     AGM::carryOver(x, y, gmc);
00207   }
00208   for(int i=0; i<800000; ++i) {
00209     float x = drand48()*1333 + 666;
00210     float y = drand48()*1333 + 666;
00211     gmc.confidence = drand48()*0.2 + 0.8;
00212     AGM::carryOver(x, y, gmc);
00213   }
00214   for(int i=0; i<800000; ++i) {
00215     float x = drand48()*1333 - 666;
00216     float y = drand48()*1333 - 666;
00217     gmc.confidence = drand48()*0.2 + 0.8;
00218     AGM::carryOver(x, y, gmc);
00219   }
00220 
00221   // cluster!
00222   cout << "Clustering..." << endl;
00223   MRvector v;
00224   AGM::genRequests(v);
00225 
00226   // write files to ensure everything's in place off the bat
00227   cout << "Saving..." << endl;
00228   cout.flush();
00229   hmPickHeight hp;
00230   hmPickTrav ht;
00231   hmPickConfidence hn;
00232   hmPickColor hc;
00233   hmPickCluster hl;
00234   ofstream ih("iHeight.mat"); AGM::dump(hp, ih); ih.close();
00235   ofstream it("iTrav.mat"); AGM::dump(ht, it); it.close();
00236   ofstream in("iConfidence.mat"); AGM::dump(hn, in); in.close();
00237   ofstream ic("iColor.mat"); AGM::dump(hc, ic); ic.close();
00238   ofstream il("iCluster.mat"); AGM::dump(hl, il); il.close();
00239 
00240   return 0;
00241 }
00242 
00243 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2