void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
  Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
  imageCoordinateSystem.origin = horizon.base;
  imageCoordinateSystem.rotation.c[0] = horizon.direction;
  imageCoordinateSystem.rotation.c[1] = Vector2<double>(-horizon.direction.y, horizon.direction.x);
  imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();

  RotationMatrix r(theCameraMatrix.rotation.transpose() * prevCameraMatrix.rotation);
  imageCoordinateSystem.offset = Vector2<double>(r.getZAngle(), r.getYAngle());

  calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b);
  imageCoordinateSystem.offsetInt = Vector2<int>(int(imageCoordinateSystem.offset.x * 1024 + 0.5), 
                                                 int(imageCoordinateSystem.offset.y * 1024 + 0.5));
  imageCoordinateSystem.aInt = int(imageCoordinateSystem.a * 1024 + 0.5);
  imageCoordinateSystem.bInt = int(imageCoordinateSystem.b * 1024 + 0.5);
  imageCoordinateSystem.cameraInfo = theCameraInfo;
  prevCameraMatrix = theCameraMatrix;
  prevTime = theFilteredJointData.timeStamp;

  DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
  ARROW("horizon",
       imageCoordinateSystem.origin.x,
       imageCoordinateSystem.origin.y,
       imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[0].x * 50,
       imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[0].y * 50,
       1, Drawings::ps_solid, ColorRGBA(255,0,0));
  ARROW("horizon",
       imageCoordinateSystem.origin.x,
       imageCoordinateSystem.origin.y,
       imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[1].x * 50,
       imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[1].y * 50,
       1, Drawings::ps_solid, ColorRGBA(255,0,0));
  GENERATE_DEBUG_IMAGE(corrected,
    INIT_DEBUG_IMAGE_BLACK(corrected);
    int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y;
    for(int ySrc = 0; ySrc < theImage.cameraInfo.resolutionHeight; ++ySrc)
      for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y; yDest <= yDest2; ++yDest)
      {
        int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x;
        for(int xSrc = 0; xSrc < theImage.cameraInfo.resolutionWidth; ++xSrc)
        {
          for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x; xDest <= xDest2; ++xDest)
          {
            DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x + 0.5), 
                                                 yDest + int(theCameraInfo.opticalCenter.y + 0.5),
                                                 theImage.image[ySrc][xSrc].y, 
                                                 theImage.image[ySrc][xSrc].cb,
                                                 theImage.image[ySrc][xSrc].cr);
          }
        }
      }
    SEND_DEBUG_IMAGE(corrected);
  );
void CognitionLogDataProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
  imageCoordinateSystem.setCameraInfo(theCameraInfo);
  DECLARE_DEBUG_DRAWING("loggedHorizon", "drawingOnImage"); // displays the horizon
  ARROW("loggedHorizon",
        imageCoordinateSystem.origin.x(),
        imageCoordinateSystem.origin.y(),
        imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 0) * 50,
        imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 0) * 50,
        0, Drawings::solidPen, ColorRGBA(255, 0, 0));
  ARROW("loggedHorizon",
        imageCoordinateSystem.origin.x(),
        imageCoordinateSystem.origin.y(),
        imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 1) * 50,
        imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 1) * 50,
        0, Drawings::solidPen, ColorRGBA(255, 0, 0));
  COMPLEX_IMAGE(corrected)
  {
    if(Blackboard::getInstance().exists("Image"))
    {
      const Image& image = (const Image&) Blackboard::getInstance()["Image"];
      INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
      int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
      for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
        for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
        {
          int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
          for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
          {
            for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
            {
              DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
                                        yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
                                        image[ySrc][xSrc].y,
                                        image[ySrc][xSrc].cb,
                                        image[ySrc][xSrc].cr);
            }
          }
        }
      SEND_DEBUG_IMAGE(corrected);
    }
  }
}
void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
  imageCoordinateSystem.setCameraInfo(theCameraInfo);

  Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
  imageCoordinateSystem.origin = horizon.base;
  imageCoordinateSystem.rotation.col(0) = horizon.direction;
  imageCoordinateSystem.rotation.col(1) = Vector2f(-horizon.direction.y(), horizon.direction.x());
  imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();

  const CameraMatrix& cmPrev = cameraMatrixPrev[theCameraInfo.camera];
  RotationMatrix r(theCameraMatrix.rotation.inverse() * cmPrev.rotation);
  imageCoordinateSystem.offset = Vector2f(r.getZAngle(), r.getYAngle());

  calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b, theJointSensorData.timestamp - cameraMatrixPrevTimeStamp[theCameraInfo.camera]);
  cameraMatrixPrev[theCameraInfo.camera] = theCameraMatrix;
  cameraMatrixPrevTimeStamp[theCameraInfo.camera] = theJointSensorData.timestamp;

  COMPLEX_IMAGE(corrected)
  {
    INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
    int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
    for(int ySrc = 0; ySrc < theImage.height; ++ySrc)
      for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
      {
        int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
        for(int xSrc = 0; xSrc < theImage.width; ++xSrc)
        {
          for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
          {
            DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
                                                 yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
                                                 theImage[ySrc][xSrc].y,
                                                 theImage[ySrc][xSrc].cb,
                                                 theImage[ySrc][xSrc].cr);
          }
        }
      }
    SEND_DEBUG_IMAGE(corrected);
  }

  COMPLEX_IMAGE(horizonAligned)
  {
    INIT_DEBUG_IMAGE_BLACK(horizonAligned, theCameraInfo.width, theCameraInfo.height);
    for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
      for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
      {
        Vector2f corrected(imageCoordinateSystem.toCorrected(Vector2i(xSrc, ySrc)));
        corrected.x() -= theCameraInfo.opticalCenter.x();
        corrected.y() -= theCameraInfo.opticalCenter.y();
        const Vector2f& horizonAligned(imageCoordinateSystem.toHorizonAligned(corrected));

        DEBUG_IMAGE_SET_PIXEL_YUV(horizonAligned, int(horizonAligned.x() + theCameraInfo.opticalCenter.x() + 0.5f),
                                                  int(horizonAligned.y() + theCameraInfo.opticalCenter.y() + 0.5f),
                                                  theImage[ySrc][xSrc].y,
                                                  theImage[ySrc][xSrc].cb,
                                                  theImage[ySrc][xSrc].cr);
      }
    SEND_DEBUG_IMAGE(horizonAligned);
  }
}