// projects space point onto a plane protected override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point) { float x = point.x / point.z; float y = point.y / point.z; if (intr.distType == KinectInterop.DistortionType.ModifiedBrownConrady) { float r2 = x * x + y * y; float f = 1f + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2; x *= f; y *= f; float dx = x + 2f * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x); float dy = y + 2f * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y); x = dx; y = dy; } if (intr.distType == KinectInterop.DistortionType.Theta) { float r = (float)Math.Sqrt(x * x + y * y); float rd = (1f / intr.distCoeffs[0] * (float)Math.Atan(2f * r * (float)Math.Tan(intr.distCoeffs[0] / 2f))); x *= rd / r; y *= rd / r; } Vector2 pixel = new Vector2(x * intr.fx + intr.ppx, y * intr.fy + intr.ppy); return(pixel); }
// projects space point onto a plane protected override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point) { if (point == Vector3.zero) { return(Vector2.zero); } System.Numerics.Vector3 fPoint = new System.Numerics.Vector3(point.x, point.y, point.z); System.Numerics.Vector2?fPixel = coordMapper.TransformTo2D(fPoint, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType); Vector2 pixel = fPixel.HasValue ? new Vector2(fPixel.Value.X, fPixel.Value.Y) : Vector2.zero; return(pixel); }
// unprojects plane point into the space protected override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth) { if (depth <= 0f) { return(Vector3.zero); } System.Numerics.Vector2 fPixel = new System.Numerics.Vector2(pixel.x, pixel.y); System.Numerics.Vector3?fPoint = coordMapper.TransformTo3D(fPixel, depth, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType); Vector3 point = fPoint.HasValue ? new Vector3(fPoint.Value.X, fPoint.Value.Y, fPoint.Value.Z) : Vector3.zero; return(point); }
// unprojects plane point into the space protected override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth) { float x = (pixel.x - intr.ppx) / intr.fx; float y = (pixel.y - intr.ppy) / intr.fy; if (intr.distType == KinectInterop.DistortionType.InverseBrownConrady) { float r2 = x * x + y * y; float f = 1 + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2; float ux = x * f + 2 * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x); float uy = y * f + 2 * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y); x = ux; y = uy; } Vector3 point = new Vector3(depth * x, depth * y, depth); return(point); }
// gets the given camera intrinsics private void GetCameraIntrinsics(Intel.RealSense.Intrinsics camIntr, ref KinectInterop.CameraIntrinsics intr, int camType) { intr = new KinectInterop.CameraIntrinsics(); intr.cameraType = camType; intr.width = camIntr.width; intr.height = camIntr.height; intr.ppx = camIntr.ppx; intr.ppy = camIntr.ppy; intr.fx = camIntr.fx; intr.fy = camIntr.fy; intr.distCoeffs = new float[camIntr.coeffs.Length]; camIntr.coeffs.CopyTo(intr.distCoeffs, 0); intr.distType = (KinectInterop.DistortionType)camIntr.model; EstimateFOV(intr); }
// gets the given camera intrinsics private void GetCameraIntrinsics(CalibrationDeviceType camType, CameraCalibration camParams, ref KinectInterop.CameraIntrinsics intr) { Intrinsics camIntr = camParams.Intrinsics; if (camIntr.Parameters.Length < 15) { throw new System.Exception("Intrinsics length is less than expected: " + camIntr.ParameterCount); } intr = new KinectInterop.CameraIntrinsics(); intr.cameraType = (int)camType; intr.width = camParams.ResolutionWidth; intr.height = camParams.ResolutionHeight; // 0 float cx; // 1 float cy; intr.ppx = camIntr.Parameters[0]; intr.ppy = camIntr.Parameters[1]; // 2 float fx; /**< Focal length x */ // 3 float fy; /**< Focal length y */ intr.fx = camIntr.Parameters[2]; intr.fy = camIntr.Parameters[3]; // 4 float k1; // 5 float k2; // 6 float k3; // 7 float k4; // 8 float k5; // 9 float k6; intr.distCoeffs = new float[6]; intr.distCoeffs[0] = camIntr.Parameters[4]; intr.distCoeffs[1] = camIntr.Parameters[5]; intr.distCoeffs[2] = camIntr.Parameters[6]; intr.distCoeffs[3] = camIntr.Parameters[7]; intr.distCoeffs[4] = camIntr.Parameters[8]; intr.distCoeffs[5] = camIntr.Parameters[9]; if (camIntr.Type == CalibrationModelType.Theta) { intr.distType = KinectInterop.DistortionType.Theta; } else if (camIntr.Type == CalibrationModelType.Polynomial3K) { intr.distType = KinectInterop.DistortionType.Polynomial3K; } else if (camIntr.Type == CalibrationModelType.Rational6KT) { intr.distType = KinectInterop.DistortionType.Rational6KT; } else { intr.distType = (KinectInterop.DistortionType)camIntr.Type; } // 10 float codx; // 11 float cody; intr.codx = camIntr.Parameters[10]; intr.cody = camIntr.Parameters[11]; // 12 float p2; // 13 float p1; intr.p2 = camIntr.Parameters[12]; intr.p1 = camIntr.Parameters[13]; // 14 float metric_radius; intr.maxRadius = camIntr.Parameters[14]; EstimateFOV(intr); }
// estimates horizontal and vertical FOV protected void EstimateFOV(KinectInterop.CameraIntrinsics intr) { intr.hFOV = (Mathf.Atan2(intr.ppx + 0.5f, intr.fx) + Mathf.Atan2(intr.width - (intr.ppx + 0.5f), intr.fx)) * 57.2957795f; intr.vFOV = (Mathf.Atan2(intr.ppy + 0.5f, intr.fy) + Mathf.Atan2(intr.height - (intr.ppy + 0.5f), intr.fy)) * 57.2957795f; }
// projects space point onto a plane protected virtual Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point) { return(Vector2.zero); }
// unprojects plane point into the space protected virtual Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth) { return(Vector3.zero); }