// Equivalent to transformation_unproject_internal(). // Size of uv and xy should be 2. public static bool Unproject(KinectIntrinsics intrinsics, float metricRadius, float[] uv, ref float[] xy, ref int valid) { float cx = intrinsics.cx; float cy = intrinsics.cy; float fx = intrinsics.fx; float fy = intrinsics.fy; float k1 = intrinsics.k1; float k2 = intrinsics.k2; float k3 = intrinsics.k3; float k4 = intrinsics.k4; float k5 = intrinsics.k5; float k6 = intrinsics.k6; float codx = intrinsics.codx; float cody = intrinsics.cody; float p1 = intrinsics.p1; float p2 = intrinsics.p2; if (!(fx > 0.0f && fy > 0.0f)) { return(false); } // correction for radial distortion float xp_d = (uv[0] - cx) / fx - codx; float yp_d = (uv[1] - cy) / fy - cody; float rs = xp_d * xp_d + yp_d * yp_d; float rss = rs * rs; float rsc = rss * rs; float a = 1.0f + k1 * rs + k2 * rss + k3 * rsc; float b = 1.0f + k4 * rs + k5 * rss + k6 * rsc; float ai; if (a != 0.0f) { ai = 1.0f / a; } else { ai = 1.0f; } float di = ai * b; xy[0] = xp_d * di; xy[1] = yp_d * di; // approximate correction for tangential params float two_xy = 2.0f * xy[0] * xy[1]; float xx = xy[0] * xy[0]; float yy = xy[1] * xy[1]; xy[0] -= (yy + 3.0f * xx) * p2 + two_xy * p1; xy[1] -= (xx + 3.0f * yy) * p1 + two_xy * p2; // add on center of distortion xy[0] += codx; xy[1] += cody; return(InterativeUnproject(intrinsics, metricRadius, uv, ref xy, ref valid, 20)); }
// Equivalent to transformation_iterative_unproject(). // Size of uv and xy should be 2. private static bool InterativeUnproject(KinectIntrinsics intrinsics, float metricRadius, float[] uv, ref float[] xy, ref int valid, int maxPasses) { valid = 1; float[] Jinv = new float[4]; float[] best_xy = new float[2] { 0.0f, 0.0f }; float best_err = float.MaxValue; for (int pass = 0; pass < maxPasses; ++pass) { float[] p = new float[2]; float[] J = new float[4]; if (!Project(intrinsics, metricRadius, xy, ref p, ref valid, ref J)) { return(false); } if (valid == 0) { return(true); } float err_x = uv[0] - p[0]; float err_y = uv[1] - p[1]; float err = err_x * err_x + err_y * err_y; if (err >= best_err) { xy[0] = best_xy[0]; xy[1] = best_xy[1]; break; } best_err = err; best_xy[0] = xy[0]; best_xy[1] = xy[1]; Invert2x2(J, ref Jinv); if (pass + 1 == maxPasses || best_err < 1e-22f) { break; } float dx = Jinv[0] * err_x + Jinv[1] * err_y; float dy = Jinv[2] * err_x + Jinv[3] * err_y; xy[0] += dx; xy[1] += dy; } if (best_err > 1e-6f) { valid = 0; } return(true); }
// Equivalent to transformation_project_internal(). // Size of uv and xy should be 2. // Size of J should be 4. private static bool Project(KinectIntrinsics intrinsics, float metricRadius, float[] xy, ref float[] uv, ref int valid, ref float[] J_xy) { float cx = intrinsics.cx; float cy = intrinsics.cy; float fx = intrinsics.fx; float fy = intrinsics.fy; float k1 = intrinsics.k1; float k2 = intrinsics.k2; float k3 = intrinsics.k3; float k4 = intrinsics.k4; float k5 = intrinsics.k5; float k6 = intrinsics.k6; float codx = intrinsics.codx; float cody = intrinsics.cody; float p1 = intrinsics.p1; float p2 = intrinsics.p2; float max_radius_for_projection = metricRadius; if (!((fx > 0.0f && fy > 0.0f))) { return(false); } valid = 1; float xp = xy[0] - codx; float yp = xy[1] - cody; float xp2 = xp * xp; float yp2 = yp * yp; float xyp = xp * yp; float rs = xp2 + yp2; if (rs > max_radius_for_projection * max_radius_for_projection) { valid = 0; return(true); } float rss = rs * rs; float rsc = rss * rs; float a = 1.0f + k1 * rs + k2 * rss + k3 * rsc; float b = 1.0f + k4 * rs + k5 * rss + k6 * rsc; float bi; if (b != 0.0f) { bi = 1.0f / b; } else { bi = 1.0f; } float d = a * bi; float xp_d = xp * d; float yp_d = yp * d; float rs_2xp2 = rs + 2.0f * xp2; float rs_2yp2 = rs + 2.0f * yp2; xp_d += rs_2xp2 * p2 + 2.0f * xyp * p1; yp_d += rs_2yp2 * p1 + 2.0f * xyp * p2; float xp_d_cx = xp_d + codx; float yp_d_cy = yp_d + cody; uv[0] = xp_d_cx * fx + cx; uv[1] = yp_d_cy * fy + cy; if (J_xy == null) { return(true); } // compute Jacobian matrix float dudrs = k1 + 2.0f * k2 * rs + 3.0f * k3 * rss; // compute d(b)/d(r^2) float dvdrs = k4 + 2.0f * k5 * rs + 3.0f * k6 * rss; float bis = bi * bi; float dddrs = (dudrs * b - a * dvdrs) * bis; float dddrs_2 = dddrs * 2.0f; float xp_dddrs_2 = xp * dddrs_2; float yp_xp_dddrs_2 = yp * xp_dddrs_2; J_xy[0] = fx * (d + xp * xp_dddrs_2 + 6.0f * xp * p2 + 2.0f * yp * p1); J_xy[1] = fx * (yp_xp_dddrs_2 + 2.0f * yp * p2 + 2.0f * xp * p1); J_xy[2] = fy * (yp_xp_dddrs_2 + 2.0f * xp * p1 + 2.0f * yp * p2); J_xy[3] = fy * (d + yp * yp * dddrs_2 + 6.0f * yp * p1 + 2.0f * xp * p2); return(true); }