/*
*
* FOLLOWING METHODS IS USED BY CALIBRATION ONLY
*
*/
/*
* Method that calibrates (aligns) the cameras. This means placing them in a global coordinate system.
* This is done by giving them a base coordinate (upper left coordinate, since X-axis directed right, Y-axis directed down),
* a rotation and a size scaling. The base coordinate defines the cameras translation (offset position).
* The rotation is how much the camera needs to be rotated (in radians) to match the other cameras.
* The size scaling is how much the camera needs to be resized to match the other cameras where scaling = 1 means no resizing.
*/
private void setCameraCoord(object sender, EventArgs e)
{
//Turns off the timer since this method only should be called once.
((Timer)sender).Dispose();
List <Camera> activeCameras = Program.cameraController.getIncludedCameras();
//firstCamera should be the upper left camera in the global coordinate system (X-axis directed right, Y-axis directed down)
//with base coordinates(upper left) = (0,0), rotation = 0 and size scaling = 1. upperLeftCamera, upperCamera and leftCamera are
//temporary cameras used to determine the firstCamera. They are all initialized do the first active camera.
Camera firstCamera = activeCameras[0], upperLeftCamera = activeCameras[0], upperCamera = activeCameras[0], leftCamera = activeCameras[0];
bool upperLeftCameraFound = false, upperCameraFound = false, leftCameraFound = false;
//Calibration is always done in full HD to get maximum precision. But if the prefered resolution in the robot platform is
//lower than full HD, "calibrationScaling" is used to convert correct values.
double calibrationScaling = (double)Program.resolution.Y / (double)Program.calibrationResolution.Y;
//When calibrating a camera, glyphs could be placed at max 4 positions, one in each image quadrant.
//This loop finds the glyphs in each camera and saves their coordinats in camCoordUpperLeft, camCoordUpperRight
//camCoordLowerLeft or camCoordLowerRight depending on in what quadrant of the camera image they are found.
for (int i = 0; i < activeCameras.Count; i++)
{
Camera cam = activeCameras[i];
//Recieves all glyphs found in the camera image
List <ExtractedGlyphData> glyphs = Program.imageProcessing.GetGlyphPosition(CameraController.GrabOneFrame(cam));
foreach (ExtractedGlyphData glyphData in glyphs)
{
IntPoint[] glyphCorners = new IntPoint[4];
//Recieves the coordinates of the 4 corners of the current glyph
glyphData.Quadrilateral.CopyTo(glyphCorners);
//Calculates the center coordinate of the glyph
DoublePoint glyphCoord = (DoublePoint)(glyphCorners[0] + glyphCorners[1] + glyphCorners[2] + glyphCorners[3]) / 4;
int glyphID = Convert.ToInt32(glyphData.RecognizedGlyph.Name);
if (glyphCoord.X <= Program.resolution.X * 0.5)
{
//Glyph found in 2nd quadrant
if (glyphCoord.Y <= Program.resolution.Y * 0.5)
{
cam.camCoordUpperLeft = glyphCoord;
cam.IdUpperLeft = glyphID;
}
//Glyph found in 3rd quadrant
else if (glyphCoord.Y >= Program.resolution.Y * 0.5)
{
cam.camCoordLowerLeft = glyphCoord;
cam.IdLowerLeft = glyphID;
}
}
else if (glyphCoord.X >= Program.resolution.X * 0.5)
{
//Glyph found in 1st quadrant
if (glyphCoord.Y <= Program.resolution.Y * 0.5)
{
cam.camCoordUpperRight = glyphCoord;
cam.IdUpperRight = glyphID;
}
//Glyph found in 4th quadrant
else if (glyphCoord.Y >= Program.resolution.Y * 0.5)
{
cam.camCoordLowerRight = glyphCoord;
cam.IdLowerRight = glyphID;
}
}
}
MessageBox.Show("Cam number: " + cam.camNumber + "\r\n" +
"UpperLeft: " + cam.camCoordUpperLeft.ToString() + " " + "ID: " + cam.IdUpperLeft + "\r\n" +
"UpperRight: " + cam.camCoordUpperRight.ToString() + " " + "ID: " + cam.IdUpperRight + "\r\n" +
"LowerLeft: " + cam.camCoordLowerLeft.ToString() + " " + "ID: " + cam.IdLowerLeft + "\r\n" +
"LowerRight: " + cam.camCoordLowerRight.ToString() + " " + "ID: " + cam.IdLowerRight);
//Determines if the camera is a middle camera, upper left camera, upper camera or left camera.
//Id = -1 means that no glyphs where found in that quadrant
if (cam.IdUpperLeft != -1 && cam.IdUpperRight != -1 && cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
//Middle camera found
}
else if (cam.IdUpperRight != -1 && cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
upperLeftCameraFound = true;
upperLeftCamera = cam;
}
else if (cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
upperCameraFound = true;
upperCamera = cam;
}
else if (cam.IdUpperRight != -1 && cam.IdLowerRight != -1)
{
leftCameraFound = true;
leftCamera = cam;
}
}
//Determines the first camera.
if (upperLeftCameraFound)
{
firstCamera = upperLeftCamera;
}
else if (upperCameraFound)
{
firstCamera = upperCamera;
}
else if (leftCameraFound)
{
firstCamera = leftCamera;
}
else
{
MessageBox.Show("No first camera was found!\r\nRearrange the glyphs and try again!");
}
List <Camera> calibratedCams = new List <Camera>();
List <Camera> unCalibratedCams = new List <Camera>();
//Places the first camera in calibratedCams, which means that the camera has been calibrated and
//placed in the global coordinate system. Also sets the calibration parameters of the first camera.
calibratedCams.Add(firstCamera);
calibratedCams[0].angleToRef = 0;
calibratedCams[0].sizeScaling = 1;
calibratedCams[0].camPosition = new DoublePoint(0, 0);
//Places the rest of the cameras in unCalibratedCams
unCalibratedCams = activeCameras.Except <Camera>(calibratedCams).ToList <Camera>();
//Determines the calibration parameters of the cameras, i.e placing them in the global coordinate system and
//adding them to the sortedCamList.
for (int i = 0; i < calibratedCams.Count; i++)
{
Camera cam1 = calibratedCams[i];
for (int j = 0; j < unCalibratedCams.Count; j++)
{
Camera cam2 = unCalibratedCams[j];
//Uncalibrated cam2 is above cam1
if (cam1.IdUpperLeft == cam2.IdLowerLeft && cam1.IdUpperRight == cam2.IdLowerRight &&
cam1.IdUpperLeft != -1 && cam2.IdLowerLeft != -1 && cam1.IdUpperRight != -1 && cam2.IdLowerRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.camCoordLowerLeft, cam2.camCoordLowerRight);
cam2.sizeScaling = distDiff(cam1.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.camCoordLowerLeft, cam2.camCoordLowerRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is below cam1
else if (cam1.IdLowerLeft == cam2.IdUpperLeft && cam1.IdLowerRight == cam2.IdUpperRight &&
cam1.IdLowerLeft != -1 && cam2.IdUpperLeft != -1 && cam1.IdLowerRight != -1 && cam2.IdUpperRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordLowerLeft, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordUpperRight);
cam2.sizeScaling = distDiff(cam1.camCoordLowerLeft, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordUpperRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordUpperLeft, cam1.camCoordLowerLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is left of cam1
else if (cam1.IdLowerLeft == cam2.IdLowerRight && cam1.IdUpperLeft == cam2.IdUpperRight &&
cam1.IdLowerLeft != -1 && cam2.IdLowerRight != -1 && cam1.IdUpperLeft != -1 && cam2.IdUpperRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.camCoordLowerRight, cam2.camCoordUpperRight);
cam2.sizeScaling = distDiff(cam1.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.camCoordLowerRight, cam2.camCoordUpperRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordLowerRight, cam1.camCoordLowerLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is right of cam1
else if (cam1.IdUpperRight == cam2.IdUpperLeft && cam1.IdLowerRight == cam2.IdLowerLeft &&
cam1.IdUpperRight != -1 && cam2.IdUpperLeft != -1 && cam1.IdLowerRight != -1 && cam2.IdLowerLeft != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordUpperRight, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordLowerLeft);
cam2.sizeScaling = distDiff(cam1.camCoordUpperRight, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordLowerLeft);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
}
}
//To be able to draw an image of all the cameraimages merged, the size of the combined images needs
//to be determined. This size is saved in imgSize.
foreach (Camera cam in activeCameras)
{
Program.imgSize.X = System.Math.Max((int)cam.camPosition.X + Program.resolution.X, Program.imgSize.X);
Program.imgSize.Y = System.Math.Max((int)cam.camPosition.Y + Program.resolution.Y, Program.imgSize.Y);
}
//Saves the calibration parameters to a save file so you don't have to calebrate the cameras every time.
saveToFile();
//Restore the cameras to their prefered resolution.
Program.cameraController.setCameraResolution(preferedCameraResolution);
Program.cameraCalibrated = true;
Program.cameraController.setShowSmallCameraImage(true);
}
