private void drawCamera(RegisteredCamera thisCamera)
{
// Set transform matrices.
float aspect = GraphicsDevice.Viewport.AspectRatio;
effect.World = Matrix.Identity; //Matrix.CreateScale(cameraModelScaling) * thisCamera.RotationMatrix;
effect.World = effect.World * Matrix.CreateTranslation(thisCamera.PositionVector); // *Matrix.CreateTranslation(viewTranslationVector);
//effect.World = effect.World * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity);
//effect.World = effect.World * Matrix.CreateScale(globalScaling);
//effect.View = Matrix.CreateLookAt(new Vector3(0, 0, -5),
// Vector3.Zero, Vector3.Up);
effect.View = ViewMatrix; //Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity) * Matrix.CreateLookAt(new Vector3(0, 0, -5 * globalScaling),
// Vector3.Zero, Vector3.Up);
effect.Projection = Matrix.CreatePerspectiveFieldOfView(1, aspect, 0.01f, 100);
// Set renderstates.
GraphicsDevice.RasterizerState = RasterizerState.CullNone;
// Draw the triangle.
effect.CurrentTechnique.Passes[0].Apply();
GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleList,
Vertices, 0, 1);
}
/// <summary>
/// Draws the control.
/// </summary>
protected override void Draw()
{
if (cameraConfig != null)
{
float averageX = cameraConfig.Cameras.Select(x => x.PositionVector.X).Average();
float averageY = cameraConfig.Cameras.Select(x => x.PositionVector.Y).Average();
float averageZ = cameraConfig.Cameras.Select(x => x.PositionVector.Z).Average();
if (IRLocationRender)
{
//ViewMatrix = Matrix.CreateTranslation(-currentTrackX, -currentTrackY, -currentTrackZ);
//ViewMatrix = ViewMatrix;// * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, roll * dragSensitivity, pitch * dragSensitivity);
ViewMatrix = Matrix.Identity;
if (ViewTrackModel)
{
ViewMatrix = ViewMatrix * Matrix.CreateLookAt(new Vector3(currentTrackX, currentTrackY, currentTrackZ), new Vector3(ModelX, ModelY, ModelZ), new Vector3(customUpX, customUpY, customUpZ));
}
else
{
ViewMatrix = Matrix.CreateTranslation(-currentTrackX, -currentTrackY, -currentTrackZ);
ViewMatrix = ViewMatrix * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, roll * dragSensitivity, pitch * dragSensitivity);
}
}
else
{
ViewMatrix = Matrix.CreateTranslation(-averageX, -averageY, -averageZ);
ViewMatrix = ViewMatrix * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, roll * dragSensitivity, pitch * dragSensitivity);
ViewMatrix = ViewMatrix * Matrix.CreateTranslation(0, 0, -(5 * globalScaling));// *Matrix.CreateReflection(new Plane(new Vector4(1, 1, 0, 0)));
}
GraphicsDevice.Clear(Color.CornflowerBlue);
try
{
if (cameraConfig != null)
{
IntersectionLines.Clear();
for (int i = 0; i < cameraConfig.Cameras.Count; i++)
{
RegisteredCamera thisCamera = cameraConfig.Cameras[i];
drawCamera(thisCamera);
//drawCamera2(thisCamera);
drawRays(thisCamera, i);
}
//drawPoints(cameraConfig);
drawIntersections();
if (ShowModel)
{
drawModel(ModelX, ModelY, ModelZ);
}
}
}
catch (Exception ex)
{
}
}
}
private void CreateFromBundlerFile(string configFile)
{
string text = System.IO.File.ReadAllText(configFile);
string[] lines = text.Split('\r');
int numCameras = int.Parse(lines[1].Split(' ')[0]);
int numPoints = int.Parse(lines[1].Split(' ')[1]);
for (int i = 0; i < numCameras; i++)
{
//Extract the focal length and the radial distrortion cooefficients
string[] line1split = lines[2 + i*5].Split(' ');
float focalLength = float.Parse(line1split[0]);
float k1 = float.Parse(line1split[1]);
float k2 = float.Parse(line1split[2]);
//Camera Rotation matrix
//m1, m2, m3
//m4, m5, m6
//m7, m8, m9
string[] line2split = lines[3 + i * 5].Split(' ');
float m1 = float.Parse(line2split[0]);
float m2 = float.Parse(line2split[1]);
float m3 = float.Parse(line2split[2]);
string[] line3split = lines[4 + i * 5].Split(' ');
float m4 = float.Parse(line3split[0]);
float m5 = float.Parse(line3split[1]);
float m6 = float.Parse(line3split[2]);
string[] line4split = lines[5 + i * 5].Split(' ');
float m7 = float.Parse(line4split[0]);
float m8 = float.Parse(line4split[1]);
float m9 = float.Parse(line4split[2]);
//Camera Translation Matrix
string[] line5split = lines[6 + i * 5].Split(' ');
float t1 = float.Parse(line5split[0]);
float t2 = float.Parse(line5split[1]);
float t3 = float.Parse(line5split[2]);
//Create the new camera and store:
Matrix RotationMatrix = new Matrix(m1, m2, m3, 0, m4, m5, m6, 0, m7, m8, m9, 0, 0, 0 , 0, 1);
Vector3 TranslationVector = new Vector3(t1, t2, t3);
RegisteredCamera thisCamera = new RegisteredCamera(focalLength, k1, k2, RotationMatrix, TranslationVector, 480, 640);
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(320, 240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(320, -240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(-320, 240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(-320, -240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(0, 0));
//Add some matched image points, to check the calibration...
System.IO.DirectoryInfo thisDIR = System.IO.Directory.GetParent(configFile);
System.IO.DirectoryInfo upDIR = System.IO.Directory.GetParent(thisDIR.ToString());
string sDIR = upDIR.ToString();
string keyText = System.IO.File.ReadAllText(sDIR + "\\" + i + ".key");
string[] keyLines = keyText.Split('\r');
for (int y = 0; y < 20; y++)
{
string[] matchcoords = keyLines[1+(y*8)].Split(' ');
float matchY = float.Parse(matchcoords[0]);
float matchX = float.Parse(matchcoords[1]);
thisCamera.TrackedPoints.Add(new TrackedImagePoint(matchX, matchY));
}
Cameras.Add(thisCamera);
}
int startLine = 1 + 6 + (numCameras - 1) * 5;
CameraMatches cameraMatches = new CameraMatches();
//Get the model points
for (int i = 0; i < numPoints; i++)
{
string[] linesplit = lines[startLine + i * 3].Split(' ');
float x = float.Parse(linesplit[0]);
float y = float.Parse(linesplit[1]);
float z = float.Parse(linesplit[2]);
Point3D thisPoint = new Point3D(x, y, z);
cameraMatches.points.Add(thisPoint);
}
modelPoints = cameraMatches;
}
private void drawRays(RegisteredCamera thisCamera, int i)
{
//for each tracked point project from the camera centre to the point:
float aspect = GraphicsDevice.Viewport.AspectRatio;
effect.World = Matrix.Identity * Matrix.CreateTranslation(viewTranslationVector);
//effect.World = effect.World * Matrix.CreateTranslation(thisCamera.PositionVector);
//effect.World = effect.World * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity);
//effect.World = effect.World * Matrix.CreateScale(globalScaling);
//effect.View = Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity) * Matrix.CreateLookAt(new Vector3(0, 0, -5 * globalScaling),
// Vector3.Zero, Vector3.Up);
effect.View = ViewMatrix;
effect.Projection = Matrix.CreatePerspectiveFieldOfView(1, aspect, 0.01f, 100);
//for (int i = 0; i < thisCamera.TrackedPoints.Count; i++)
//{
//int i = (int)rotAngleX;
//if (i < thisCamera.TrackedPoints.Count)
//{
//TrackedImagePoint thisPoint = thisCamera.TrackedPoints[i];
int testy = 0;
//if (WebCamPermute != null)
//{
//int thisCamIndex = WebCamPermute[i];
int thisCamIndex = i;
if (thisCamIndex < WebCamsEye.Count)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count > 0)
{
//if (WebCamPermute.Count > 0)
// {
try
{
for (int r = 0; r < WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count; r++)
{
WebCamTrack thisTrack = WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints[r];
if (thisTrack.Points.Count > 0)
{
Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points.Select(x => x.X).Average(), thisTrack.Points.Select(x => x.Y).Average(), -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
//Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points[0].X, thisTrack.Points[0].Y, -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
Vector3 startPoint = new Vector3(thisCamera.PositionVector.X, thisCamera.PositionVector.Y, thisCamera.PositionVector.Z);
Vector3 endPoint = new Vector3(WordCoordinates.X, (float)WordCoordinates.Y, (float)WordCoordinates.Z);
//trackInCamera.Add(thisCamIndex);
IntersectionLines.Add(new Line3D { LineStart = new Point3D(startPoint.X, startPoint.Y, startPoint.Z), LineEnd = new Point3D(endPoint.X, endPoint.Y, endPoint.Z) });
effect.CurrentTechnique.Passes[0].Apply();
var vertices = new[] { new VertexPositionColor(startPoint, Microsoft.Xna.Framework.Color.Red), new VertexPositionColor(endPoint, Microsoft.Xna.Framework.Color.Red) };
effect.GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vertices, 0, 1);
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
catch (Exception test)
{
testy = 1;
}
return;
//}
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
//}
//IntersectionLines[thisCamIndex].Displ
}
}
private void drawCamera2(RegisteredCamera thisCamera)
{
// Set transform matrices.
float aspect = GraphicsDevice.Viewport.AspectRatio;
effect.World = Matrix.CreateScale(cameraModelScaling) * thisCamera.RotationMatrix;
effect.World = effect.World * Matrix.CreateTranslation(thisCamera.TranslationVector);
effect.World = effect.World * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity);
effect.World = effect.World * Matrix.CreateScale(globalScaling);
effect.View = Matrix.CreateLookAt(new Vector3(0, 0, -5),
Vector3.Zero, Vector3.Up);
effect.Projection = Matrix.CreatePerspectiveFieldOfView(1, aspect, 0.01f, 100);
// Set renderstates.
GraphicsDevice.RasterizerState = RasterizerState.CullNone;
// Draw the triangle.
effect.CurrentTechnique.Passes[0].Apply();
GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleList,
Vertices, 0, 1);
}
private void CreateFromBundlerFile(string configFile)
{
string text = System.IO.File.ReadAllText(configFile);
string[] lines = text.Split('\r');
int numCameras = int.Parse(lines[1].Split(' ')[0]);
int numPoints = int.Parse(lines[1].Split(' ')[1]);
for (int i = 0; i < numCameras; i++)
{
//Extract the focal length and the radial distrortion cooefficients
string[] line1split = lines[2 + i * 5].Split(' ');
float focalLength = float.Parse(line1split[0]);
float k1 = float.Parse(line1split[1]);
float k2 = float.Parse(line1split[2]);
//Camera Rotation matrix
//m1, m2, m3
//m4, m5, m6
//m7, m8, m9
string[] line2split = lines[3 + i * 5].Split(' ');
float m1 = float.Parse(line2split[0]);
float m2 = float.Parse(line2split[1]);
float m3 = float.Parse(line2split[2]);
string[] line3split = lines[4 + i * 5].Split(' ');
float m4 = float.Parse(line3split[0]);
float m5 = float.Parse(line3split[1]);
float m6 = float.Parse(line3split[2]);
string[] line4split = lines[5 + i * 5].Split(' ');
float m7 = float.Parse(line4split[0]);
float m8 = float.Parse(line4split[1]);
float m9 = float.Parse(line4split[2]);
//Camera Translation Matrix
string[] line5split = lines[6 + i * 5].Split(' ');
float t1 = float.Parse(line5split[0]);
float t2 = float.Parse(line5split[1]);
float t3 = float.Parse(line5split[2]);
//Create the new camera and store:
Matrix RotationMatrix = new Matrix(m1, m2, m3, 0, m4, m5, m6, 0, m7, m8, m9, 0, 0, 0, 0, 1);
Vector3 TranslationVector = new Vector3(t1, t2, t3);
RegisteredCamera thisCamera = new RegisteredCamera(focalLength, k1, k2, RotationMatrix, TranslationVector, 480, 640);
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(320, 240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(320, -240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(-320, 240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(-320, -240));
//thisCamera.TrackedPoints.Add(new TrackedImagePoint(0, 0));
//Add some matched image points, to check the calibration...
System.IO.DirectoryInfo thisDIR = System.IO.Directory.GetParent(configFile);
System.IO.DirectoryInfo upDIR = System.IO.Directory.GetParent(thisDIR.ToString());
string sDIR = upDIR.ToString();
string keyText = System.IO.File.ReadAllText(sDIR + "\\" + i + ".key");
string[] keyLines = keyText.Split('\r');
for (int y = 0; y < 20; y++)
{
string[] matchcoords = keyLines[1 + (y * 8)].Split(' ');
float matchY = float.Parse(matchcoords[0]);
float matchX = float.Parse(matchcoords[1]);
thisCamera.TrackedPoints.Add(new TrackedImagePoint(matchX, matchY));
}
Cameras.Add(thisCamera);
}
int startLine = 1 + 6 + (numCameras - 1) * 5;
CameraMatches cameraMatches = new CameraMatches();
//Get the model points
for (int i = 0; i < numPoints; i++)
{
string[] linesplit = lines[startLine + i * 3].Split(' ');
float x = float.Parse(linesplit[0]);
float y = float.Parse(linesplit[1]);
float z = float.Parse(linesplit[2]);
Point3D thisPoint = new Point3D(x, y, z);
cameraMatches.points.Add(thisPoint);
}
modelPoints = cameraMatches;
}
private void drawRays(RegisteredCamera thisCamera, int i)
{
//for each tracked point project from the camera centre to the point:
float aspect = GraphicsDevice.Viewport.AspectRatio;
effect.World = Matrix.Identity * Matrix.CreateTranslation(viewTranslationVector);
//effect.World = effect.World * Matrix.CreateTranslation(thisCamera.PositionVector);
//effect.World = effect.World * Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity);
//effect.World = effect.World * Matrix.CreateScale(globalScaling);
//effect.View = Matrix.CreateFromYawPitchRoll(yaw * dragSensitivity, pitch * dragSensitivity, roll * dragSensitivity) * Matrix.CreateLookAt(new Vector3(0, 0, -5 * globalScaling),
// Vector3.Zero, Vector3.Up);
effect.View = ViewMatrix;
effect.Projection = Matrix.CreatePerspectiveFieldOfView(1, aspect, 0.01f, 100);
//for (int i = 0; i < thisCamera.TrackedPoints.Count; i++)
//{
//int i = (int)rotAngleX;
//if (i < thisCamera.TrackedPoints.Count)
//{
//TrackedImagePoint thisPoint = thisCamera.TrackedPoints[i];
int testy = 0;
//if (WebCamPermute != null)
//{
//int thisCamIndex = WebCamPermute[i];
int thisCamIndex = i;
if (thisCamIndex < WebCamsEye.Count)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints != null)
{
if (WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count > 0)
{
//if (WebCamPermute.Count > 0)
// {
try
{
for (int r = 0; r < WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints.Count; r++)
{
WebCamTrack thisTrack = WebCamsEye[thisCamIndex].FilteredTrackedPoints.TrackedPoints[r];
if (thisTrack.Points.Count > 0)
{
Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points.Select(x => x.X).Average(), thisTrack.Points.Select(x => x.Y).Average(), -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
//Vector3 WordCoordinates = thisCamera.ImageToWorld(thisTrack.Points[0].X, thisTrack.Points[0].Y, -100, transposeRot, negateRot, invertRot, invertZ, rotAngleX, rotAngleY, rotAngleZ);
Vector3 startPoint = new Vector3(thisCamera.PositionVector.X, thisCamera.PositionVector.Y, thisCamera.PositionVector.Z);
Vector3 endPoint = new Vector3(WordCoordinates.X, (float)WordCoordinates.Y, (float)WordCoordinates.Z);
//trackInCamera.Add(thisCamIndex);
IntersectionLines.Add(new Line3D {
LineStart = new Point3D(startPoint.X, startPoint.Y, startPoint.Z), LineEnd = new Point3D(endPoint.X, endPoint.Y, endPoint.Z)
});
effect.CurrentTechnique.Passes[0].Apply();
var vertices = new[] { new VertexPositionColor(startPoint, Microsoft.Xna.Framework.Color.Red), new VertexPositionColor(endPoint, Microsoft.Xna.Framework.Color.Red) };
effect.GraphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vertices, 0, 1);
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
catch (Exception test)
{
testy = 1;
}
return;
//}
}
else
{
//trackInCamera[thisCamIndex] = false;
}
}
}
//}
//IntersectionLines[thisCamIndex].Displ
}
}