Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/GT2005SelfLocator/GT2005SampleTemplateGenerator.cpp

Go to the documentation of this file.

    00001 #include "GT2005SampleTemplateGenerator.h"
    00002
    00003
    00004 /*
    00005 * Template generation for the gt2005 SelfLocator, completely
    00006 * identical to the gt2004 SelfLocators template generation
    00007 *
    00008 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00009 * @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    00010 * @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
    00011 */
    00012
    00013
    00014 GT2005SampleTemplateGenerator::GT2005SampleTemplateGenerator(const GT2005SelfLocatorParameters& parameters) :
    00015   parameters(parameters)
    00016 {
    00017   numberOfTemplates = 0;
    00018   numberOfFlags = 0;
    00019   nextTemplate = 0;
    00020   randomFactor = 0;
    00021 }
    00022
    00023
    00024 /**
    00025 * this method returns a generated template
    00026 * TODO: declare constant value as constants to modify during runtime
    00027 */
    00028 GT2005SelfLocatorSample GT2005SampleTemplateGenerator::getTemplate()
    00029 {
    00030   if(nextTemplate >= numberOfTemplates)
    00031   {
    00032     nextTemplate = 0;
    00033     ++randomFactor;
    00034   }
    00035
    00036   while(nextTemplate < numberOfTemplates)
    00037   {
    00038     const Pose2D& t = templates[nextTemplate++];
    00039
    00040     // if the template is inside the field, we put it to this position +/- random value
    00041     if(field.isInside(t.translation))
    00042     {
    00043       return GT2005SelfLocatorSample(Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
    00044                                                                   t.translation.x + randomFactor * 50),
    00045                                                     Range<double>(t.translation.y - randomFactor * 50,
    00046                                                                   t.translation.y + randomFactor * 50),
    00047                                                     Range<double>(t.getAngle() - randomFactor * 0.1,
    00048                                                                   t.getAngle() + randomFactor * 0.1)));
    00049     }
    00050   }
    00051   return GT2005SelfLocatorSample(field.randomPose());
    00052 }
    00053
    00054 /*
    00055 * this method generates a robot pose from 3 bearings to known mark0,1,2
    00056 */
    00057 bool GT2005SampleTemplateGenerator::poseFromBearings(double dir0,double dir1,double dir2,
    00058                     const Vector2<double>& mark0,
    00059                     const Vector2<double>& mark1,
    00060                     const Vector2<double>& mark2,
    00061                     const Vector2<double>& cameraOffset,
    00062                     Pose2D& resultingPose)
    00063 {
    00064   double w1 = dir1 - dir0,
    00065          w2 = dir2 - dir1;
    00066
    00067   if(sin(w1) != 0 && sin(w2) != 0)
    00068   {
    00069     Vector2<double> p = mark2 - mark1;
    00070     double a2 = p.abs();
    00071     p = (Pose2D(mark0) - Pose2D(atan2(p.y,p.x),mark1)).translation; // rotates the vec 1->0 arount 1 by - atan(py,px)
    00072     double a1 = p.abs();
    00073     if(a1 > 0 && a2 > 0)
    00074     {
    00075       double w3 = pi2 - w1 - w2 - atan2(p.y,p.x);
    00076       double t = a1 * sin(w2) / (a2 * sin(w1)) + cos(w3);
    00077       if(t != 0)
    00078       {
    00079         double w = atan(sin(w3) / t);
    00080         double b = a1 * sin(w) / sin(w1);
    00081         w = pi - w1 - w;
    00082         p = mark0 - mark1;
    00083         resultingPose = Pose2D(atan2(p.y,p.x) - w,mark1)
    00084                         + Pose2D(Vector2<double>(b,0));
    00085         p = (Pose2D(mark1) - resultingPose).translation;
    00086         resultingPose += Pose2D(atan2(p.y,p.x) - dir1);
    00087         resultingPose += Pose2D(Vector2<double>(-cameraOffset.x,
    00088                                                 -cameraOffset.y));
    00089         if (!checkTemplatePose(resultingPose, mark0, dir0) ||
    00090             !checkTemplatePose(resultingPose, mark1, dir1) ||
    00091             !checkTemplatePose(resultingPose, mark2, dir2))
    00092           return 0;
    00093         return true;
    00094       }
    00095     }
    00096   }
    00097   return false;
    00098 }
    00099
    00100 /*
    00101 * this method generates up to 2 possible poses from two marks seen and the distance
    00102 * to "mark1"
    00103 */
    00104 int GT2005SampleTemplateGenerator::poseFromBearingsAndDistance(
    00105   double dir0,double dir1,double dist, // dir0, dir1 seen angles to the marks, dist= distance to ,mark1
    00106   const Vector2<double>& mark0,   // mark0,1 = mark0/1 in absolute coordinates
    00107   const Vector2<double>& mark1,
    00108   const Vector2<double>& cameraOffset,
    00109   Pose2D& resultingPose1,
    00110   Pose2D& resultingPose2)
    00111   {
    00112     double alpha = dir0 - dir1;         // seen angle  between marks
    00113     Vector2<double> diff = mark0 - mark1;   // deltavector between marks in absolute coords pointing from 1 to 0
    00114     double dir10 = atan2(diff.y,diff.x);    // angle at mark1 to absolute coordinate system pointing to mark0
    00115     double f = dist * sin(alpha) / diff.abs();  // angle at dist*sin(alpha) is 90deg, f = sin(gamma) angle at mark0
    00116     if(fabs(f) <= 1)              // valid length relations?
    00117     {
    00118       double gamma = asin(f),         // gamma = asin(f) and beta (winkel at mark1) from angles sum
    00119         beta = pi - alpha - gamma;
    00120
    00121       resultingPose1 = Pose2D(dir10 + beta,mark1) +         // if alpha>pi/2 dir10+beta points to robot postion
    00122               Pose2D(pi - dir1,Vector2<double>(dist,0)) +     // this pose is always inserted
    00123               Pose2D(Vector2<double>(-cameraOffset.x,
    00124                   -cameraOffset.y));
    00125       if (!checkTemplatePose(resultingPose1, mark0, dir0) ||
    00126           !checkTemplatePose(resultingPose1, mark1, dir1))
    00127         return 0;
    00128
    00129       if(fabs(alpha) < pi_2 || fabs(alpha) > 3 * pi_2)
    00130       {                               // if alpha<pi/2 gamma might be >pi/2 so this solution might be correct
    00131       if(gamma > 0)
    00132         gamma = pi - gamma;
    00133       else
    00134         gamma = -pi - gamma;
    00135       beta = pi - alpha - gamma;
    00136       resultingPose2 = Pose2D(dir10 + beta,mark1) +
    00137               Pose2D(pi - dir1,Vector2<double>(dist,0)) +
    00138               Pose2D(Vector2<double>(-cameraOffset.x,
    00139                   -cameraOffset.y));
    00140       if (!checkTemplatePose(resultingPose2, mark0, dir0) ||
    00141           !checkTemplatePose(resultingPose2, mark1, dir1))
    00142         return 0;
    00143
    00144       return 2;
    00145       }
    00146       else
    00147       return 1;
    00148     }
    00149     else
    00150       return 0;
    00151 }
    00152
    00153
    00154 bool GT2005SampleTemplateGenerator::getBearing(const LandmarksPercept& landmarksPercept,int i,
    00155                 Vector2<double>& mark,
    00156                 double& dir,double& dist)
    00157 {
    00158   if(i < numberOfFlags)
    00159   {
    00160     const Flag& flag = flags[i];
    00161     mark = flag.position;
    00162     dir = flag.angle;
    00163     dist = flag.distance;
    00164     return true;
    00165   }
    00166   else
    00167   {
    00168     i -= numberOfFlags;
    00169     const Goal& goal = landmarksPercept.goals[i >> 1];
    00170     if((i & 1) == 0 && !goal.isOnBorder(goal.x.min))
    00171     {
    00172       mark = goal.rightPost;
    00173       dir = goal.x.min;
    00174       dist = goal.distance;
    00175       return true;
    00176     }
    00177     else if((i & 1) == 1 && !goal.isOnBorder(goal.x.max))
    00178     {
    00179       mark = goal.leftPost;
    00180       dir = goal.x.max;
    00181       dist = goal.distance;
    00182       return true;
    00183     }
    00184     else
    00185       return false;
    00186   }
    00187 }
    00188
    00189 /*
    00190 * replaces the flag of the current type in the flags buffer
    00191 *
    00192 */
    00193 void GT2005SampleTemplateGenerator::addFlag(const Flag& flag)
    00194 {
    00195   if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max))
    00196   {
    00197     int i;
    00198     for(i = 0; i < numberOfFlags && flags[i].type != flag.type; ++i)
    00199       ; // i is the index of the first occurance of the flag type
    00200
    00201     if(i < numberOfFlags)
    00202       --numberOfFlags;
    00203
    00204     if(i < FLAGS_MAX)
    00205       ++i;
    00206
    00207     while(--i > 0)
    00208       flags[i] = flags[i - 1];
    00209
    00210     flags[0] = flag;
    00211
    00212     if(numberOfFlags < FLAGS_MAX)
    00213       ++numberOfFlags; // one was added
    00214   }
    00215 }
    00216
    00217 /**
    00218 * this method generates new sample templates using seen-before flags and current ones
    00219 * TODO: smarter solution for flags/goals maybe regard goals as flags
    00220 */
    00221 void GT2005SampleTemplateGenerator::generateTemplates(const LandmarksPercept& landmarksPercept,
    00222                                                       const LinesPercept& linesPercept,
    00223                                                       const Pose2D& odometry)
    00224 {
    00225   NDECLARE_DEBUGDRAWING( "gt05-sl:templates" , "drawingOnField" , "shows the current template positions for new samples");
    00226   NDECLARE_DEBUGDRAWING( "gt05-sl:flags" , "drawingOnField" , "shows the currently buffered flags for template calculation");
    00227
    00228   DEBUG_RESPONSE( "gt05-sl:reset flag buffer",
    00229     numberOfFlags = 0;
    00230   );
    00231
    00232   randomFactor = 0;
    00233
    00234   // for all old flags stored update them by odometry
    00235   // TODO: decrease probability after odo update?
    00236   int i;
    00237   for(i = 0; i < numberOfFlags; ++i)
    00238   {
    00239     Flag& flag = flags[i];
    00240     Vector2<double> p = ( Pose2D(flag.angle) + Pose2D(Vector2<double>(flag.distance,0)) - odometry).translation;
    00241     flag.angle = atan2(p.y,p.x);
    00242     flag.distance = p.abs();
    00243   }
    00244
    00245   for (i=0;i<landmarksPercept.numberOfFlags;i++){
    00246     addFlag(landmarksPercept.flags[i]);
    00247   }
    00248
    00249   //quick hack only current flags
    00250   //for (i=0;i<landmarksPercept.numberOfFlags;i++)
    00251   //  flags[i] = landmarksPercept.flags[i];
    00252   //numberOfFlags = landmarksPercept.numberOfFlags;
    00253
    00254   NCOMPLEX_DRAWING ( "gt05-sl:flags",
    00255     // draw flags
    00256     for (int i = 0; i < numberOfFlags; i++)
    00257     {
    00258       NCIRCLE("gt05-sl:flags", flags[i].position.x, flags[i].position.y,
    00259             50,1,Drawings::ps_solid, Drawings::white);
    00260     }
    00261   );
    00262
    00263   numberOfTemplates = 0;
    00264   nextTemplate = 0;
    00265   double dir0,
    00266          dir1,
    00267          dir2;
    00268   double dist0,
    00269          dist1,
    00270          dist2;
    00271   Vector2<double> mark0,
    00272                   mark1,
    00273                   mark2,
    00274                   cameraOffset(landmarksPercept.cameraOffset.x,
    00275                                landmarksPercept.cameraOffset.y);
    00276
    00277
    00278   int n = numberOfFlags + 2 * landmarksPercept.numberOfGoals;
    00279
    00280   for(i = 0; i < n - 1; ++i)
    00281     // calculate bearing for the i-th mark inside the landmarks percept
    00282     if(getBearing(landmarksPercept,i,mark0,dir0,dist0))
    00283       for(int j = i + 1; j < n; ++j)
    00284         // calculate bearing for the j-th mark inside the landmarks percept
    00285         if(getBearing(landmarksPercept,j,mark1,dir1,dist1))
    00286         {
    00287           if(numberOfTemplates >= TEMPLATES_MAX - 4)
    00288             return;
    00289          int t = poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
    00290                                                         templates[numberOfTemplates],templates[numberOfTemplates + 1]);
    00291          if (t > 0) draw(templates[numberOfTemplates],Drawings::red, mark0, mark1, dir0, dir1);
    00292          if (t > 1) draw(templates[numberOfTemplates + 1],Drawings::red, mark0, mark1, dir0, dir1);
    00293          numberOfTemplates += t;
    00294
    00295          t = poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
    00296                                                         templates[numberOfTemplates],templates[numberOfTemplates + 1]);
    00297          if (t > 0) draw(templates[numberOfTemplates],Drawings::red, mark0, mark1, dir0, dir1);
    00298          if (t > 1) draw(templates[numberOfTemplates + 1],Drawings::red, mark0, mark1, dir0, dir1);
    00299          numberOfTemplates += t;
    00300
    00301           for(int k = j + 1; k < n; ++k)
    00302             if(getBearing(landmarksPercept,k,mark2,dir2,dist2))
    00303               if(poseFromBearings(dir0,dir1,dir2,mark0,mark1,mark2,cameraOffset,
    00304                                   templates[numberOfTemplates]))
    00305               {
    00306                 draw(templates[numberOfTemplates],Drawings::pink, mark0, mark1, mark2, dir0, dir1, dir2);
    00307                 if(++numberOfTemplates == TEMPLATES_MAX)
    00308                   return;
    00309               }
    00310     }
    00311
    00312
    00313   // add template positions from goal linepoints
    00314   // this is not so nice, will see if we change it later
    00315   for(i = LinesPercept::yellowGoal; i <= LinesPercept::skyblueGoal; ++i)
    00316   {
    00317     for(int j = 0; j < linesPercept.points[i].numberOfPoints; ++j)
    00318     {
    00319       const Vector2<int>& point = linesPercept.points[i].pointsOnField[j];
    00320       Pose2D t = templateTable->sample(point.abs());
    00321       if((getPlayer().getTeamColor() == Player::blue && i == LinesPercept::yellowGoal) ||
    00322          (getPlayer().getTeamColor() == Player::red && i == LinesPercept::skyblueGoal))
    00323       {
    00324         // mirror position for opponent goal
    00325         t = Pose2D(pi) + t;
    00326       }
    00327       templates[numberOfTemplates++] = t + Pose2D(-atan2((double)point.y,(double)point.x));
    00328       draw(templates[numberOfTemplates - 1], Drawings::yellow);
    00329       if(numberOfTemplates == TEMPLATES_MAX)
    00330         return;
    00331     }
    00332   }
    00333 }
    00334
    00335 int GT2005SampleTemplateGenerator::getNumberOfTemplates() const
    00336 {
    00337   return numberOfTemplates;
    00338 }
    00339
    00340 void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color) const
    00341 {
    00342   NCOMPLEX_DRAWING( "gt05-sl:templates",
    00343     // draw templates
    00344     NCIRCLE("gt05-sl:templates", pose.translation.x, pose.translation.y,
    00345           50,1,Drawings::ps_solid, color);
    00346     Pose2D p = pose + Pose2D(50,0);
    00347     NLINE("gt05-sl:templates",
    00348         pose.translation.x,
    00349         pose.translation.y,
    00350         p.translation.x,
    00351         p.translation.y,
    00352         1,
    00353         Drawings::ps_solid,
    00354         color);
    00355   );
    00356 }
    00357
    00358 void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color,
    00359                                          const Vector2<double>& mark1,
    00360                                          const Vector2<double>& mark2, double dir1, double dir2) const
    00361 {
    00362   NCOMPLEX_DRAWING( "gt05-sl:templates",
    00363     // draw templates
    00364     NLINE("gt05-sl:templates",
    00365         pose.translation.x,
    00366         pose.translation.y,
    00367         mark1.x,
    00368         mark1.y,
    00369         1,
    00370         Drawings::ps_solid,
    00371         Drawings::black);
    00372     NLINE("gt05-sl:templates",
    00373         pose.translation.x,
    00374         pose.translation.y,
    00375         mark2.x,
    00376         mark2.y,
    00377         1,
    00378         Drawings::ps_solid,
    00379         Drawings::black);
    00380     Pose2D p1 = pose + Pose2D(dir1) + Pose2D(50,0);
    00381     Pose2D p2 = pose + Pose2D(dir1) + Pose2D(100,0);
    00382     NLINE("gt05-sl:templates",
    00383         p1.translation.x,
    00384         p1.translation.y,
    00385         p2.translation.x,
    00386         p2.translation.y,
    00387         1,
    00388         Drawings::ps_solid,
    00389         color);
    00390     p1 = pose + Pose2D(dir2) + Pose2D(50,0);
    00391     p2 = pose + Pose2D(dir2) + Pose2D(100,0);
    00392     NLINE("gt05-sl:templates",
    00393         p1.translation.x,
    00394         p1.translation.y,
    00395         p2.translation.x,
    00396         p2.translation.y,
    00397         1,
    00398         Drawings::ps_solid,
    00399         color);
    00400     draw(pose, color);
    00401   );
    00402 }
    00403
    00404 void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color,
    00405                                          const Vector2<double>& mark1,
    00406                                          const Vector2<double>& mark2,
    00407                                          const Vector2<double>& mark3,
    00408                                          double dir1, double dir2, double dir3) const
    00409 {
    00410   NCOMPLEX_DRAWING( "gt05-sl:templates",
    00411     // draw templates
    00412     NLINE("gt05-sl:templates",
    00413         pose.translation.x,
    00414         pose.translation.y,
    00415         mark1.x,
    00416         mark1.y,
    00417         1,
    00418         Drawings::ps_solid,
    00419         Drawings::black);
    00420     Pose2D p1 = pose + Pose2D(dir1) + Pose2D(50,0);
    00421     Pose2D p2 = pose + Pose2D(dir1) + Pose2D(100,0);
    00422     NLINE("gt05-sl:templates",
    00423         p1.translation.x,
    00424         p1.translation.y,
    00425         p2.translation.x,
    00426         p2.translation.y,
    00427         1,
    00428         Drawings::ps_solid,
    00429         color);
    00430     draw(pose, color, mark2, mark3, dir2, dir3);
    00431   );
    00432 }
    00433
    00434 bool GT2005SampleTemplateGenerator::checkTemplatePose(const Pose2D &pose, const Vector2<double> &mark, double dir)
    00435 {
    00436   double dirFromPose = (mark - pose.translation).angle();
    00437   double dirFromBearing = dir + pose.rotation;
    00438
    00439   return fabs(normalize(dirFromPose - dirFromBearing)) < 0.05;
    00440 }