public static void CalibrationError( Emgu.CV.ExtrinsicCameraParameters ecp, Emgu.CV.IntrinsicCameraParameters icp, System.Drawing.PointF[] image_points, Vector[] reference_points, out double[] deviations, out Vector[] isect_points) { // Shoot rays through image points, // intersect with plane defined by extrinsic // and measure distance to reference points Matrix inv_ecp = Matrix.Identity(4, 4); inv_ecp.SetMatrix(0, 2, 0, 3, ecp.ExtrinsicMatrix.ToParsley()); inv_ecp = inv_ecp.Inverse(); Ray[] rays = Ray.EyeRays(icp, image_points); Plane p = new Plane(ecp); isect_points = new Vector[rays.Length]; deviations = new double[rays.Length]; for (int i = 0; i < rays.Length; ++i) { double t; Intersection.RayPlane(rays[i], p, out t); Vector isect = rays[i].At(t); Vector x = new Vector(new double[]{isect[0],isect[1],isect[2],1}); x = (inv_ecp * x.ToColumnMatrix()).GetColumnVector(0); Vector final = new Vector(new double[]{x[0], x[1], x[2]}); isect_points[i] = final; deviations[i] = (final - reference_points[i]).Norm(); } }
public void RayPlane() { Plane p = new Plane(MakeVector(1, 1, 1), MakeVector(0, 0, 1)); double t; Assert.True(Intersection.RayPlane(new Ray(MakeVector(0, 0, 1)), p, out t)); Assert.AreEqual(1.0, t, 0.00001); Assert.False(Intersection.RayPlane(new Ray(MakeVector(0, 1, 0)), p, out t)); }
public void ConstructorPointNormal() { Plane p = new Plane(MakeVector(0, 0, 0), MakeVector(0, 0, 10)); Assert.AreEqual(0, p.D); Assert.AreEqual(1, p.Normal.Norm(), 0.00001); Assert.AreEqual(0, (MakeVector(0, 0, 1) - p.Normal).Norm(), 0.00001); p = new Plane(MakeVector(1, 1, 1), MakeVector(0, 0, 1)); Assert.AreEqual(p.D, -1, 0.00001); }
/// <summary> /// Ray/Plane intersection /// </summary> /// <param name="r">Ray</param> /// <param name="p">Plane</param> /// <param name="t">Parametric t</param> /// <returns></returns> public static bool RayPlane(Ray r, Plane p, out double t) { // http://www.siggraph.org/education/materials/HyperGraph/raytrace/rayplane_intersection.htm double denom = Vector.ScalarProduct(p.Normal, r.Direction); if (Math.Abs(denom) > 1e-10) { // Since the ray passes through the origin, the intersection calculation can // be greatly simplified. t = -p.D / denom; return true; } else { t = Double.MaxValue; return false; } }
/// <summary> /// Construct plane from three points /// </summary> /// <param name="a">First point</param> /// <param name="b">Second point</param> /// <param name="c">Third point</param> /// <param name="plane">Created plane</param> /// <returns>True points are not colinear, false otherwise</returns> public static bool FromPoints(Vector a, Vector b, Vector c, out Plane plane) { bool not_colinear = false; Vector n = Vector.CrossProduct(b - a, c - a); double norm = n.Norm(); if (norm > 0) { n.ScaleInplace(1.0 / norm); double d = -Vector.ScalarProduct(a, n); plane = new Plane(n, d); not_colinear = true; } else { plane = null; } return not_colinear; }
public void FitByAveraging() { Plane p = new Plane(MakeVector(1, 1, 1), MakeVector(1, 1, 1).Normalize()); Vector[] v = PlaneTest.RandomPointsOnPlane(p, 1000); Profile prof = new Profile("average-fit"); Plane r; Assert.True(Plane.FitByAveraging(v, out r)); prof.Dispose(); Assert.AreEqual(1.0, Math.Abs(Vector.ScalarProduct(r.Normal, p.Normal)), 0.00001); Assert.AreEqual(Math.Abs(r.D) - Math.Abs(p.D), 0, 0.00001); Assert.False(Plane.FitByPCA(new Vector[] { MakeVector(0, 0, 0) }, out r)); }
public static Vector[] RandomPointsOnPlane(Plane p, int count) { // 2 free variables, last one is calculated Vector[] vecs = new Vector[count]; Vector n = p.Normal; int i = n[0] > 0 ? 0 : (n[1] > 0 ? 1 : 2); int j = (i + 1) % 3; int k = (i + 2) % 3; Random _r = new Random(); for (int x = 0; x < count; ++x) { Vector v = new Vector(3); v[j] = -100.0 + _r.NextDouble() * 200; v[k] = -100.0 + _r.NextDouble() * 200; v[i] = (- p.D - p.Normal[j] * v[j] - p.Normal[k] * v[k]) / p.Normal[i]; vecs[x] = v; } return vecs; }
/// <summary> /// Find best intersection of eye-rays and reference planes. /// </summary> /// <remarks>Best is defined as the the plane with the shortest distance</remarks> /// <param name="eye_rays">Eye-rays</param> /// <param name="planes">Calibrated reference planes</param> /// <param name="isect_t">Parametric ray intersection distance</param> /// <param name="isect_p">Intersection points (optional).</param> public static void FindEyeRayPlaneIntersections(Ray[] eye_rays, Plane[] planes, out double[] isect_t, out Vector[] isect_p, out int[] isect_plane_ids) { isect_t = new double[eye_rays.Length]; isect_p = new Vector[eye_rays.Length]; isect_plane_ids = new int[eye_rays.Length]; for (int i = 0; i < eye_rays.Length; ++i) { Ray r = eye_rays[i]; double t = Double.MaxValue; int id = -1; for (int p = 0; p < planes.Length; ++p) { double this_t; Intersection.RayPlane(r, planes[p], out this_t); if (this_t < t) { t = this_t; id = p; } } isect_t[i] = t; isect_p[i] = r.At(t); isect_plane_ids[i] = id; } }
/// <summary> /// Fit by averaging using newell's method /// "Real-Time Collision Detection" page 494 /// </summary> /// <remarks>Faster by a factor of 5 compared to PCA method above</remarks> /// <param name="points">Points to fit plane through</param> /// <returns>Fitted Plane</returns> public static bool FitByAveraging(IEnumerable<Vector> points, out Plane plane) { // Assume at least three points plane = null; IEnumerator<Vector> a = points.GetEnumerator(); IEnumerator<Vector> b = points.GetEnumerator(); Vector centroid = new Vector(3, 0.0); Vector normal = new Vector(3, 0.0); b.MoveNext(); int count = 0; while (a.MoveNext()) { if (!b.MoveNext()) { b = points.GetEnumerator(); // b.Reset(); Reset is not supported when using yield b.MoveNext(); } Vector i = a.Current; Vector j = b.Current; normal[0] += (i[1] - j[1]) * (i[2] + j[2]); // Project on yz normal[1] += (i[2] - j[2]) * (i[0] + j[0]); // Project on xz normal[2] += (i[0] - j[0]) * (i[1] + j[1]); // Project on xy centroid += j; count++; } if (count < 3) return false; centroid /= (double)count; plane = new Plane(centroid, normal.Normalize()); return true; }
/// <summary> /// Fit plane through points by linear orthogonal regression. /// </summary> /// <remarks>http://www.lsr.ei.tum.de/fileadmin/publications/K._Klasing/KlasingAlthoff-ComparisonOfSurfaceNormalEstimationMethodsForRangeSensingApplications_ICRA09.pdf</remarks> /// <param name="points">Points to fit by plane</param> /// <returns>Best-fit plane in terms of orthogonal least square regression.</returns> public static bool FitByPCA(IEnumerable<Vector> points, out Plane plane) { // Perform orth lin regression by PCA which requires the estimation // of the covariance matrix of the samples // http://en.wikipedia.org/wiki/Estimation_of_covariance_matrices plane = null; int count = 0; Vector mean = new Vector(3, 0.0); foreach (Vector p in points) { mean.AddInplace(p); count++; } if (count < 3) return false; mean /= (double)count; Matrix cov = new Matrix(3, 3, 0.0); foreach (Vector p in points) { Vector v = p - mean; //code below is equivalent to //cov += v.ToColumnMatrix() * v.ToRowMatrix(); //optimized double v00 = v[0] * v[0]; double v01 = v[0] * v[1]; double v02 = v[0] * v[2]; double v11 = v[1] * v[1]; double v12 = v[1] * v[2]; double v22 = v[2] * v[2]; cov[0, 0] += v00; cov[0, 1] += v01; cov[0, 2] += v02; cov[1, 0] += v01; cov[1, 1] += v11; cov[1, 2] += v12; cov[2, 0] += v02; cov[2, 1] += v12; cov[2, 2] += v22; } // Skip normalization of matrix try { EigenvalueDecomposition decomp = cov.EigenvalueDecomposition; plane = new Plane(mean, decomp.EigenVectors.GetColumnVector(0)); return true; } catch (IndexOutOfRangeException) { return false; } }
public void DistanceTo() { Plane p = new Plane(MakeVector(2, 2, 2), MakeVector(0, 0, 1)); Assert.AreEqual(-2, p.SignedDistanceTo(MakeVector(0, 0, 0)), 0.00001); Assert.AreEqual(0, p.SignedDistanceTo(MakeVector(0, 0, 2)), 0.00001); Assert.AreEqual(1, p.SignedDistanceTo(MakeVector(3, 2, 3)), 0.00001); }