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

almMain.cc

Go to the documentation of this file.
00001 /*
00002  * almMain.cc -- defines the main class for interfacing with the AIBO
00003  *   Local Map. See almMain.h for details.
00004  *
00005  * Started 12-13-2002, tss
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   // Defines for HM_CELL_COUNT, DM_CELL_COUNT, and GM_CELL_COUNT
00039   // have been moved to Configuration.h
00040 
00041   // Handy math constant: sqrt(2*M_PI)
00042 #define SQRT_2_PI 2.5066282746310002416
00043 
00044   // Here we keep the map data itself, which is declared as a global because
00045   // the AIBO has a buggy malloc, and anyway we only need one map. I know this
00046   // is bad, but I'll try to write the code so that it doesn't matter so much.
00047   //   Here is the horizonal height map. We actually have two so that we can
00048   // do motion updates with ease--a double buffering scheme, if you will. The
00049   // pointer HM will refer to the "current" height map
00050 hm_cell HMs[2][HM_CELL_COUNT];
00051 hm_cell *HM;
00052   //   Here is the spherical depth map. We actually have two so that we can
00053   // do motion updates with ease--a double buffering scheme, if you will. The
00054   // pointer DM will refer to the "current" depth map
00055 dm_cell DMs[2][DM_CELL_COUNT];
00056 dm_cell *DM;
00057 
00058   // This method initializes the static tables used to hold the map data.
00059   // Yes, you have to manually run an initializer. I apologize!
00060 void ALM::init(void)
00061 {
00062   // clear out both sets of maps and leave the first set as the current one
00063   // set #2
00064   DM = DMs[1];
00065   HM = HMs[1];
00066   nukeAndPaveCurrentMap();
00067   // set #1
00068   DM = DMs[0];
00069   HM = HMs[0];
00070   nukeAndPaveCurrentMap();
00071 }
00072 
00073   // This method applies motion to the maps, updating them so that they
00074   // remain egocentric and so that uncertain regions are duly noted.
00075   // The parameters are the same as those applied to the motion commands.
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   // Make the other set of maps the current map
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   // Clear out the current map. This clearing out does make some big
00092   // assumptions for us--whatever we don't fill in from the other map set
00093   // will be assumed to be level ground beneath us or walls out of range over
00094   // the horizon.
00095   nukeAndPaveCurrentMap();
00096 
00097   // We now find the relative motion that the robot underwent during this
00098   // maneuver
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   // Good. Now we construct transformation matricies we'll use to translate
00106   // and rotate the data in the maps according to that relative motion.
00107   //   For transforming the HM data, we use the following matrix, whose
00108   // entries correspond to local variables defined below:
00109   //     [  cosrtheta  sinrtheta  t13_or14 ]
00110   //     [ -sinrtheta  cosrtheta  t23_or24 ]
00111   //     [      0          0          1    ]
00112   //  For transforming the DM data, we use this matrix. Same deal with the
00113   // entries:
00114   //     [  cosrtheta  sinrtheta  0  t13_or14 ]
00115   //     [ -sinrtheta  cosrtheta  0  t23_or24 ]
00116   //     [      0          0      1      0    ]
00117   //     [      0          0      0      1    ]
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   // Now we actually do the transformation. Let's start with the height map.
00124   for(int index=0; index<HM_CELL_COUNT; ++index) {
00125     // Find the xy location of this cell
00126     double cell_x, cell_y;
00127     hm_index2xy(index, cell_x, cell_y);
00128 
00129     // Transform!
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     // Find the new index for this transformed location.
00134     // Make certain the index is in bounds
00135     int new_index;
00136     if(!xy2hm_index(new_cell_x, new_cell_y, new_index)) continue;
00137 
00138     // TODO--bring in GM data for this cell?
00139 
00140     // Check the certainty value at the new index. If it's higher than the
00141     // cell we're moving, then abort--that one is more likely to be correct.
00142     if(HM[new_index].confidence > old_HM[index].confidence) continue;
00143 
00144     // OK, we're good; copy the cell!
00145     HM[new_index] = old_HM[index];
00146 
00147     // Diminish the confidence value to reflect motion uncertainty
00148     HM[new_index].confidence *= ALM_HM_TAX;
00149   }
00150 
00151   // Now let's transform the depth map. It's basically the same deal as
00152   // with the height map.
00153   for(int index=0; index<DM_CELL_COUNT; ++index) {
00154     // If the depth of this cell is at or greater than IROORDIST, leave
00155     // it alone--there's effectively no data there, since whatever's off
00156     // on that radial is out of range
00157     if(old_DM[index].depth >= IROORDIST) continue;
00158 
00159     // Find the azimuth and altitude of this cell.
00160     double cell_az, cell_alt;
00161     dm_index2angles(index, cell_az, cell_alt);
00162 
00163     // Find the XYZ values for the azimuth, altitude, and depth of this cell
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     // Transform!
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     // z does not change!
00172 
00173     // Find the azimuth, altitude, and range of the new xyz values
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     // Constrain new cell depth to IROORDIST--we really probably shouldn't
00179     // rely on information that far out. We'll remain confident that it's
00180     // far away, though.
00181     if(new_cell_depth > IROORDIST) new_cell_depth = IROORDIST;
00182 
00183     // Now find the corresponding index
00184     // Make sure the index is in bounds.
00185     int new_index;
00186     if(!angles2dm_index(new_cell_az, new_cell_alt, new_index)) continue;
00187 
00188     // Check the depth values at the new index. If it's lower than the
00189     // depth of the cell we're moving, then abort--occlusion!
00190     if(DM[new_index].depth < new_cell_depth) continue;
00191 
00192     // OK, we're good, copy the cell and update the depth!
00193     DM[new_index] = old_DM[index];
00194     DM[new_index].depth = new_cell_depth;
00195 
00196     // Diminish the confidence value to reflect motion uncertainty
00197     DM[new_index].confidence = old_DM[index].confidence * ALM_DM_TAX;
00198   }
00199 }
00200 
00201   // This method clears the current maps to simulate an empty room: the ground
00202   // will appear in the proper place in the spherical map with the rest empty;
00203   // the height map will be zeroed everywhere. Actually, in addition to other
00204   // administrativa, init just calls this function.
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     // We have to specifically set the depth for each cell. If the cell is
00215     // above the horizon, we say that the depth is out of range. If it's
00216     // below the borizon, we plot out a flat plane at AIBO height.
00217     double azimuth, altitude;
00218 
00219     // Find angles for cell i.
00220     dm_index2angles(i, azimuth, altitude);
00221 
00222     // If altitude > 0, we're peering off into the distance.
00223     if(altitude > 0) {
00224       dmc.depth = IROORDIST; // really WorldState::IROORDist--see #define at top
00225 #ifndef UNIT_TEST
00226       dmc.color = COLOR_BLUE;
00227 #else
00228       dmc.color = BLUE; // wall of the pool
00229 #endif
00230     }
00231     // Otherwise we're staring at the ground. Find out how far away the
00232     // ground is, anyway.
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; // the ground
00238 #else
00239       dmc.color = GREEN; // the ground
00240 #endif
00241     }
00242 
00243     DM[i] = dmc;
00244   }
00245 
00246   hmc.height = 0.0;
00247   hmc.trav = 0.5;   // maybe traversable? maybe...
00248   hmc.confidence = 0.0;
00249 #ifndef UNIT_TEST
00250   hmc.color = COLOR_GREEN;  // assumption: grass as far as the eye can see
00251 #else
00252   hmc.color = GREEN;    // assumption: grass as far as the eye can see
00253 #endif
00254 
00255   for(i=0; i<HM_CELL_COUNT; ++i) HM[i] = hmc;
00256 }
00257 
00258   // This method registers a depth measurement in the local maps
00259 void ALM::registerDepth(double depth, double tilt, double pan)
00260 {
00261   registerDepth(depth, tilt, pan, 0);
00262 } 
00263 
00264   // This method does the same, but it allows the user to specify kludges
00265   // that can give "better" performance in certain limited environments.
00266   // See the WM2Kludge namespace definition in WorldModel2.h for details.
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   // The first thing we do is convert the AIBO's depth measurement to a
00275   // more accurate measurement based on calibration data. We only do this
00276   // if the measurement is in range. Fortunately, we seem to only need to
00277   // modify the measurement linearly.
00278   if(depth < IROORDIST)
00279     depth = AIBO_IR_CAL_MULTIPLIER*depth + AIBO_IR_CAL_OFFSET;
00280 
00281   // Turn head depth measurement into XYZ coordinates
00282   head_range2xyz(depth, pan, tilt, x, y, z);
00283 
00284   // If we're ignoring measurements with z < 0, then, well, ignore away.
00285   if((kludges & WM2Kludge::IgnoreZLessThanZero) && (z < 0.0)) return;
00286 
00287   // Turn XYZ coordinates into neck depth measurement
00288   xyz2neck_range(x, y, z, n_depth, n_azimuth, n_altitude);
00289 
00290   // Find out how many pixels away from center of the depth measurement on
00291   // the height map we'll be filling in points. Two standard deviations
00292   // ought to be plenty.
00293   int splat_width = (int)(n_depth * 2.0*ALM_IR_SPLAT_STDDEV);
00294 
00295   // Find DM array index for depth measurement. Skip if this measurement is
00296   // out of DM bounds. TODO--not optimal, since some measurements that are
00297   // no good for the DM may be good for the HM.
00298   if(!angles2dm_index(n_azimuth, n_altitude, index)) return;
00299 
00300   // Find a color for this cell
00301 #ifndef UNIT_TEST_ALM_MA
00302   // TODO-- get rid of this temporary, inefficient center code, replace with
00303   // something in vision.c that gets the color of the central image cell.
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   // If we're ignoring measurements of green objects, just quit now.
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   // Fill in depth measurements in "splat" area around the depth measurement
00324   for(int dx=-splat_width; dx<=splat_width; ++dx) {
00325     for(int dy=-splat_width; dy<=splat_width; ++dy) {
00326       // Compute distance from this index to the center of the splat
00327       double splat_var = (double)(dx*dx + dy*dy);
00328 
00329       // Ignore any dx or dy greater than two standard deviations away from
00330       // the center of the splat
00331       if(splat_var > 4.0*(double)(splat_width*splat_width)) continue;
00332 
00333       // Make certain the dx won't wrap over to the other side of the
00334       // array
00335       int x_check = (index % ALM_DM_H_SIZE) + dx;
00336       if((x_check < 0) || (x_check >= ALM_DM_H_SIZE)) continue;
00337 
00338       // Which part of the splat are we in?
00339       int splat_index = index + dy*ALM_DM_H_SIZE + dx;
00340       // Make certain we're not going out of array bounds
00341       if((splat_index < 0) || (splat_index >= DM_CELL_COUNT)) continue;
00342       // What's the confidence value for this part of the splat?
00343       // It's whatever this Gaussian says it is.
00344       double splat_confidence =
00345   1/((double)splat_width*SQRT_2_PI)*exp(-splat_var/2.0*(double)splat_var);
00346 
00347       // Compute azimuth, altitude, X, Y, and Z for this cell index. We don't
00348       // need this for the DM portion of the splat functionality, but we do
00349       // need it for the HM part. We're just computing it here to see if any
00350       // of the kludges apply.
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       // See if a kludge mandates us ignoring this part of the splat because it
00357       // corresponds to a z value less than 0
00358       if((kludges & WM2Kludge::IgnoreZLessThanZero) && (splat_z < 0.0))
00359   continue;
00360 
00361       // Start with DM. Make sure this part of the splat is in bounds first
00362       if((splat_index >= 0) && (splat_index < DM_CELL_COUNT))
00363   // Only modify this place if we're confident about our measurement
00364   // being better
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       // We now apply the splat to the horizontal height map. We use the
00372       // X, Y, and Z values we computed previously. Please note the reuse
00373       // of the splat_index variable.
00374       // Only work if this part of the splat is in bounds
00375       if(xy2hm_index(splat_x, splat_y, splat_index)) {
00376   // Only modify this place if we're confident about our measurement
00377   // being better
00378   if(splat_confidence > HM[splat_index].confidence) {
00379     HM[splat_index].height  = splat_z;
00380     HM[splat_index].confidence  = splat_confidence;
00381     // TODO -- come up with a more intelligent way of setting
00382     // traversability than this
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   // TODO--replace with intelligent sampling
00394 }
00395 
00396   // This method uses the ground plane assumption to fill the local maps
00397   // with information about the location of the ground based on the location
00398   // of the head and the image coming from the camera. Head and image
00399   // information are obtained from WorldState, etc.
00400 
00401   // For now we just assume that the AIBO is staring straight ahead, a
00402   // condition checked for by the main WorldModel2 code. This is
00403   // a cop-out, but it gives us a starting place.
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; // curr_img_* / 2
00412   static int cih_2 = 0;
00413   static double z = AIBO_TILT_PIVOT_HEIGHT+AIBO_NECK_HEIGHT;
00414 
00415   // To do ground plane assumption calculations, we assume (naively, perhaps)
00416   // that the AIBO's camera is like a pinhole camera. This may be a safe-ish
00417   // assumption to make since the FOV is so narrow. Anyway, in any pinhole
00418   // camera the distance of the film to the pinhole determines the FOV of
00419   // that particular piece of film--the smaller the distance, the bigger
00420   // the FOV (try drawing it if you're having trouble seeing it).
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     // now, we need to know the distance of the "film" here in the AIBO's
00429     // camera system. We measure this depth in pixels instead of actual
00430     // physical units, since pixels can be scaled. To compute the depth,
00431     // simply do (img_width/2)*tan(horiz_angle_of_view/2) or the analogous
00432     // computation for image height.
00433     cam_depth_wpix = ((double) ciw_2)*tan(AIBO_CAM_H_FOV/2);
00434     // So why do we compute it for both? It turns out that the AIBO's camera
00435     // doesn't quite have square pixels, at least if we assume it's a pinhole
00436     // camera. So the depth in horizontal pixels is slightly different from the
00437     // depth in vertical pixels.
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; // y distance from center of image
00443       // Skip if this run is on or above the horizon
00444     if(dy <= 0) continue;
00445       // Skip unless this run is green
00446     if(vision->rmap[i].color != COLOR_GREEN) continue;
00447 
00448       // Find the x distance value for this scanline
00449       // NOTE: this x is a quite different x from the x in dx!
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       // Find the y value for this pixel
00456       // NOTE: this y is a quite different y from the y in dy
00457       double y = ((double) dx)*x / cam_depth_wpix;
00458 
00459       // OK, we've got x, y, and z for this pixel. Now to use it.
00460       // Fill in height map information
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       // Fill in depth map information
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   // This method copies the information from the HM onto the global map
00485   // (or, more poetically, turns the HM into a rubber stamp).
00486   // You need to furnish it with the current pose information, which you
00487   // get from FastSLAM
00488 void ALM::stampHM(afsPose &pose)
00489 {
00490 #ifdef WANT_GLOBAL_MAP
00491   // For transforming the HM data from egocentric to allocentric coordinates,
00492   // we use this matrix
00493   //     [  cosntheta  sinntheta  pose.x ]
00494   //     [ -sinntheta  cosntheta  pose.y ]
00495   //     [      0          0         1   ]
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       // make certain we're in the circle guaranteed to contain valid
00504       // local HM data
00505     if((lx*lx + ly*ly) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00506 
00507       // we're in. transform x and y values to global coordinates.
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   // This method generates motion requests for WorldModel2. It's the reason
00517   // WorldModel2 is our friend.
00518   // For the time being, the method used here is K-means clustering.
00519   // Hope you like loops!
00520 void ALM::genRequests(MRvector &requests)
00521 {
00522     // useful storage space
00523   MotionRequest request;
00524     // find handy constants
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     ////////////////// Do K-means for the DM first
00530     //////////////////
00531   { // BEGIN of enclosing block, not indented to preserve screen space
00532   double dm_centers[ALM_DM_NUMCLUSTERS][2]; // would use new, but bad malloc
00533     // Randomly distribute centers through DM
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     // The actual K-means bit
00540   for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00541       // Array keeps track of how popular each cluster center is in terms
00542       // of member uncertainty
00543     double popularity[ALM_DM_NUMCLUSTERS];
00544     for(int i=0; i<ALM_DM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00545 
00546       // Tag all of the points as belonging to a specific cluster
00547     for(int i=0; i<DM_CELL_COUNT; ++i) {
00548       double az, alt, bestdist = HUGE_VAL;
00549       dm_index2angles(i, az, alt);  // get az and alt for this index
00550 
00551   // Find out which cluster center is best for this point
00552       for(int cindex=0; cindex<ALM_DM_NUMCLUSTERS; cindex++) {
00553   double raz, ralt, dist;
00554 
00555   raz  = az  - dm_centers[cindex][0]; // get relative azimuth
00556   ralt = alt - dm_centers[cindex][1]; // get relative altitude
00557         dist = raz*raz + ralt*ralt;         // get relative distance squared
00558 
00559     // is this point the best seen yet?
00560   if(dist < bestdist) { bestdist = dist; DM[i].cluster = cindex; }
00561       }
00562   // increment chosen cluster center popularity--more uncertain
00563   // members mean greater popularity!
00564       popularity[DM[i].cluster] += 1 - DM[i].confidence;
00565     }
00566 
00567       // Now shift the cluster centers to the weighted center of their clusters.
00568       // Start by resetting everything to 0
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       // Now add the influence of all the members of the DM
00574     for(int i=0; i<DM_CELL_COUNT; ++i) {
00575       double az, alt, uncertainty;
00576       dm_index2angles(i, az, alt);  // get az and alt for this index
00577       uncertainty = 1 - DM[i].confidence; // get uncertainty for this index
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     // K-means is finished for the DM. Store results in the requests vector
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   } // END of enclosing block
00594 
00595 
00596     ////////////////// Do K-means for the HM next
00597     //////////////////
00598 #ifdef UNIT_TEST_ALM_MA
00599   // explicitly clean out HM cluster assignments first to get rid of
00600   // bad values outside of ALM_HM_RADIUS. Only need to do this for the
00601   // sake of frazzled unit testers--everyone else is on their own!
00602   for(int i=0; i<HM_CELL_COUNT; ++i) HM[i].cluster = -1;
00603 #endif
00604 
00605   { // BEGIN of enclosing block, not indented to preserve screen space
00606   double hm_centers[ALM_HM_NUMCLUSTERS][2]; // would use new, but bad malloc
00607     // Randomly distribute centers through HM
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       // constrain to circle. there are better ways to do this.
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     // The actual K-means bit
00620   for(int iteration=0; iteration<AM_KMEANS_ITERATIONS; ++iteration) {
00621       // Array keeps track of how popular each cluster center is in terms
00622       // of member uncertainty
00623     double popularity[ALM_HM_NUMCLUSTERS];
00624     for(int i=0; i<ALM_HM_NUMCLUSTERS; ++i) popularity[i] = 0.0;
00625 
00626       // Tag all of the points as belonging to a specific cluster
00627     for(int i=0; i<HM_CELL_COUNT; ++i) {
00628       double x, y, bestdist = HUGE_VAL;
00629       hm_index2xy(i, x, y); // get x and y for this index
00630         // ignore points outside of circle
00631       if((x*x + y*y) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00632 
00633   // Find out which cluster center is best for this point
00634       for(int cindex=0; cindex<ALM_HM_NUMCLUSTERS; cindex++) {
00635   double rx, ry, dist;
00636 
00637   rx   = x - hm_centers[cindex][0]; // get relative x value
00638   ry   = y - hm_centers[cindex][1]; // get relative y value
00639         dist = rx*rx + ry*ry;             // get relative distance squared
00640 
00641     // is this point the best seen yet?
00642   if(dist < bestdist) { bestdist = dist; HM[i].cluster = cindex; }
00643       }
00644   // increment chosen cluster center popularity--more uncertain
00645   // members mean greater popularity!
00646       popularity[HM[i].cluster] += 1 - HM[i].confidence;
00647     }
00648 
00649       // Now shift the cluster centers to the weighted center of their clusters.
00650       // Start by resetting everything to 0
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       // Now add the influence of all the members of the HM
00656     for(int i=0; i<HM_CELL_COUNT; ++i) {
00657       double x, y, uncertainty;
00658       hm_index2xy(i, x, y); // get x and y for this index
00659         // ignore points outside of circle
00660       if((x*x + y*y) > ALM_HM_RADIUS*ALM_HM_RADIUS) continue;
00661       uncertainty = 1 - HM[i].confidence; // get uncertainty for this index
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     // K-means is finished for the HM. Store results in the requests vector
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   } // END of enclosing block
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   // These methods provide direct access to the local map data arrays. You
00701   // should use DM_CELL_COUNT and HM_CELL_COUNT to index these arrays.
00702   // These methods are PRIVATE and are intended only for use by WorldModel2
00703   // objects.
00704 dm_cell *ALM::getDM() { return DM; }
00705 hm_cell *ALM::getHM() { return HM; }
00706 
00707 
00708   // What this thing does with all this information remains to be seen...
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   /* Now that we've got that out of our system... */
00717 
00718   // 10 RANDOMIZE TIMER
00719   srand(time(NULL));
00720   srand48(time(NULL));
00721 
00722   /* ignition! */
00723   ALM::init();
00724 
00725   /* try some depth measurements */
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   // cluster!
00731   cout << "Clustering..." << endl;
00732   MRvector v;
00733   ALM::genRequests(v);
00734 
00735   //ALM::move(0, 50, 0, 1000);
00736   ALM::move(0, 0, 1.12, 1000);
00737 
00738   // Stamp it onto the HM
00739   afsPose p;
00740   p.x = 0;
00741   p.y = 100;
00742   p.theta = 0;
00743   ALM::stampHM(p);
00744 
00745   // DUMP
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

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