00001
00002
00003
00004
00005
00006
00007
00008 #include "Configuration.h"
00009 #include "almMain.h"
00010 #include "almUtility.h"
00011 #include "almStructures.h"
00012 #include "agmMain.h"
00013
00014 #include "../Common/WalkMotionModel.h"
00015 #include "../FastSLAM/afsParticle.h"
00016 #include "../WorldModel2.h"
00017
00018 #ifndef UNIT_TEST_ALM_MA
00019 #include "Shared/WorldState.h"
00020 #include "Vision/Vision.h"
00021 #include "Vision/Visiondefines.h"
00022 #endif
00023
00024 #include <cmath>
00025 #include <iostream>
00026
00027 #ifdef UNIT_TEST_ALM_MA
00028 #warning Compiling unit test code for almMain.cc
00029 #include <fstream>
00030 #include <cstdlib>
00031 #include <ctime>
00032
00033 #define IROORDIST 900
00034 #else
00035 #define IROORDIST WorldState::IROORDist
00036 #endif
00037
00038
00039
00040
00041
00042 #define SQRT_2_PI 2.5066282746310002416
00043
00044
00045
00046
00047
00048
00049
00050 hm_cell HMs[2][HM_CELL_COUNT];
00051 hm_cell *HM;
00052
00053
00054
00055 dm_cell DMs[2][DM_CELL_COUNT];
00056 dm_cell *DM;
00057
00058
00059
00060 void ALM::init(void)
00061 {
00062
00063
00064 DM = DMs[1];
00065 HM = HMs[1];
00066 nukeAndPaveCurrentMap();
00067
00068 DM = DMs[0];
00069 HM = HMs[0];
00070 nukeAndPaveCurrentMap();
00071 }
00072
00073
00074
00075
00076 void ALM::move(double dx, double dy, double da, unsigned int time)
00077 {
00078 dm_cell *old_DM = DM;
00079 hm_cell *old_HM = HM;
00080
00081
00082 if(old_DM == DMs[0]) {
00083 DM = DMs[1];
00084 HM = HMs[1];
00085 }
00086 else {
00087 DM = DMs[0];
00088 HM = HMs[0];
00089 }
00090
00091
00092
00093
00094
00095 nukeAndPaveCurrentMap();
00096
00097
00098
00099 double rel_x = 0;
00100 double rel_y = 0;
00101 double rel_theta = 0;
00102
00103 WalkMotionModel(&rel_x, &rel_y, &rel_theta, dx, dy, da, time);
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 double sinrtheta = sin(rel_theta);
00119 double cosrtheta = cos(rel_theta);
00120 double t13_or14 = -cosrtheta*rel_x - sinrtheta*rel_y;
00121 double t23_or24 = sinrtheta*rel_x - cosrtheta*rel_y;
00122
00123
00124 for(int index=0; index<HM_CELL_COUNT; ++index) {
00125
00126 double cell_x, cell_y;
00127 hm_index2xy(index, cell_x, cell_y);
00128
00129
00130 double new_cell_x = cosrtheta*cell_x + sinrtheta*cell_y + t13_or14;
00131 double new_cell_y = -sinrtheta*cell_x + cosrtheta*cell_y + t23_or24;
00132
00133
00134
00135 int new_index;
00136 if(!xy2hm_index(new_cell_x, new_cell_y, new_index)) continue;
00137
00138
00139
00140
00141
00142 if(HM[new_index].confidence > old_HM[index].confidence) continue;
00143
00144
00145 HM[new_index] = old_HM[index];
00146
00147
00148 HM[new_index].confidence *= ALM_HM_TAX;
00149 }
00150
00151
00152
00153 for(int index=0; index<DM_CELL_COUNT; ++index) {
00154
00155
00156
00157 if(old_DM[index].depth >= IROORDIST) continue;
00158
00159
00160 double cell_az, cell_alt;
00161 dm_index2angles(index, cell_az, cell_alt);
00162
00163
00164 double cell_x, cell_y, cell_z;
00165 neck_range2xyz(old_DM[index].depth, cell_az, cell_alt,
00166 cell_x, cell_y, cell_z);
00167
00168
00169 double new_cell_x = cosrtheta*cell_x + sinrtheta*cell_y + t13_or14;
00170 double new_cell_y = -sinrtheta*cell_x + cosrtheta*cell_y + t13_or14;
00171
00172
00173
00174 double new_cell_az, new_cell_alt, new_cell_depth;
00175 xyz2neck_range(new_cell_x, new_cell_y, cell_z,
00176 new_cell_depth, new_cell_az, new_cell_alt);
00177
00178
00179
00180
00181 if(new_cell_depth > IROORDIST) new_cell_depth = IROORDIST;
00182
00183
00184
00185 int new_index;
00186 if(!angles2dm_index(new_cell_az, new_cell_alt, new_index)) continue;
00187
00188
00189
00190 if(DM[new_index].depth < new_cell_depth) continue;
00191
00192
00193 DM[new_index] = old_DM[index];
00194 DM[new_index].depth = new_cell_depth;
00195
00196
00197 DM[new_index].confidence = old_DM[index].confidence * ALM_DM_TAX;
00198 }
00199 }
00200
00201
00202
00203
00204
00205 void ALM::nukeAndPaveCurrentMap(void)
00206 {
00207 int i;
00208 hm_cell hmc;
00209 dm_cell dmc;
00210
00211 dmc.confidence = 0.0;
00212
00213 for(i=0; i<DM_CELL_COUNT; ++i) {
00214
00215
00216
00217 double azimuth, altitude;
00218
00219
00220 dm_index2angles(i, azimuth, altitude);
00221
00222
00223 if(altitude > 0) {
00224 dmc.depth = IROORDIST;
00225 #ifndef UNIT_TEST
00226 dmc.color = COLOR_BLUE;
00227 #else
00228 dmc.color = BLUE;
00229 #endif
00230 }
00231
00232
00233 else {
00234 dmc.depth = AIBO_TILT_PIVOT_HEIGHT / -sin(altitude);
00235 if(dmc.depth > IROORDIST) dmc.depth = IROORDIST;
00236 #ifndef UNIT_TEST
00237 dmc.color = COLOR_GREEN;
00238 #else
00239 dmc.color = GREEN;
00240 #endif
00241 }
00242
00243 DM[i] = dmc;
00244 }
00245
00246 hmc.height = 0.0;
00247 hmc.trav = 0.5;
00248 hmc.confidence = 0.0;
00249 #ifndef UNIT_TEST
00250 hmc.color = COLOR_GREEN;
00251 #else
00252 hmc.color = GREEN;
00253 #endif
00254
00255 for(i=0; i<HM_CELL_COUNT; ++i) HM[i] = hmc;
00256 }
00257
00258
00259 void ALM::registerDepth(double depth, double tilt, double pan)
00260 {
00261 registerDepth(depth, tilt, pan, 0);
00262 }
00263
00264
00265
00266
00267 void ALM::registerDepth(double depth, double tilt, double pan,
00268 unsigned int kludges)
00269 {
00270 double x, y, z;
00271 double n_depth, n_azimuth, n_altitude;
00272 int index;
00273
00274
00275
00276
00277
00278 if(depth < IROORDIST)
00279 depth = AIBO_IR_CAL_MULTIPLIER*depth + AIBO_IR_CAL_OFFSET;
00280
00281
00282 head_range2xyz(depth, pan, tilt, x, y, z);
00283
00284
00285 if((kludges & WM2Kludge::IgnoreZLessThanZero) && (z < 0.0)) return;
00286
00287
00288 xyz2neck_range(x, y, z, n_depth, n_azimuth, n_altitude);
00289
00290
00291
00292
00293 int splat_width = (int)(n_depth * 2.0*ALM_IR_SPLAT_STDDEV);
00294
00295
00296
00297
00298 if(!angles2dm_index(n_azimuth, n_altitude, index)) return;
00299
00300
00301 #ifndef UNIT_TEST_ALM_MA
00302
00303
00304 colortype center_cell_color;
00305 {
00306 int i;
00307 int vx = vision->width / 2;
00308 int vy = vision->height / 2;
00309 for(i=0; i<vision->num_runs; ++i)
00310 if((vision->rmap[i].y == vy) &&
00311 (vision->rmap[i].x + vision->rmap[i].width >= vx)) break;
00312 center_cell_color = vision->rmap[i].color;
00313 }
00314
00315
00316 if((kludges & WM2Kludge::IgnoreGreenItems) &&
00317 (center_cell_color == COLOR_GREEN))
00318 return;
00319 #else
00320 colortype center_cell_color = GREEN;
00321 #endif
00322
00323
00324 for(int dx=-splat_width; dx<=splat_width; ++dx) {
00325 for(int dy=-splat_width; dy<=splat_width; ++dy) {
00326
00327 double splat_var = (double)(dx*dx + dy*dy);
00328
00329
00330
00331 if(splat_var > 4.0*(double)(splat_width*splat_width)) continue;
00332
00333
00334
00335 int x_check = (index % ALM_DM_H_SIZE) + dx;
00336 if((x_check < 0) || (x_check >= ALM_DM_H_SIZE)) continue;
00337
00338
00339 int splat_index = index + dy*ALM_DM_H_SIZE + dx;
00340
00341 if((splat_index < 0) || (splat_index >= DM_CELL_COUNT)) continue;
00342
00343
00344 double splat_confidence =
00345 1/((double)splat_width*SQRT_2_PI)*exp(-splat_var/2.0*(double)splat_var);
00346
00347
00348
00349
00350
00351 double splat_az, splat_alt;
00352 double splat_x, splat_y, splat_z;
00353 dm_index2angles(splat_index, splat_az, splat_alt);
00354 neck_range2xyz(n_depth, splat_az, splat_alt, splat_x, splat_y, splat_z);
00355
00356
00357
00358 if((kludges & WM2Kludge::IgnoreZLessThanZero) && (splat_z < 0.0))
00359 continue;
00360
00361
00362 if((splat_index >= 0) && (splat_index < DM_CELL_COUNT))
00363
00364
00365 if(splat_confidence > DM[splat_index].confidence) {
00366 DM[splat_index].depth = n_depth;
00367 DM[splat_index].confidence = splat_confidence;
00368 DM[splat_index].color = center_cell_color;
00369 }
00370
00371
00372
00373
00374
00375 if(xy2hm_index(splat_x, splat_y, splat_index)) {
00376
00377
00378 if(splat_confidence > HM[splat_index].confidence) {
00379 HM[splat_index].height = splat_z;
00380 HM[splat_index].confidence = splat_confidence;
00381
00382
00383 if((splat_z > AIBO_MAX_BUMP) && (splat_z < AIBO_MIN_CLEARANCE))
00384 HM[splat_index].trav = 0.0;
00385 else
00386 HM[splat_index].trav = 1.0;
00387 HM[splat_index].color = center_cell_color;
00388 }
00389 }
00390 }
00391 }
00392
00393
00394 }
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404 void ALM::registerGround()
00405 {
00406 #ifndef UNIT_TEST_ALM_MA
00407 static int curr_img_width = 0;
00408 static int curr_img_height = 0;
00409 static double cam_depth_wpix = 0;
00410 static double cam_depth_hpix = 0;
00411 static int ciw_2 = 0;
00412 static int cih_2 = 0;
00413 static double z = AIBO_TILT_PIVOT_HEIGHT+AIBO_NECK_HEIGHT;
00414
00415
00416
00417
00418
00419
00420
00421 if((vision->width != curr_img_width) || (vision->height != curr_img_height)) {
00422 curr_img_width = vision->width;
00423 curr_img_height = vision->height;
00424
00425 ciw_2 = curr_img_width/2;
00426 cih_2 = curr_img_height/2;
00427
00428
00429
00430
00431
00432
00433 cam_depth_wpix = ((double) ciw_2)*tan(AIBO_CAM_H_FOV/2);
00434
00435
00436
00437
00438 cam_depth_hpix = ((double) cih_2)*tan(AIBO_CAM_V_FOV/2);
00439 }
00440
00441 for(int i=0; i<vision->num_runs; ++i) {
00442 int dy = vision->rmap[i].y - cih_2;
00443
00444 if(dy <= 0) continue;
00445
00446 if(vision->rmap[i].color != COLOR_GREEN) continue;
00447
00448
00449
00450 double x = cam_depth_hpix / ((double) dy)*z;
00451
00452 int dx = vision->rmap[i].x - ciw_2;
00453 int xend = dx+vision->rmap[i].width;
00454 for(; dx < xend; ++dx) {
00455
00456
00457 double y = ((double) dx)*x / cam_depth_wpix;
00458
00459
00460
00461 hm_cell hmc;
00462 hmc.height = 0.0;
00463 hmc.trav = 1.0;
00464 hmc.confidence = ALM_GPA_CONFIDENCE;
00465 hmc.color = COLOR_GREEN;
00466
00467 int index;
00468 if(xy2hm_index(x, y, index)) HM[index] = hmc;
00469
00470
00471 double az, alt, depth;
00472 xyz2neck_range(x, y, z, depth, az, alt);
00473 dm_cell dmc;
00474 dmc.depth = depth;
00475 dmc.confidence = ALM_GPA_CONFIDENCE;
00476 dmc.color = COLOR_GREEN;
00477
00478 if(angles2dm_index(az, alt, index)) DM[index] = dmc;
00479 }
00480 }
00481 #endif
00482 }
00483
00484
00485
00486
00487
00488 void ALM::stampHM(afsPose &pose)
00489 {
00490 #ifdef WANT_GLOBAL_MAP
00491
00492
00493
00494
00495
00496 double cosntheta = cos(-pose.theta);
00497 double sinntheta = sin(-pose.theta);
00498
00499 for(int i=0; i<HM_CELL_COUNT; ++i) {
00500 double lx, ly;
00501
00502 hm_index2xy(i, lx, ly);
00503
00504
00505 if((lx*lx + ly*ly) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00506
00507
00508 double gx = cosntheta*lx + sinntheta*ly + pose.x;
00509 double gy = -sinntheta*lx + cosntheta*ly + pose.y;
00510
00511 AGM::carryOver(gx, gy, HM[i]);
00512 }
00513 #endif
00514 }
00515
00516
00517
00518
00519
00520 void ALM::genRequests(MRvector &requests)
00521 {
00522
00523 MotionRequest request;
00524
00525 static const double d_az = ALM_DM_LEFT - ALM_DM_RIGHT;
00526 static const double d_alt = ALM_DM_TOP - ALM_DM_BOTTOM;
00527 static const double d_edge = 2*ALM_HM_SIZE;
00528
00529
00530
00531 {
00532 double dm_centers[ALM_DM_NUMCLUSTERS][2];
00533
00534 for(int i=0; i<ALM_DM_NUMCLUSTERS; ++i) {
00535 dm_centers[i][0] = ALM_DM_LEFT + (double) rand() * d_az / (double) RAND_MAX;
00536 dm_centers[i][1] = ALM_DM_TOP + (double) rand() * d_alt / (double) RAND_MAX;
00537 }
00538
00539
00540 for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00541
00542
00543 double popularity[ALM_DM_NUMCLUSTERS];
00544 for(int i=0; i<ALM_DM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00545
00546
00547 for(int i=0; i<DM_CELL_COUNT; ++i) {
00548 double az, alt, bestdist = HUGE_VAL;
00549 dm_index2angles(i, az, alt);
00550
00551
00552 for(int cindex=0; cindex<ALM_DM_NUMCLUSTERS; cindex++) {
00553 double raz, ralt, dist;
00554
00555 raz = az - dm_centers[cindex][0];
00556 ralt = alt - dm_centers[cindex][1];
00557 dist = raz*raz + ralt*ralt;
00558
00559
00560 if(dist < bestdist) { bestdist = dist; DM[i].cluster = cindex; }
00561 }
00562
00563
00564 popularity[DM[i].cluster] += 1 - DM[i].confidence;
00565 }
00566
00567
00568
00569 for(int cindex=0; cindex<ALM_DM_NUMCLUSTERS; cindex++) {
00570 dm_centers[cindex][0] = 0.0;
00571 dm_centers[cindex][1] = 0.0;
00572 }
00573
00574 for(int i=0; i<DM_CELL_COUNT; ++i) {
00575 double az, alt, uncertainty;
00576 dm_index2angles(i, az, alt);
00577 uncertainty = 1 - DM[i].confidence;
00578
00579 dm_centers[DM[i].cluster][0] +=
00580 az * uncertainty / popularity[DM[i].cluster];
00581 dm_centers[DM[i].cluster][1] +=
00582 alt * uncertainty / popularity[DM[i].cluster];
00583 }
00584 }
00585
00586
00587 request.type = MotionRequest::LOOK_AT;
00588 for(int cindex=0; cindex<ALM_DM_NUMCLUSTERS; cindex++) {
00589 request.azalt.azimuth = dm_centers[cindex][0];
00590 request.azalt.altitude = dm_centers[cindex][1];
00591 requests.push_back(request);
00592 }
00593 }
00594
00595
00596
00597
00598 #ifdef UNIT_TEST_ALM_MA
00599
00600
00601
00602 for(int i=0; i<HM_CELL_COUNT; ++i) HM[i].cluster = -1;
00603 #endif
00604
00605 {
00606 double hm_centers[ALM_HM_NUMCLUSTERS][2];
00607
00608 for(int i=0; i<ALM_HM_NUMCLUSTERS; ++i) {
00609 double x, y;
00610 do {
00611 x = -ALM_HM_SIZE + (double)rand() * d_edge / (double) RAND_MAX;
00612 y = -ALM_HM_SIZE + (double)rand() * d_edge / (double) RAND_MAX;
00613
00614 } while((x*x + y*y) > ALM_HM_RADIUS*ALM_HM_RADIUS);
00615 hm_centers[i][0] = x;
00616 hm_centers[i][1] = y;
00617 }
00618
00619
00620 for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00621
00622
00623 double popularity[ALM_HM_NUMCLUSTERS];
00624 for(int i=0; i<ALM_HM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00625
00626
00627 for(int i=0; i<HM_CELL_COUNT; ++i) {
00628 double x, y, bestdist = HUGE_VAL;
00629 hm_index2xy(i, x, y);
00630
00631 if((x*x + y*y) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00632
00633
00634 for(int cindex=0; cindex<ALM_HM_NUMCLUSTERS; cindex++) {
00635 double rx, ry, dist;
00636
00637 rx = x - hm_centers[cindex][0];
00638 ry = y - hm_centers[cindex][1];
00639 dist = rx*rx + ry*ry;
00640
00641
00642 if(dist < bestdist) { bestdist = dist; HM[i].cluster = cindex; }
00643 }
00644
00645
00646 popularity[HM[i].cluster] += 1 - HM[i].confidence;
00647 }
00648
00649
00650
00651 for(int cindex=0; cindex<ALM_HM_NUMCLUSTERS; cindex++) {
00652 hm_centers[cindex][0] = 0.0;
00653 hm_centers[cindex][1] = 0.0;
00654 }
00655
00656 for(int i=0; i<HM_CELL_COUNT; ++i) {
00657 double x, y, uncertainty;
00658 hm_index2xy(i, x, y);
00659
00660 if((x*x + y*y) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00661 uncertainty = 1 - HM[i].confidence;
00662
00663 hm_centers[HM[i].cluster][0] +=
00664 x * uncertainty / popularity[HM[i].cluster];
00665 hm_centers[HM[i].cluster][1] +=
00666 y * uncertainty / popularity[HM[i].cluster];
00667 }
00668 }
00669
00670
00671 request.type = MotionRequest::LOOK_DOWN_AT;
00672 for(int cindex=0; cindex<ALM_HM_NUMCLUSTERS; cindex++) {
00673 request.xy.x = hm_centers[cindex][0];
00674 request.xy.y = hm_centers[cindex][1];
00675 requests.push_back(request);
00676 }
00677 }
00678 }
00679
00680 void ALM::dumpDM(dmPicker &p, std::ostream &out)
00681 {
00682 for(int y=0; y<ALM_DM_V_SIZE; ++y) {
00683 int yi = y*ALM_DM_H_SIZE;
00684 out << p(DM[yi]);
00685 for(int x=1; x<ALM_DM_H_SIZE; ++x) out << '\t' << p(DM[++yi]);
00686 out << std::endl;
00687 }
00688 }
00689
00690 void ALM::dumpHM(hmPicker &p, std::ostream &out)
00691 {
00692 for(int y=0; y<2*ALM_HM_SIZE; ++y) {
00693 int yi = y*2*ALM_HM_SIZE;
00694 out << p(HM[yi]);
00695 for(int x=1; x<2*ALM_HM_SIZE; ++x) out << '\t' << p(HM[++yi]);
00696 out << std::endl;
00697 }
00698 }
00699
00700
00701
00702
00703
00704 dm_cell *ALM::getDM() { return DM; }
00705 hm_cell *ALM::getHM() { return HM; }
00706
00707
00708
00709
00710 #ifdef UNIT_TEST_ALM_MA
00711 int main(int argc, char **argv)
00712 {
00713 using namespace std;
00714
00715 cout << "Puppies!!" << endl;
00716
00717
00718
00719 srand(time(NULL));
00720 srand48(time(NULL));
00721
00722
00723 ALM::init();
00724
00725
00726 for(float t=ALM_DM_LEFT/2; t>ALM_DM_RIGHT/2;
00727 t += (ALM_DM_RIGHT-ALM_DM_LEFT)/2/1000)
00728 ALM::registerDepth(100, 0, t);
00729
00730
00731 cout << "Clustering..." << endl;
00732 MRvector v;
00733 ALM::genRequests(v);
00734
00735
00736 ALM::move(0, 0, 1.12, 1000);
00737
00738
00739 afsPose p;
00740 p.x = 0;
00741 p.y = 100;
00742 p.theta = 0;
00743 ALM::stampHM(p);
00744
00745
00746 hmPickHeight hp;
00747 hmPickTrav ht;
00748 hmPickConfidence hn;
00749 hmPickColor hc;
00750 hmPickCluster hl;
00751 dmPickDepth dd;
00752 dmPickConfidence dn;
00753 dmPickColor dc;
00754 dmPickCluster dl;
00755 ofstream gh("gHeight.mat"); AGM::dump(hp, gh); gh.close();
00756 ofstream ih("hHeight.mat"); ALM::dumpHM(hp, ih); ih.close();
00757 ofstream it("hTrav.mat"); ALM::dumpHM(ht, it); it.close();
00758 ofstream in("hConfidence.mat"); ALM::dumpHM(hn, in); in.close();
00759 ofstream ic("hColor.mat"); ALM::dumpHM(hc, ic); ic.close();
00760 ofstream il("hCluster.mat"); ALM::dumpHM(hl, il); il.close();
00761 ofstream jd("dDepth.mat"); ALM::dumpDM(dd, jd); jd.close();
00762 ofstream jn("dConfidence.mat"); ALM::dumpDM(dn, jn); jn.close();
00763 ofstream jc("dColor.mat"); ALM::dumpDM(dc, jc); jc.close();
00764 ofstream jl("dCluster.mat"); ALM::dumpDM(dl, jl); jl.close();
00765
00766 return 0;
00767 }
00768 #endif