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