/// <summary> /// Solve camera to screen calibration /// </summary> /// <param name="data">Calibration data</param> /// <param name="points">Point to screen dataset</param> /// <returns>Calibration result, view and projection matrix</returns> public ProjectorCalibrationResult Solve(ProjectorCalibrationData data, IReadOnlyList<CameraToScreenPoint> points) { if (data == null) throw new ArgumentNullException("data"); if (points == null) throw new ArgumentNullException("points"); if (points.Count < 5) throw new ArgumentOutOfRangeException("points", "Calibration data should contain at least 4 points"); Vector3[] worldPoints = points.Select(cp => cp.CameraPoint).ToArray(); Vector2[] imagePoints = points.Select(cp => cp.ScreenPoint).ToArray(); IntPtr result; IntPtr dist, t, r; Vector3[] dst = ObjectPoints(worldPoints); fixed (Vector2* pImg = &imagePoints[0]) { fixed (Vector3* pDst = &dst[0]) { int res = CalibrateCamera(pImg, pDst, worldPoints.Length, (int)data.SensorSize.X, (int)data.SensorSize.Y, 0, out result, out dist, out t, out r); } } float* trans = (float*)t; Matrix view = Matrix.Identity; view.M41 = trans[0]; view.M42 = trans[1]; view.M43 = trans[2]; float* rot = (float*)r; view.M11 = rot[0]; view.M21 = rot[1]; view.M31 = rot[2]; view.M12 = rot[3]; view.M22 = rot[4]; view.M32 = rot[5]; view.M13 = rot[6]; view.M23 = rot[7]; view.M33 = rot[8]; view = FlipMatrix(view); float* proj = (float*)result; Matrix p = Matrix.Identity; p.M11 = proj[0]; p.M21 = proj[1]; p.M31 = proj[2]; p.M12 = proj[3]; p.M22 = proj[4]; p.M32 = proj[5]; p.M13 = proj[6]; p.M23 = proj[7]; p.M33 = proj[8]; p.M34 = 1.0f; p.M44 = 0; float invx = 2.0f / data.SensorSize.X; float invy = 2.0f / data.SensorSize.Y; Matrix camView = Matrix.Translation(-1.0f, 1.0f, 0.0f); camView = Matrix.Scaling(invx, -invy, 1.0f) * camView; Matrix pcam = p * camView * Matrix.Scaling(1, -1, 1); float n = data.Near; float f = data.Far; float q = f / (f - n); float qf = -2.0f * q * n; pcam.M33 = q; pcam.M43 = qf; FreeMemory(result); FreeMemory(dist); FreeMemory(t); FreeMemory(r); return new ProjectorCalibrationResult(view, pcam); }
/// <summary> /// Solve camera to screen calibration /// </summary> /// <param name="data">Calibration data</param> /// <param name="points">Point to screen dataset</param> /// <returns>Calibration result, view and projection matrix</returns> public ProjectorCalibrationResult Solve(ProjectorCalibrationData data, IReadOnlyList <CameraToScreenPoint> points) { if (data == null) { throw new ArgumentNullException("data"); } if (points == null) { throw new ArgumentNullException("points"); } if (points.Count < 5) { throw new ArgumentOutOfRangeException("points", "Calibration data should contain at least 4 points"); } Vector3[] worldPoints = points.Select(cp => cp.CameraPoint).ToArray(); Vector2[] imagePoints = points.Select(cp => cp.ScreenPoint).ToArray(); IntPtr result; IntPtr dist, t, r; Vector3[] dst = ObjectPoints(worldPoints); fixed(Vector2 *pImg = &imagePoints[0]) { fixed(Vector3 *pDst = &dst[0]) { int res = CalibrateCamera(pImg, pDst, worldPoints.Length, (int)data.SensorSize.X, (int)data.SensorSize.Y, 0, out result, out dist, out t, out r); } } float *trans = (float *)t; Matrix view = Matrix.Identity; view.M41 = trans[0]; view.M42 = trans[1]; view.M43 = trans[2]; float *rot = (float *)r; view.M11 = rot[0]; view.M21 = rot[1]; view.M31 = rot[2]; view.M12 = rot[3]; view.M22 = rot[4]; view.M32 = rot[5]; view.M13 = rot[6]; view.M23 = rot[7]; view.M33 = rot[8]; view = FlipMatrix(view); float *proj = (float *)result; Matrix p = Matrix.Identity; p.M11 = proj[0]; p.M21 = proj[1]; p.M31 = proj[2]; p.M12 = proj[3]; p.M22 = proj[4]; p.M32 = proj[5]; p.M13 = proj[6]; p.M23 = proj[7]; p.M33 = proj[8]; p.M34 = 1.0f; p.M44 = 0; float invx = 2.0f / data.SensorSize.X; float invy = 2.0f / data.SensorSize.Y; Matrix camView = Matrix.Translation(-1.0f, 1.0f, 0.0f); camView = Matrix.Scaling(invx, -invy, 1.0f) * camView; Matrix pcam = p * camView * Matrix.Scaling(1, -1, 1); float n = data.Near; float f = data.Far; float q = f / (f - n); float qf = -2.0f * q * n; pcam.M33 = q; pcam.M43 = qf; FreeMemory(result); FreeMemory(dist); FreeMemory(t); FreeMemory(r); return(new ProjectorCalibrationResult(view, pcam)); }