00001
00002
00003
00004
00005
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
00026
00027
00028
00029 hm_cell GM[GM_CELL_COUNT];
00030
00031
00032
00033 void AGM::init(void)
00034 {
00035 int i;
00036 hm_cell gmc;
00037
00038 gmc.height = 0.0;
00039 gmc.trav = 0.5;
00040 gmc.confidence = 0.0;
00041 #ifndef UNIT_TEST
00042 gmc.color = COLOR_GREEN;
00043 #else
00044 gmc.color = GREEN;
00045 #endif
00046 gmc.cluster = 0;
00047
00048 for(i=0; i<GM_CELL_COUNT; ++i) GM[i] = gmc;
00049 }
00050
00051
00052
00053
00054 void AGM::carryOver(double x, double y, hm_cell &cell)
00055 {
00056 int i;
00057
00058
00059
00060 if(!xy2gm_index(x, y, i)) return;
00061
00062
00063 if(cell.confidence > GM[i].confidence) GM[i] = cell;
00064 }
00065
00066
00067 void AGM::decay()
00068 {
00069 for(int i=0; i<GM_CELL_COUNT; ++i) GM[i].confidence *= ALM_GM_TAX;
00070 }
00071
00072
00073
00074
00075
00076 void AGM::genRequests(MRvector &requests)
00077 {
00078
00079 MotionRequest request;
00080
00081 static const double d_x = AGM_LEFT - AGM_RIGHT;
00082 static const double d_y = AGM_TOP - AGM_BOTTOM;
00083
00084 double gm_centers[AGM_NUMCLUSTERS][2];
00085
00086
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
00093 for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00094
00095
00096 double popularity[AGM_NUMCLUSTERS];
00097 for(int i=0; i<AGM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00098
00099
00100 for(int i=0; i<GM_CELL_COUNT; ++i) {
00101 double x, y, bestdist = HUGE_VAL;
00102 gm_index2xy(i, x, y);
00103
00104
00105 for(int cindex=0; cindex<AGM_NUMCLUSTERS; cindex++) {
00106 double rx, ry, dist;
00107
00108 rx = x - gm_centers[cindex][0];
00109 ry = y - gm_centers[cindex][1];
00110 dist = rx*rx + ry*ry;
00111
00112
00113 if(dist < bestdist) { bestdist = dist; GM[i].cluster = cindex; }
00114 }
00115
00116
00117 popularity[GM[i].cluster] += 1 - GM[i].confidence;
00118 }
00119
00120
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
00126 for(int i=0; i<GM_CELL_COUNT; ++i) {
00127 double x, y, uncertainty;
00128 gm_index2xy(i, x, y);
00129 uncertainty = 1 - GM[i].confidence;
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
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
00158
00159
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
00169 srand(time(NULL));
00170 srand48(time(NULL));
00171
00172
00173 AGM::init();
00174
00175
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
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
00222 cout << "Clustering..." << endl;
00223 MRvector v;
00224 AGM::genRequests(v);
00225
00226
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