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

# Modules/SelfLocator/SlamSelfLocator/SlamSampleTemplateGenerator.cpp

Go to the documentation of this file.

    00001 #include "SlamSampleTemplateGenerator.h"
    00002
    00003
    00004 /*
    00005 *
    00006 *  -Buffer class not used. maybe implement priority queue
    00007 *  not so important task since template generation is already ok
    00008 *  -Implement usage of different seen linetypes!
    00009 *   maybe we get this with a separated observation model
    00010 *
    00011 */
    00012
    00013
    00014   SlamSampleTemplateGenerator::SlamSampleTemplateGenerator()
    00015   {
    00016     numberOfTemplates = 0;
    00017     numberOfFlags = 0;
    00018     nextTemplate = 0;
    00019     randomFactor = 0;
    00020   }
    00021
    00022
    00023   /**
    00024   * this method returns a generated template
    00025   * TODO: declare constant value as constants to modify during runtime
    00026   */
    00027   SlamSelfLocatorSample SlamSampleTemplateGenerator::getTemplate()
    00028   {
    00029     if(nextTemplate >= numberOfTemplates)
    00030     {
    00031       nextTemplate = 0;
    00032       ++randomFactor;
    00033     }
    00034
    00035     while(nextTemplate < numberOfTemplates)
    00036     {
    00037       const Pose2D& t = templates[nextTemplate++];
    00038
    00039       // if the template is inside the field, we put it to this position +/- random value
    00040       if(field.isInside(t.translation))
    00041       {
    00042       return SlamSelfLocatorSample(Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
    00043                             t.translation.x + randomFactor * 50),
    00044                     Range<double>(t.translation.y - randomFactor * 50,
    00045                             t.translation.y + randomFactor * 50),
    00046                     Range<double>(t.getAngle() - randomFactor * 0.1,
    00047                             t.getAngle() + randomFactor * 0.1)));
    00048       }
    00049     }
    00050     return SlamSelfLocatorSample(field.randomPose());
    00051   }
    00052
    00053
    00054
    00055   /*
    00056   * this method generates a robot pose from 3 bearings to known mark0,1,2
    00057   */
    00058   bool SlamSampleTemplateGenerator::poseFromBearings(double dir0,double dir1,double dir2,
    00059                       const Vector2<double>& mark0,
    00060                       const Vector2<double>& mark1,
    00061                       const Vector2<double>& mark2,
    00062                       const Vector2<double>& cameraOffset,
    00063                       Pose2D& resultingPose)
    00064   {
    00065     double w1 = dir1 - dir0,
    00066         w2 = dir2 - dir1;
    00067
    00068     if(sin(w1) != 0 && sin(w2) != 0)
    00069     {
    00070       Vector2<double> p = mark2 - mark1;
    00071       double a2 = p.abs();
    00072       p = (Pose2D(mark0) - Pose2D(atan2(p.y,p.x),mark1)).translation; // rotates the vec 1->0 arount 1 by - atan(py,px)
    00073       double a1 = p.abs();
    00074       if(a1 > 0 && a2 > 0)
    00075       {
    00076       double w3 = pi2 - w1 - w2 - atan2(p.y,p.x);
    00077       double t = a1 * sin(w2) / (a2 * sin(w1)) + cos(w3);
    00078       if(t != 0)
    00079       {
    00080         double w = atan(sin(w3) / t);
    00081         double b = a1 * sin(w) / sin(w1);
    00082         w = pi - w1 - w;
    00083         p = mark0 - mark1;
    00084         resultingPose = Pose2D(atan2(p.y,p.x) - w,mark1)
    00085                 + Pose2D(Vector2<double>(b,0));
    00086         p = (Pose2D(mark1) - resultingPose).translation;
    00087         resultingPose += Pose2D(atan2(p.y,p.x) - dir1);
    00088         resultingPose += Pose2D(Vector2<double>(-cameraOffset.x,
    00089                             -cameraOffset.y));
    00090         return true;
    00091       }
    00092       }
    00093     }
    00094     return false;
    00095   }
    00096
    00097   /*
    00098   * this method generates up to 2 possible poses from two marks seen and the distance
    00099   * to "mark1"
    00100   */
    00101   int SlamSampleTemplateGenerator::poseFromBearingsAndDistance(
    00102     double dir0,double dir1,double dist, // dir0, dir1 seen angles to the marks, dist= distance to ,mark1
    00103     const Vector2<double>& mark0,   // mark0,1 = mark0/1 in absolute coordinates
    00104     const Vector2<double>& mark1,
    00105     const Vector2<double>& cameraOffset,
    00106     Pose2D& resultingPose1,
    00107     Pose2D& resultingPose2)
    00108     {
    00109       double alpha = dir0 - dir1;         // seen angle  between marks
    00110       Vector2<double> diff = mark0 - mark1;   // deltavector between marks in absolute coords pointing from 1 to 0
    00111       double dir10 = atan2(diff.y,diff.x);    // angle at mark1 to absolute coordinate system pointing to mark0
    00112       double f = dist * sin(alpha) / diff.abs();  // angle at dist*sin(alpha) is 90deg, f = sin(gamma) angle at mark0
    00113       if(fabs(f) <= 1)              // valid length relations?
    00114       {
    00115         double gamma = asin(f),         // gamma = asin(f) and beta (winkel at mark1) from angles sum
    00116           beta = pi - alpha - gamma;
    00117
    00118         resultingPose1 = Pose2D(dir10 + beta,mark1) +         // if alpha>pi/2 dir10+beta points to robot postion
    00119                 Pose2D(pi - dir1,Vector2<double>(dist,0)) +     // this pose is always inserted
    00120                 Pose2D(Vector2<double>(-cameraOffset.x,
    00121                     -cameraOffset.y));
    00122
    00123         if(fabs(alpha) < pi_2 || fabs(alpha) > 3 * pi_2)
    00124         {                               // if alpha<pi/2 gamma might be >pi/2 so this solution might be correct
    00125         if(gamma > 0)
    00126           gamma = pi - gamma;
    00127         else
    00128           gamma = -pi - gamma;
    00129         beta = pi - alpha - gamma;
    00130         resultingPose2 = Pose2D(dir10 + beta,mark1) +
    00131                 Pose2D(pi - dir1,Vector2<double>(dist,0)) +
    00132                 Pose2D(Vector2<double>(-cameraOffset.x,
    00133                     -cameraOffset.y));
    00134         return 2;
    00135         }
    00136         else
    00137         return 1;
    00138       }
    00139       else
    00140         return 0;
    00141   }
    00142
    00143
    00144   bool SlamSampleTemplateGenerator::getBearing(const LandmarksPercept& landmarksPercept,int i,
    00145                   Vector2<double>& mark,
    00146                   double& dir,double& dist)
    00147   {
    00148     if(i < numberOfFlags)
    00149     {
    00150       const Flag& flag = flags[i];
    00151       mark = flag.position;
    00152       dir = flag.angle;
    00153       dist = flag.distance;
    00154       return true;
    00155     }
    00156     else
    00157     {
    00158       i -= numberOfFlags;
    00159       const Goal& goal = landmarksPercept.goals[i >> 1];
    00160       if((i & 1) == 0 && !goal.isOnBorder(goal.x.min))
    00161       {
    00162       mark = goal.rightPost;
    00163       dir = goal.x.min;
    00164       dist = goal.distance;
    00165       return true;
    00166       }
    00167       else if((i & 1) == 1 && !goal.isOnBorder(goal.x.max))
    00168       {
    00169       mark = goal.leftPost;
    00170       dir = goal.x.max;
    00171       dist = goal.distance;
    00172       return true;
    00173       }
    00174       else
    00175       return false;
    00176     }
    00177   }
    00178
    00179   /*
    00180   * replaces the flag of the current type in the flags buffer
    00181   *
    00182   */
    00183   void SlamSampleTemplateGenerator::addFlag(const Flag& flag)
    00184   {
    00185     if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max))
    00186     {
    00187       int i;
    00188       for(i = 0; i < numberOfFlags && flags[i].type != flag.type; ++i)
    00189         ; // i is the index of the first occurance of the flag type
    00190
    00191       if(i < numberOfFlags)
    00192         --numberOfFlags;
    00193
    00194       if(i < FLAGS_MAX)
    00195         ++i;
    00196
    00197       while(--i > 0)
    00198         flags[i] = flags[i - 1];
    00199
    00200       flags[0] = flag;
    00201
    00202       if(numberOfFlags < FLAGS_MAX)
    00203         ++numberOfFlags; // one was added
    00204     }
    00205   }
    00206
    00207   /**
    00208   * this method generates new sample templates using seen-before flags and current ones
    00209   * TODO: find a smarter solution for numberOfPerceptTypes and types[]
    00210   * TODO: smarter solution for flags/goals maybe regard goals as flags
    00211   */
    00212 void SlamSampleTemplateGenerator::generateTemplates(const LandmarksPercept& landmarksPercept,
    00213                            const LinesPercept& linesPercept,
    00214                            const Pose2D& odometry,
    00215                            int   numberOfPerceptTypes,
    00216                            const LinesPercept::LineType types[LinesPercept::numberOfLineTypes],
    00217                            bool useLandmarks)
    00218 {
    00219   randomFactor = 0;
    00220
    00221   // for all old flags stored update them by odometry
    00222   // TODO: decrease probability after odo update?
    00223   int i;
    00224   for(i = 0; i < numberOfFlags; ++i)
    00225   {
    00226     Flag& flag = flags[i];
    00227     Vector2<double> p = ( Pose2D(flag.angle) + Pose2D(Vector2<double>(flag.distance,0)) - odometry).translation;
    00228     flag.angle = atan2(p.y,p.x);
    00229     flag.distance = p.abs();
    00230   }
    00231
    00232   for (i=0;i<landmarksPercept.numberOfFlags;i++){
    00233     addFlag(landmarksPercept.flags[i]);
    00234   }
    00235
    00236   numberOfTemplates = 0;
    00237   nextTemplate = 0;
    00238   double dir0,
    00239          dir1,
    00240          dir2;
    00241   double dist0,
    00242          dist1;
    00243   Vector2<double> mark0,
    00244                   mark1,
    00245                   mark2,
    00246                   cameraOffset(landmarksPercept.cameraOffset.x,
    00247                                landmarksPercept.cameraOffset.y);
    00248
    00249   /**
    00250   * Do not generate new templates from landmark percepts
    00251   */
    00252   if (useLandmarks)
    00253   {
    00254     int n = numberOfFlags + 2 * landmarksPercept.numberOfGoals;
    00255     for(i = 0; i < n - 2; ++i)
    00256       // calculate bearing for the i-th mark inside the landmarks percept
    00257       if(getBearing(landmarksPercept,i,mark0,dir0,dist0))
    00258       for(int j = i + 1; j < n - 1; ++j)
    00259         // calculate bearing for the j-th mark inside the landmarks percept
    00260         if(getBearing(landmarksPercept,j,mark1,dir1,dist1))
    00261         {
    00262         if(numberOfTemplates >= TEMPLATES_MAX - 4)
    00263           return;
    00264         numberOfTemplates += poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
    00265                                 templates[numberOfTemplates],templates[numberOfTemplates + 1]);
    00266         numberOfTemplates += poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
    00267                                 templates[numberOfTemplates],templates[numberOfTemplates + 1]);
    00268
    00269         for(int k = j + 1; k < n; ++k)
    00270           if(getBearing(landmarksPercept,k,mark2,dir2,dist0))
    00271           if(poseFromBearings(dir0,dir1,dir2,mark0,mark1,mark2,cameraOffset,
    00272                     templates[numberOfTemplates]))
    00273             if(++numberOfTemplates == TEMPLATES_MAX)
    00274             return;
    00275
    00276         }
    00277   };
    00278   /**
    00279   * ----------------------------------------------------
    00280   */
    00281
    00282   // this is not so nice, will see if we change it later
    00283   for(i = 0; i < numberOfPerceptTypes; ++i)
    00284   {
    00285     LinesPercept::LineType type = types[i];
    00286     for(int j = 0; j < linesPercept.points[type].numberOfPoints; ++j)
    00287     {
    00288       const Vector2<int>& point = linesPercept.points[type].pointsOnField[j];
    00289       int index = type;
    00290       if(getPlayer().getTeamColor() == Player::blue && type >= LinesPercept::yellowGoal)
    00291         index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
    00292       templates[numberOfTemplates++] =  templateTable[index - LinesPercept::yellowGoal].sample(point.abs()) + Pose2D(-atan2((double)point.y,(double)point.x));
    00293       if(numberOfTemplates == TEMPLATES_MAX)
    00294         return;
    00295     }
    00296   }
    00297 }
    00298
    00299 int SlamSampleTemplateGenerator::getNumberOfTemplates(){
    00300   return numberOfTemplates;
    00301 }
    00302
    00303 /*
    00304 SlamSampleTemplateGenerator::~SlamSampleTemplateGenerator(void)
    00305 {
    00306 }
    00307 */