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

# Tools/Math/Geometry.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Tools/Math/Geometry.h
    00003 * Declares class Geometry
    00004 *
    00005 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
    00006 * @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
    00007 */
    00008
    00009 #ifndef __Geometry_h__
    00010 #define __Geometry_h__
    00011
    00012 #include "Tools/Math/Pose2D.h"
    00013 #include "Representations/Perception/CameraMatrix.h"
    00014 #include "Representations/Perception/CameraInfo.h"
    00015 #include "Representations/Cognition/RobotPose.h"
    00016 #include "Tools/Actorics/RobotDimensions.h"
    00017
    00018
    00019
    00020
    00021 /**
    00022 * The class Geometry defines representations for geometric objects and Methods
    00023 * for calculations with such object.
    00024 *
    00025 */
    00026 class Geometry
    00027 {
    00028 public:
    00029
    00030   /** Defines a circle by its center and its radius*/
    00031   struct Circle
    00032   {
    00033     Circle();
    00034     Vector2<double> center;
    00035     double radius;
    00036   };
    00037
    00038   /** Defines a line by two vectors*/
    00039   struct Line
    00040   {
    00041     Vector2<double> base;
    00042     Vector2<double> direction;
    00043
    00044     Line() {};
    00045     Line(Vector2<double> base, Vector2<double> direction) :
    00046     base(base),
    00047       direction(direction)
    00048     {};
    00049
    00050     Line(Vector2<int> base, Vector2<double> direction) :
    00051     direction(direction)
    00052     {
    00053       this->base.x = base.x;
    00054       this->base.y = base.y;
    00055     };
    00056
    00057     Line(Vector2<int> base, Vector2<int> direction)
    00058     {
    00059       this->base.x = base.x;
    00060       this->base.y = base.y;
    00061       this->direction.x = direction.x;
    00062       this->direction.y = direction.y;
    00063     };
    00064
    00065
    00066     Line(double baseX, double baseY, double directionX, double directionY)
    00067     {
    00068       base.x = baseX;
    00069       base.y = baseY;
    00070       direction.x = directionX;
    00071       direction.y = directionY;
    00072     };
    00073
    00074     void normalizeDirection();
    00075   };
    00076
    00077   struct PixeledLine
    00078   {
    00079     PixeledLine(int x1, int x2, int y1, int y2):x1(x1),y1(y1),x2(x2),y2(y2)
    00080     {
    00081       calculatePixels();
    00082     };
    00083
    00084     PixeledLine(Vector2<int> start, Vector2<int> end):x1(start.x),y1(start.y),x2(end.x),y2(end.y)
    00085     {
    00086       calculatePixels();
    00087     };
    00088
    00089     void calculatePixels()
    00090     {
    00091       char sign;
    00092       if(x1 == x2 && y1 == y2)
    00093       {
    00094         numberOfPixels = 0;
    00095       }
    00096       else //begin and end differ
    00097       {
    00098         if(abs(x2 - x1) > abs(y2 - y1) )
    00099         {
    00100           if(x1 < x2) sign = 1; else sign = -1;
    00101           numberOfPixels = abs(x2 - x1) + 1;
    00102           for(int x = 0; x < numberOfPixels; x++)
    00103           {
    00104             int y = (int)(x * (y2 - y1) / (x2 - x1));
    00105             x_coordinate[x] = x1 + x*sign;
    00106             y_coordinate[x] = y1 + y*sign;
    00107           }
    00108         }
    00109         else
    00110         {
    00111           if(y1 < y2) sign = 1; else sign = -1;
    00112           numberOfPixels = abs(y2 - y1) + 1;
    00113           for(int y = 0; y < numberOfPixels; y++)
    00114           {
    00115             int x = (int)(y * (x2 - x1) / (y2 - y1));
    00116             x_coordinate[y] = x1 + x*sign;
    00117             y_coordinate[y] = y1 + y*sign;
    00118           }
    00119         }
    00120       } //begin and end differ
    00121     }// calculatePixels
    00122
    00123     inline int getNumberOfPixels() const
    00124     {
    00125       return numberOfPixels;
    00126     }
    00127
    00128     inline int getPixelX(int i) const
    00129     {
    00130       return x_coordinate[i];
    00131     }
    00132
    00133     inline int getPixelY(int i) const
    00134     {
    00135       return y_coordinate[i];
    00136     }
    00137
    00138   private:
    00139     int x1, y1, x2, y2;
    00140     int numberOfPixels;
    00141     //enum{maxNumberOfPixelsInLine = 228}; //diagonal size of image
    00142   enum{maxNumberOfPixelsInLine = 600}; //diagonal size of BW image
    00143     int x_coordinate[maxNumberOfPixelsInLine];
    00144     int y_coordinate[maxNumberOfPixelsInLine];
    00145   };
    00146
    00147   template <class V, int maxNumberOfPoints> class SetOfPoints
    00148   {
    00149   public:
    00150     /** Constructor */
    00151     SetOfPoints() {init();}
    00152     void add(V x, V y)
    00153     {
    00154       if(numberOfPoints >= maxNumberOfPoints) return;
    00155       points[numberOfPoints].x = y;
    00156       points[numberOfPoints].y = y;
    00157       numberOfPoints++;
    00158     }
    00159     void add(Vector2<V> newPoint)
    00160     {
    00161       if(numberOfPoints >= maxNumberOfPoints) return;
    00162       points[numberOfPoints] = newPoint;
    00163       numberOfPoints++;
    00164     }
    00165
    00166     void init() {numberOfPoints = 0;};
    00167
    00168     double linearRegression(Line& line)
    00169     {
    00170       if(numberOfPoints == 0) return 0;
    00171       double col1[maxNumberOfPoints], col2[maxNumberOfPoints], col3[maxNumberOfPoints], col4[maxNumberOfPoints], col5[maxNumberOfPoints];
    00172       double sumX, sumY, averageX, averageY, c3sum, c4sum, c5sum, m, n;
    00173
    00174       sumX=0; sumY=0; c3sum=0; c4sum=0; c5sum=0;
    00175
    00176       int i;
    00177       for (i = 0; i < numberOfPoints; i++)
    00178       {
    00179         sumX = sumX + points[i].x;
    00180         sumY = sumY + points[i].y;
    00181       }
    00182
    00183       averageX = sumX / numberOfPoints;
    00184       averageY = sumY / numberOfPoints;
    00185
    00186       for (i = 0; i < numberOfPoints; i++)
    00187       {
    00188         col1[i] = points[i].x - averageX;
    00189         col2[i] = points[i].y - averageY;
    00190         col3[i] = col1[i] * col2[i];
    00191         col4[i] = col1[i] * col1[i];
    00192         col5[i] = col2[i] * col2[i];
    00193       }
    00194
    00195       for (i = 0; i < numberOfPoints; i++)
    00196       {
    00197         c3sum = c3sum + col3[i];
    00198         c4sum = c4sum + col4[i];
    00199         c5sum = c5sum + col5[i];
    00200       }
    00201
    00202       m = c3sum / c4sum;
    00203       n = averageY - m * averageX;
    00204
    00205       line.base.x = 0;
    00206       line.base.y = n;
    00207       line.direction.x = 1;
    00208       line.direction.y = m;
    00209
    00210       // reliability:
    00211       return fabs(c3sum / sqrt(c4sum * c5sum));
    00212     }
    00213   private:
    00214     Vector2<V> points[maxNumberOfPoints];
    00215     int numberOfPoints;
    00216   };
    00217
    00218   /**
    00219   * Calculates the angle between a pose and a position
    00220   * @param from The base pose.
    00221   * @param to The other position.
    00222   * @return the angle from the pose to the position.
    00223   */
    00224   static double angleTo(const Pose2D& from,
    00225     const Vector2<double>& to);
    00226
    00227   /**
    00228   * Calculates the distance from a pose to a position
    00229   * @param from The base pose.
    00230   * @param to The other position.
    00231   * @return the distance from the pose to the position.
    00232   */
    00233   static double distanceTo(const Pose2D& from,
    00234     const Vector2<double>& to);
    00235
    00236   /**
    00237   * Calculates the relative vector from a pose to a position
    00238   * @param from The base pose.
    00239   * @param to The other position.
    00240   * @return the vector from the pose to the position.
    00241   */
    00242   static Vector2<double> vectorTo(const Pose2D& from,
    00243     const Vector2<double>& to);
    00244
    00245   /**
    00246   * Returns the circle defined by the three points.
    00247   * @param point1 The first point.
    00248   * @param point2 The second point.
    00249   * @param point3 The third point.
    00250   * @return The circle defined by point1, point2 and point3.
    00251   */
    00252   static Circle getCircle(
    00253     const Vector2<int> point1,
    00254     const Vector2<int> point2,
    00255     const Vector2<int> point3
    00256     );
    00257
    00258   static bool getIntersectionOfLines(
    00259     const Line line1,
    00260     const Line line2,
    00261     Vector2<double>& intersection
    00262     );
    00263
    00264   static bool getIntersectionOfLines(
    00265     const Line line1,
    00266     const Line line2,
    00267     Vector2<int>& intersection
    00268     );
    00269
    00270   static bool getIntersectionOfRaysFactor(
    00271     const Line ray1,
    00272     const Line ray2,
    00273     double& intersection
    00274     );
    00275
    00276   static double getDistanceToLine(
    00277     const Line line,
    00278     const Vector2<double>& point
    00279     );
    00280
    00281   static double getDistanceToEdge(
    00282     const Line line,
    00283     const Vector2<double>& point
    00284     );
    00285
    00286   static double distance(
    00287     const Vector2<double>& point1,
    00288     const Vector2<double>& point2
    00289     );
    00290
    00291   static double distance(
    00292     const Vector2<int>& point1,
    00293     const Vector2<int>& point2
    00294     );
    00295
    00296 private:
    00297   static double calculateScaleFactor(
    00298     int y,
    00299     unsigned long frameNumber,
    00300     unsigned long prevFrameNumber,
    00301     const CameraInfo& cameraInfo
    00302     );
    00303
    00304 public:
    00305   static Vector3<double> rayFromCamera(
    00306     int y,
    00307     const CameraMatrix& cameraMatrix,
    00308     const CameraMatrix& prevCameraMatrix,
    00309     const Vector3<double> vector,
    00310     const CameraInfo& cameraInfo
    00311     );
    00312
    00313   static void calculateAnglesForPoint(
    00314     const Vector2<int>& point,
    00315     const CameraMatrix& cameraMatrix,
    00316     const CameraInfo& cameraInfo,
    00317     Vector2<double>& angles
    00318     );
    00319
    00320   static void calculateAnglesForPoint(
    00321     const Vector2<int>& point,
    00322     const CameraMatrix& cameraMatrix,
    00323     const CameraMatrix& prevCameraMatrix,
    00324     const CameraInfo& cameraInfo,
    00325     Vector2<double>& angles
    00326     );
    00327
    00328   static void calculatePointByAngles(
    00329     const Vector2<double>& angles,
    00330     const CameraMatrix& cameraMatrix,
    00331     const CameraInfo& cameraInfo,
    00332     Vector2<int>& point
    00333     );
    00334
    00335   static bool clipLineWithQuadrangle(
    00336     const Line lineToClip,
    00337     const Vector2<double>& corner0,
    00338     const Vector2<double>& corner1,
    00339     const Vector2<double>& corner2,
    00340     const Vector2<double>& corner3,
    00341     Vector2<double>& clipPoint1,
    00342     Vector2<double>& clipPoint2
    00343     );
    00344
    00345   static bool clipLineWithQuadrangle(
    00346     const Line lineToClip,
    00347     const Vector2<double>& corner0,
    00348     const Vector2<double>& corner1,
    00349     const Vector2<double>& corner2,
    00350     const Vector2<double>& corner3,
    00351     Vector2<int>& clipPoint1,
    00352     Vector2<int>& clipPoint2
    00353     );
    00354
    00355   static bool isPointInsideRectangle(
    00356     const Vector2<double>& bottomLeftCorner,
    00357     const Vector2<double>& topRightCorner,
    00358     const Vector2<double>& point
    00359     );
    00360
    00361   static bool isPointInsideRectangle(
    00362     const Vector2<int>& bottomLeftCorner,
    00363     const Vector2<int>& topRightCorner,
    00364     const Vector2<int>& point
    00365     );
    00366
    00367   static bool clipPointInsideRectange(
    00368     const Vector2<int>& bottomLeftCorner,
    00369     const Vector2<int>& topRightCorner,
    00370     Vector2<int>& point
    00371     );
    00372
    00373   static bool clipPointInsideRectange(
    00374     const Vector2<int>& bottomLeftCorner,
    00375     const Vector2<int>& topRightCorner,
    00376     Vector2<double>& point
    00377     );
    00378
    00379   /**
    00380   * Calculates where a pixel in the image lies on the ground (relative to the robot).
    00381   * @param x Specifies the x-coordinate of the pixel.
    00382   * @param y Specifies the y-coordinate of the pixel.
    00383   * @param cameraMatrix The camera matrix of the image.
    00384   * @param cameraInfo The camera info of the image.
    00385   * @param pointOnField The resulting point.
    00386   */
    00387   static bool calculatePointOnField(
    00388     const int x,
    00389     const int y,
    00390     const CameraMatrix& cameraMatrix,
    00391     const CameraInfo& cameraInfo,
    00392     Vector2<double>& pointOnField
    00393     );
    00394
    00395   /**
    00396   * Calculates where a pixel in the image lies on the ground (relative to the robot).
    00397   * @param x Specifies the x-coordinate of the pixel.
    00398   * @param y Specifies the y-coordinate of the pixel.
    00399   * @param cameraMatrix The camera matrix of the image.
    00400   * @param prevCameraMatrix The camera matrix of the previous image.
    00401   * @param cameraInfo The camera info of the image.
    00402   * @param pointOnField The resulting point.
    00403   */
    00404   static bool calculatePointOnField(
    00405     const int x,
    00406     const int y,
    00407     const CameraMatrix& cameraMatrix,
    00408     const CameraMatrix& prevCameraMatrix,
    00409     const CameraInfo& cameraInfo,
    00410     Vector2<double>& pointOnField
    00411     );
    00412
    00413   /**
    00414   * Calculates where a pixel in the image lies on the ground (relative to the robot).
    00415   * @param x Specifies the x-coordinate of the pixel.
    00416   * @param y Specifies the y-coordinate of the pixel.
    00417   * @param cameraMatrix The camera matrix of the image.
    00418   * @param cameraInfo The camera info of the image.
    00419   * @param pointOnField The resulting point.
    00420   */
    00421   static bool calculatePointOnField(
    00422     const int x,
    00423     const int y,
    00424     const CameraMatrix& cameraMatrix,
    00425     const CameraInfo& cameraInfo,
    00426     Vector2<int>& pointOnField
    00427     )
    00428   {
    00429     Vector2<double> pointOnFieldDouble;
    00430     bool onField = calculatePointOnField(x,y,cameraMatrix,cameraInfo,pointOnFieldDouble);
    00431     pointOnField.x = (int)pointOnFieldDouble.x;
    00432     pointOnField.y = (int)pointOnFieldDouble.y;
    00433     return onField;
    00434   }
    00435
    00436   /**
    00437   * Calculates where a pixel in the image lies on the ground (relative to the robot).
    00438   * @param x Specifies the x-coordinate of the pixel.
    00439   * @param y Specifies the y-coordinate of the pixel.
    00440   * @param cameraMatrix The camera matrix of the image.
    00441   * @param cameraInfo The camera info of the image.
    00442   * @param pointOnField The resulting point.
    00443   */
    00444   static bool calculatePointOnField(
    00445     const int x,
    00446     const int y,
    00447     const CameraMatrix& cameraMatrix,
    00448     const CameraMatrix& prevCameraMatrix,
    00449     const CameraInfo& cameraInfo,
    00450     Vector2<int>& pointOnField
    00451     )
    00452   {
    00453     Vector2<double> pointOnFieldDouble;
    00454     bool onField = calculatePointOnField(x,y,cameraMatrix,prevCameraMatrix,cameraInfo,pointOnFieldDouble);
    00455     pointOnField.x = (int)pointOnFieldDouble.x;
    00456     pointOnField.y = (int)pointOnFieldDouble.y;
    00457     return onField;
    00458   }
    00459
    00460   /**
    00461   * Calculates where a relative point on the ground appears in an image.
    00462   * @param point The coordinates of the point relative to the robot's origin.
    00463   * @param cameraMatrix The camera matrix of the image.
    00464   * @param cameraInfo The camera info of the image.
    00465   * @param pointInImage The resulting point.
    00466   */
    00467   static void calculatePointInImage
    00468   (
    00469    const Vector2<int>& point,
    00470    const CameraMatrix& cameraMatrix,
    00471    const CameraInfo& cameraInfo,
    00472    Vector2<int>& pointInImage
    00473    );
    00474
    00475   /**
    00476   * Clips a line with a rectangle
    00477   * @param bottomLeft The bottom left corner of the rectangle
    00478   * @param topRight The top right corner of the rectangle
    00479   * @param line The line to be clipped
    00480   * @param point1 The starting point of the resulting line
    00481   * @param point2 The end point of the resulting line
    00482   * @return states whether clipping was necessary (and done)
    00483   */
    00484   static bool getIntersectionPointsOfLineAndRectangle(
    00485    const Vector2<int>& bottomLeft,
    00486    const Vector2<int>& topRight,
    00487    const Geometry::Line line,
    00488    Vector2<int>& point1,
    00489    Vector2<int>& point2
    00490    );
    00491
    00492
    00493   /**
    00494   * Clips a line with the Cohen-Sutherland-Algorithm
    00495   * @param bottomLeft The bottom left corner of the rectangle
    00496   * @param topRight The top right corner of the rectangle
    00497   * @param point1 The starting point of the line
    00498   * @param point2 The end point of the line
    00499   * @return states whether clipping was necessary (and done)
    00500   */
    00501   static bool clipLineWithRectangleCohenSutherland(
    00502    const Vector2<int>& bottomLeft,
    00503    const Vector2<int>& topRight,
    00504    Vector2<int>& point1,
    00505    Vector2<int>& point2
    00506    );
    00507
    00508   /**
    00509   * Clips a line with the Cohen-Sutherland-Algorithm if it has a point outside the rectangle.
    00510   * Expands the line if it has a point inside the rectangle.
    00511   * @param bottomLeft The bottom left corner of the rectangle
    00512   * @param topRight The top right corner of the rectangle
    00513   * @param point1 The starting point of the line
    00514   * @param point2 The end point of the line
    00515   */
    00516   /*
    00517   static bool ExpandOrClipLineWithRectangleCohenSutherland(
    00518    const Vector2<int>& bottomLeft,
    00519    const Vector2<int>& topRight,
    00520    Vector2<int>& point1,
    00521    Vector2<int>& point2
    00522    );
    00523 */
    00524
    00525   /**
    00526   * Calculates the Cohen Sutherland Code for a given point and a given rectangle
    00527   */
    00528   static int cohenSutherlandOutCode(
    00529     const Vector2<int>& bottomLeft,
    00530     const Vector2<int>& topRight,
    00531     Vector2<int>& point
    00532     )
    00533   {
    00534     int code;
    00535     if ( point.x < bottomLeft.x )
    00536       code = 1 ;
    00537     else
    00538       code = 0 ;
    00539     if ( point.x > topRight.x )
    00540       code += 2 ;
    00541     if ( point.y < bottomLeft.y )
    00542       code += 4 ;
    00543     if ( point.y > topRight.y )
    00544       code += 8 ;
    00545     return(code) ;
    00546   }
    00547
    00548   /**
    00549   * Calculates the intersection of an arbitrary line and a horizontal or vertical line.
    00550   */
    00551   static int intersection(int a1,int b1,int a2,int b2,int value);
    00552
    00553   /**
    00554   * Function does the transformation from 2d relative robot coordinates
    00555   * to absolute field coordinates.
    00556   * @param rp current Robot Pose.
    00557   * @param x relative x-coordinate of ball (relative to robot)
    00558   * @param y relative y-coordinate of ball (relative to robot)
    00559   * @return Returns the ball positon in absolute coordinates
    00560   */
    00561   static Vector2<double> relative2FieldCoord(RobotPose rp, double x, double y);
    00562
    00563   /**
    00564   * Function does the transformation from 2d field coordinates
    00565   * to coordinates relative to the robot.
    00566   * @param robotPose current Robot Pose.
    00567   * @param fieldCoord
    00568   * @return Returns the positon in relative
    00569   */
    00570   static Vector2<double> fieldCoord2Relative(RobotPose robotPose, Vector2<double> fieldCoord);
    00571
    00572
    00573   /**
    00574   * The function approximates the shape of a ball in the camera image.
    00575   * Note: currently, the approximation is not exact.
    00576   * @param ballOffset The ball's position relative to the robot's body origin.
    00577   * @param cameraMatrix The position and orientation of the robot's camera.
    00578   * @param cameraInfo The resolution and the opening angle of the robot's camera.
    00579   * @param circle The approximated shape generated by the function.
    00580   * @return If false, only the center of the circle is valid, not the radius.
    00581   */
    00582   static bool calculateBallInImage(const Vector2<double>& ballOffset,
    00583                                    const CameraMatrix& cameraMatrix,
    00584                                    const CameraInfo& cameraInfo,
    00585                                    Circle& circle);
    00586
    00587   /**
    00588   * The function determines how far an object is away depending on its real size and the size in the image.
    00589   * @param cameraInfo Information about the camera (opening angles, resolution, etc.).
    00590   * @param sizeInReality The real size of the object.
    00591   * @param sizeInPixels The size in the image.
    00592   * @return The distance between camera and object.
    00593   */
    00594 /*  static int getDistanceBySize(
    00595     int sizeInReality,
    00596     int sizeInPixels,
    00597     int imageWidthPixels,
    00598     double imageWidthAngle
    00599     );
    00600 */
    00601   static double getDistanceBySize
    00602     (
    00603      const CameraInfo& cameraInfo,
    00604      double sizeInReality,
    00605      double sizeInPixels
    00606      );
    00607
    00608   /**
    00609   * The function determines how far an object is away depending on its real size and the size in the image
    00610   * along with its center position, using camera intrinsic parameters.
    00611   * @param cameraInfo Class containing the intrinsic paramaters
    00612   * @param sizeInReality The real size of the object.
    00613   * @param sizeInPixels The size in the image.
    00614   * @param centerX X coordinate (in image reference) of object's baricenter.
    00615   * @param centerY Y coordinate (in image reference) of object's baricenter.
    00616   * @return The distance between camera and object.
    00617   */
    00618   static double getDistanceBySize(
    00619     const CameraInfo& cameraInfo,
    00620     double sizeInReality,
    00621     double sizeInPixels,
    00622     double centerX,
    00623     double centerY
    00624     );
    00625
    00626     /**
    00627   * The function determines how far an object is away depending on its real size and the size in the image.
    00628   * @param sizeInReality The real size of the object.
    00629   * @param sizeInPixels The size in the image.
    00630   * @return The distance between camera and object.
    00631   */
    00632   static double getDistanceByAngleSize(
    00633     double sizeInReality,
    00634     double sizeInPixels
    00635     );
    00636
    00637     /**
    00638   * The function determines how far the ball is away depending on its real size and the size in the image.
    00639   * @param sizeInReality The real size of the ball.
    00640   * @param sizeInPixels The size in the image.
    00641   * @return The distance between the camera and the ball.
    00642   */
    00643   static double getBallDistanceByAngleSize(
    00644     double sizeInReality,
    00645     double sizeInPixels
    00646     );
    00647
    00648   /**
    00649   * The function determines how big an object appears in the image depending on its distance and size.
    00650   * @param sizeInReality The real size of the object.
    00651   * @param distance The distance to the object.
    00652   * @param imageWidthPixels The horizontal resolution of the image.
    00653   * @param imageWidthAngle The horizontal opening angle of the camera.
    00654   * @return The size as it would appear in the image.
    00655   */
    00656   static double getSizeByDistance(
    00657     double sizeInReality,
    00658     double distance,
    00659     double imageWidthPixels,
    00660     double imageWidthAngle
    00661     );
    00662
    00663   /**
    00664   * The function determines how big an object appears in the image depending on its distance and size.
    00665   * @param cameraInfo Object containing camera paramters.
    00666   * @param sizeInReality The real size of the object.
    00667   * @param distance The distance to the object.
    00668   * @return The size as it would appear in the image.
    00669   */
    00670   static double getSizeByDistance
    00671     (
    00672      const CameraInfo& cameraInfo,
    00673      double sizeInReality,
    00674      double distance
    00675     );
    00676
    00677   /**
    00678   * The function calculates the horizon.
    00679   * @param cameraMatrix The camera matrix.
    00680   * @param cameraInfo Object containing camera parameters.
    00681   * @return The line of the horizon in the image.
    00682   */
    00683   static Geometry::Line calculateHorizon(
    00684     const CameraMatrix& cameraMatrix,
    00685     const CameraInfo& cameraInfo
    00686     );
    00687
    00688   /**
    00689   * The function calculates the expected size (pixel) of a field line in an image.
    00690   * @param pointInImage The point where the line is expected.
    00691   * @param cameraMatrix The camera matrix.
    00692   * @param cameraInfo Object containing the camera parameters.
    00693   * @return The size of a line pixel.
    00694   */
    00695   static int calculateLineSize(
    00696     const Vector2<int>& pointInImage,
    00697     const CameraMatrix& cameraMatrix,
    00698     const CameraInfo& cameraInfo
    00699     );
    00700
    00701   /**
    00702   * Calculates the angle size for a given pixel size.
    00703   */
    00704   static double pixelSizeToAngleSize(double pixelSize, const CameraInfo& cameraInfo);
    00705
    00706   /**
    00707   * Calculates the pixel size for a given angle size.
    00708   */
    00709   static double angleSizeToPixelSize(double angleSize, const CameraInfo& cameraInfo);
    00710
    00711   /**
    00712   * A radial distortion correction look-up table element
    00713   */
    00714   struct CorrectedCoords {
    00715     unsigned char x;
    00716     unsigned char y;
    00717   };
    00718
    00719   /**
    00720   * Corrects the radial distortion introduced by the camera lens (using a 4th order model) of a given pixel.
    00721   * @param cameraInfo Class containing camera intrinsic parameters
    00722   * @param srcX X coordinate (image reference) of pixel to be distortion-corrected.
    00723   * @param srcY Y coordinate (image reference) of pixel to be distortion-corrected.
    00724   * @param correctedX X coordinate (image reference) of resulting distortion-corrected pixel.
    00725   * @param correctedY Y coordinate (image reference) of resulting distortion-corrected pixel.
    00726   */
    00727   static inline void radialDistortionCorrection(const CameraInfo& cameraInfo, const int srcX, const int srcY, double& correctedX, double& correctedY){
    00728     double x1 = srcX - cameraInfo.opticalCenter.x;
    00729     double y1 = srcY - cameraInfo.opticalCenter.y;
    00730     double r_square = x1*x1 + y1*y1;
    00731     double k1_num = cameraInfo.secondOrderRadialDistortion;
    00732     double k1_den = cameraInfo.focalLenPow2;
    00733     double k2_num = cameraInfo.fourthOrderRadialDistortion;
    00734     double k2_den = cameraInfo.focalLenPow4;
    00735     correctedX = srcX + x1*(k1_num*r_square/k1_den + k2_num*r_square*r_square/k2_den);
    00736     correctedY = srcY + y1*(k1_num*r_square/k1_den + k2_num*r_square*r_square/k2_den);
    00737   }
    00738
    00739   /**
    00740   * This function builds a radial distortion correction look-up table based on the parameters contained in cameraInfo.
    00741   * @param cameraInfo Object containing intrinisc (and other) camera parameters.
    00742   */
    00743   static void setupRadialCorrection(const CameraInfo& cameraInfo);
    00744
    00745   static inline void setupRadialCorrectionERS7()
    00746   {
    00747     CameraInfo cameraInfo(RobotDesign::ERS7);
    00748     setupRadialCorrection(cameraInfo);
    00749   }
    00750
    00751   static inline void setupRadialCorrectionERS210()
    00752   {
    00753     CameraInfo cameraInfo(RobotDesign::ERS210);
    00754     setupRadialCorrection(cameraInfo);
    00755   }
    00756
    00757   inline static bool insideCircle(Geometry::Circle &circle,double radius2,double x,double y){
    00758     return  (((circle.center.x-x)*(circle.center.x-x)\
    00759       + (circle.center.y-y)*(circle.center.y-y)) < radius2);
    00760   }
    00761
    00762   /**
    00763   * Corrects the radial distortion introduced by the camera lens (using a 4th order model) of a given pixel,
    00764   * using a pre-computed look-up table.
    00765   * @param srcX X coordinate (image reference) of pixel to be distortion-corrected.
    00766   * @param srcY Y coordinate (image reference) of pixel to be distortion-corrected.
    00767   * @param correctedX X coordinate (image reference) of resulting distortion-corrected pixel.
    00768   * @param correctedY Y coordinate (image reference) of resulting distortion-corrected pixel.
    00769   */
    00770   static inline void radialDistortionCorrectionFast(const int srcX, const int srcY, int& correctedX, int& correctedY)
    00771   {
    00772     CorrectedCoords result = radialCorrectionLUT[srcY][srcX];
    00773     correctedX = result.x;
    00774     correctedY = result.y;
    00775   }
    00776
    00777 private:
    00778   static CorrectedCoords radialCorrectionLUT[cameraResolutionHeight_ERS7][cameraResolutionWidth_ERS7]; /** Radius look up table, used to index the correction table. */
    00779 };
    00780
    00781 #endif //__Geometry_h____