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();
      }
    }
Ejemplo n.º 2
0
 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));
 }
Ejemplo n.º 3
0
    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);
    }
Ejemplo n.º 4
0
 /// <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;
   }
 }
Ejemplo n.º 5
0
 /// <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;
 }
Ejemplo n.º 6
0
    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));
    }
Ejemplo n.º 7
0
    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;
    }
Ejemplo n.º 8
0
    /// <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;
      }
    }
Ejemplo n.º 9
0
    /// <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;
    }
Ejemplo n.º 10
0
    /// <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;
      }
      
    }
Ejemplo n.º 11
0
 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);
 }