// 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);
}
