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

almUtility.cc

Go to the documentation of this file.
00001 /*
00002  * almUtility.cc -- contains utility routines for the AIBO LocalMap system.
00003  *
00004  * Started on 12/16/2002 by tss
00005  */
00006 
00007 #include "Configuration.h"
00008 
00009 #include <cmath>
00010 
00011 #ifdef UNIT_TEST_ALM_UT
00012 #warning Compiling unit test code for almUtility.cc
00013 #include <iostream>
00014 #endif
00015 
00016 /* This routine takes a grid cell index in the spherical depth map and returns
00017  * the corresponding azimuth and altitude angles. */
00018 void dm_index2angles(int index, double &azimuth, double &altitude)
00019 {
00020   static const int width  = ALM_DM_H_SIZE;
00021   static const int height = ALM_DM_V_SIZE;
00022 
00023   static const double xsize = ALM_DM_RIGHT - ALM_DM_LEFT;
00024   static const double ysize = ALM_DM_BOTTOM -ALM_DM_TOP;
00025 
00026   azimuth = ALM_DM_LEFT + xsize * (double)(index % width) / (double)width;
00027   altitude = ALM_DM_TOP + ysize * (double)(index / width) / (double)height;
00028 }
00029 
00030 /* This routine does the opposite of the routine above */
00031 bool angles2dm_index(double azimuth, double altitude, int &index)
00032 {
00033   static const int width  = ALM_DM_H_SIZE;
00034   static const int height = ALM_DM_V_SIZE;
00035 
00036   static const double xsize = ALM_DM_RIGHT - ALM_DM_LEFT;
00037   static const double ysize = ALM_DM_BOTTOM -ALM_DM_TOP;
00038 
00039   double dx = azimuth - ALM_DM_LEFT;
00040   double dy = altitude - ALM_DM_TOP;
00041 
00042   int xindex = (int) ((dx / xsize) * (double) width);
00043   int yindex = (int) ((dy / ysize) * (double) height);
00044 
00045   if((xindex < 0) || (yindex < 0)) return false;
00046   if((xindex >= width) || (yindex >= height)) return false;
00047 
00048   index = yindex*width + xindex;
00049   return true;
00050 }
00051 
00052 /* This routine converts an index into the horizontal height map and returns
00053  * the corresponding X and Y locations relative to the AIBO neck tilt
00054  * pivot */
00055 void hm_index2xy(int index, double &x, double &y)
00056 {
00057   static const int width  = 2*ALM_HM_SIZE;
00058   static const int height = 2*ALM_HM_SIZE;
00059 
00060   static const double xsize =  2*ALM_HM_RADIUS;
00061   static const double ysize = -2*ALM_HM_RADIUS;
00062 
00063   x = (double) -ALM_HM_RADIUS +
00064   xsize * (double)(index % width) / (double)width;
00065   y = (double)  ALM_HM_RADIUS +
00066   ysize * (double)(index / width) / (double)height;
00067 }
00068 
00069 /* This routine does the opposite of the routine above. It is the user's
00070  * responsibility to make certain the index is in bounds. */
00071 bool xy2hm_index(double x, double y, int &index)
00072 {
00073   static const int width  = 2*ALM_HM_SIZE;
00074   static const int height = 2*ALM_HM_SIZE;
00075 
00076   static const double xsize =  2*ALM_HM_RADIUS;
00077   static const double ysize = -2*ALM_HM_RADIUS;
00078 
00079   double dx = x - -ALM_HM_RADIUS;
00080   double dy = y -  ALM_HM_RADIUS;
00081 
00082   int xindex = (int) ((dx / xsize) * (double) width);
00083   int yindex = (int) ((dy / ysize) * (double) height);
00084 
00085   if((xindex < 0) || (yindex < 0)) return false;
00086   if((xindex >= width) || (yindex >= height)) return false;
00087 
00088   index = yindex*width + xindex;
00089   return true;
00090 }
00091 
00092 /* This routine converts an index into the global height map and returns
00093  * the corresponding allocentric X and Y locations. */
00094 void gm_index2xy(int index, double &x, double &y)
00095 {
00096   static const int width  = AGM_H_SIZE;
00097   static const int height = AGM_V_SIZE;
00098 
00099   static const double xsize = (double) AGM_RIGHT - (double) AGM_LEFT;
00100   static const double ysize = (double) AGM_BOTTOM - (double) AGM_TOP;
00101 
00102   x = (double) AGM_LEFT + xsize * (double)(index % width) / (double)width;
00103   y = (double) AGM_TOP  + ysize * (double)(index / width) / (double)height;
00104 }
00105 
00106 /* This routine does the opposite of the routine above. It is the user's
00107  * responsibility to make certain the index is in bounds. */
00108 bool xy2gm_index(double x, double y, int &index)
00109 {
00110   static const int width  = AGM_H_SIZE;
00111   static const int height = AGM_V_SIZE;
00112 
00113   static const double xsize = (double) AGM_RIGHT - (double) AGM_LEFT;
00114   static const double ysize = (double) AGM_BOTTOM - (double) AGM_TOP;
00115 
00116   double dx = x - AGM_LEFT;
00117   double dy = y - AGM_TOP;
00118 
00119   int xindex = (int) ((dx / xsize) * (double) width);
00120   int yindex = (int) ((dy / ysize) * (double) height);
00121 
00122   if((xindex < 0) || (yindex < 0)) return false;
00123   if((xindex >= width) || (yindex >= height)) return false;
00124 
00125   index = yindex*width + xindex;
00126   return true;
00127 }
00128 
00129 /* This routine takes a depth measurement and the pan/tilt angles of the
00130  * head and returns the 3D XYZ position of the measured point. The
00131  * XYZ coordinate system has its origin on the ground just beneath the
00132  * center of AIBO's tilt pivot point. The X axis points out in front of
00133  * the AIBO, the Y axis leftwards, and the Z axis straight up. */
00134 void head_range2xyz(double depth, double pan, double tilt,
00135         double &x, double &y, double &z)
00136 {
00137   // To accomplish this task, we need a transformation matrix. The local
00138   // variables in this function correspond to the following transformation
00139   // matrix locations:
00140   //   [    t11      t12    sinntilt  t14 ]
00141   //   [ -sinnpan  cosnpan      0     t24 ]
00142   //   [    t31      t32    cosntilt  t34 ]
00143   //   [     0        0         0      1  ]
00144   // Since the vector for the distance of the obstacle from the head is
00145   // just of the form [ depth 0 0 1 ] in AIBO "xyz" head coordinates,
00146   // we don't need to compute the values for the inner two columns of the
00147   // matrix. So we ignore t12 and t32.
00148 
00149   double sinntilt = sin(-tilt);
00150   double cosntilt = cos(-tilt);
00151   double sinnpan  = sin(-pan);
00152   double cosnpan  = cos(-pan);
00153 
00154   double t11 = cosntilt*cosnpan;
00155   double t31 = -sinntilt*cosnpan;
00156   double t14 = AIBO_HEAD_LENGTH*cosntilt*cosnpan + AIBO_NECK_HEIGHT*sinntilt;
00157   double t24 = -AIBO_HEAD_LENGTH*sinnpan;
00158   double t34 = -AIBO_HEAD_LENGTH*sinntilt*cosnpan + AIBO_NECK_HEIGHT*cosntilt +
00159     AIBO_TILT_PIVOT_HEIGHT; // TODO: s/ATPH/motion model/
00160 
00161   x = t11*depth + t14;
00162   y = -sinnpan*depth + t24;
00163   z = t31*depth + t34;
00164 }
00165 
00166 /* This routine takes a location in the 3D XYZ coordinates mentioned above
00167  * and computes the range, azimuth, and altitude values for the location
00168  * from the center of the AIBO's tilt pivot point. */
00169 void xyz2neck_range(double x, double y, double z,
00170         double &depth, double &azimuth, double &altitude)
00171 {
00172   double dz = z - AIBO_TILT_PIVOT_HEIGHT; // TODO -- replace w/ motion model
00173 
00174   depth = sqrt(x*x + y*y + dz*dz);
00175   azimuth = atan2(y, x);
00176   altitude = asin(dz/depth);
00177 }
00178 
00179 /* This routine does the reverse of xyz2neck_range. Not much more to say
00180  * about that */
00181 void neck_range2xyz(double depth, double azimuth, double altitude,
00182         double &x, double &y, double &z)
00183 {
00184   // To accomplish this task, we need a transformation matrix. The local
00185   // variables in this function correspond to the following transformation
00186   // matrix locations:
00187   //   [    t11      t12   sinnalt            0            ]
00188   //   [ -sinnaz   cosnaz     0               0            ]
00189   //   [    t31      t32   cosnalt  AIBO_TILT_PIVOT_HEIGHT ]
00190   //   [     0        0       0               1            ]
00191   // Since the vector for the distance of the obstacle from the head is
00192   // just of the form [ depth 0 0 1 ] in AIBO "xyz" head coordinates,
00193   // we don't need to compute the values for the inner two columns of the
00194   // matrix. So we ignore t13 and t23.
00195 
00196   double sinnaz  = sin(-azimuth);
00197   double cosnaz  = cos(-azimuth);
00198   double sinnalt = sin(-altitude);
00199   double cosnalt = cos(-altitude);
00200 
00201   double t11 = cosnalt*cosnaz;
00202   double t31 = -sinnalt*cosnaz;
00203 
00204   x = t11*depth;
00205   y = -sinnaz*depth;
00206   z = t31*depth + AIBO_TILT_PIVOT_HEIGHT;
00207 }
00208 
00209 #ifdef UNIT_TEST_ALM_UT
00210 
00211 int main(int argc, char **argv)
00212 {
00213   int index;
00214   double az, alt, depth, x, y, z;
00215 
00216   using namespace std;
00217 
00218   cout << "   Mismatches of 1 or of " << ALM_DM_H_SIZE << "+-1 are probably OK"
00219        << endl;
00220   for(int i=0;
00221       i < ALM_DM_V_SIZE*ALM_DM_H_SIZE;
00222       i += ALM_DM_V_SIZE*ALM_DM_H_SIZE/13) {
00223     cout << "dm_index2angles(" << i <<", az, alt)";
00224     dm_index2angles(i, az, alt);
00225     cout << "\taz = " << az << "\talt = " << alt << endl;
00226 
00227     cout << "angles2dm_index(az, alt, index)";
00228     angles2dm_index(az, alt, index);
00229     cout << "\tindex = " << index;
00230 
00231     if(index != i) cout << "\t***MISMATCH***";
00232 
00233     cout << endl;
00234   }
00235   cout << endl << endl;
00236 
00237   for(int i=0;
00238       i < 4*ALM_HM_SIZE*ALM_HM_SIZE;
00239       i += 4*ALM_HM_SIZE*ALM_HM_SIZE/13) {
00240     cout << "hm_index2xy(" << i <<", x, y)";
00241     hm_index2xy(i, x, y);
00242     cout << "\tx = " << x << "\ty = " << y << endl;
00243 
00244     cout << "xy2hm_index(x, y, index)";
00245     xy2hm_index(x, y, index);
00246     cout << "\tindex = " << index;
00247 
00248     if(index != i) cout << "\t***MISMATCH***";
00249 
00250     cout << endl;
00251   }
00252   cout << endl << endl;
00253 
00254   for(int i=0;
00255       i < AGM_V_SIZE*AGM_H_SIZE;
00256       i += AGM_V_SIZE*AGM_H_SIZE/13) {
00257     cout << "gm_index2xy(" << i <<", x, y)";
00258     gm_index2xy(i, x, y);
00259     cout << "\tx = " << x << "\ty = " << y << endl;
00260 
00261     cout << "xy2gm_index(x, y, index)";
00262     xy2gm_index(x, y, index);
00263     cout << "\tindex = " << index;
00264 
00265     if(index != i) cout << "\t***MISMATCH***";
00266 
00267     cout << endl;
00268   }
00269   cout << endl << endl;
00270 
00271   cout << "head_range2xyz(200, 0, 0, x, y, z)";
00272   head_range2xyz(200, 0, 0, x, y, z);
00273   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00274   cout << "head_range2xyz(200, R(45), 0, x, y, z)";
00275   head_range2xyz(200, R(45), 0, x, y, z);
00276   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00277   cout << "head_range2xyz(200, 0, R(45), x, y, z)";
00278   head_range2xyz(200, 0, R(45), x, y, z);
00279   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00280   cout << "head_range2xyz(200, R(45), R(45), x, y, z)";
00281   head_range2xyz(200, R(45), R(45), x, y, z);
00282   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00283   cout << endl << endl;
00284 
00285   cout << "neck_range2xyz(200, 0, 0, x, y, z)";
00286   neck_range2xyz(200, 0, 0, x, y, z);
00287   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00288   cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00289   xyz2neck_range(x, y, z, depth, az, alt);
00290   cout << "\tdepth = " << depth << "  az = " << az << "  alt = " << alt << endl;
00291   cout << "neck_range2xyz(200, R(45), 0, x, y, z)";
00292   neck_range2xyz(200, R(45), 0, x, y, z);
00293   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00294   cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00295   xyz2neck_range(x, y, z, depth, az, alt);
00296   cout << "\tdepth = " << depth << "  az = " << az << "  alt = " << alt << endl;
00297   cout << "neck_range2xyz(200, 0, R(45), x, y, z)";
00298   neck_range2xyz(200, 0, R(45), x, y, z);
00299   cout << "\tx = " << x << "  y = " << y << "  z = " << z << endl;
00300   cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00301   xyz2neck_range(x, y, z, depth, az, alt);
00302   cout << "\tdepth = " << depth << "  az = " << az << "  alt = " << alt << endl;
00303 
00304   return 0;
00305 }
00306 
00307 #endif

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