static TandTi GetNormalizedMatrix(PointFloat[] pts)
{
var xmean = getMean(pts, p => p.X);
var ymean = getMean(pts, p => p.Y);
var xvar = getVariance(pts, p => p.X);
var yvar = getVariance(pts, p => p.Y);
var xs = Math.Sqrt(2 / xvar);
var ys = Math.Sqrt(2 / yvar);
var m = new GMatrix(new double[, ]
{
{ xs, 0, -xs * xmean },
{ 0, ys, -ys * ymean },
{ 0, 0, 1 }
});
var mi = new GMatrix(new double[, ] {
{ 1 / xs, 0, xmean },
{ 0, 1 / ys, ymean },
{ 0, 0, 1 }
});
return(new TandTi
{
t = m,
ti = mi,
});
}
public Ert GetRT(GMatrix m)
{
var svd = JacobSvd.JacobiSVD(m);
var W = new GMatrix(new double[, ]
{
{ 0, -1, 0 },
{ 1, 0, 0 },
{ 0, 0, 1 }
});
var Wt = new GMatrix(new double[, ]
{
{ 0, 1, 0 },
{ -1, 0, 0 },
{ 0, 0, 1 }
});
svd.W[2] = 0;
var t = svd.U.dot(W).dot(svd.getWMat()).dot(svd.U.tranpose());
var r = svd.U.dot(Wt).dot(svd.Vt);
return(new Ert
{
R = r,
T = t,
});
}
public static GMatrix CalcFundm(PointFloat[] points1, PointFloat[] points2)
{
var m1 = new GMatrix(points1.Length, 9);
for (var i = 0; i < points1.Length; i++)
{
var pp1 = points1[i];
var pp2 = points2[i];
var y1 = pp1.X;
var y2 = pp1.Y;
var p1 = pp2.X;
var p2 = pp2.Y;
var cur = m1.storage[i];
cur[0] = y1 * p1;
cur[1] = p1 * y2;
cur[2] = p1;
cur[3] = p2 * y1;
cur[4] = p2 * y2;
cur[5] = p2;
cur[6] = y1;
cur[7] = y2;
cur[8] = 1;
}
var Fhat = SolveSvd3x3(m1);
var FhatSvd = JacobSvd.JacobiSVD(Fhat);
var d = FhatSvd.getWMat();
d.storage[2][2] = 0;
var res = FhatSvd.U.dot(d).dot(FhatSvd.Vt);
return(res);
}
public GMatrix dot(GMatrix m)
{
var r = rows;
var mc = m.cols;
//if (r != mc) throw new InvalidOperationException($"Cross: row {r} and col {mc} must equal");
if (cols != m.rows)
{
throw new InvalidOperationException($"Cross: col {cols} and row {m.rows} must equal");
}
var newStorage = new double[r, mc];
var c = cols;
for (var i = 0; i < r; i++)
{
for (var j = 0; j < mc; j++)
{
double total = 0;
for (var k = 0; k < c; k++)
{
total += storage[i][k] * m.storage[k][j];
}
newStorage[i, j] = total;
}
}
return(new GMatrix(newStorage));
}
private static JacobSvd.SvdRes SolveSvd(GMatrix m1)
{
//double max = 1;
//double min = -1;
//for (var i = 0; i < m1.rows; i++)
//{
// for (var j = 0; j < m1.cols; j++)
// {
// var v = m1.storage[i][j];
// if (v > max) max = v;
// if (v < min) min = v;
// }
//}
//var scal = Math.Max(Math.Abs(min), max);
//for (var i = 0; i < m1.rows; i++)
//{
// for (var j = 0; j < m1.cols; j++)
// {
// var v = m1.storage[i][j];
// m1.storage[i][j] = v / scal;
// }
//}
var svdA = JacobSvd.JacobiSVD(m1);
return(svdA);
}
public static GMatrix GetH2(PointFloat e, PointFloat imgSize)
{
var w2 = -imgSize.X / 2;
var h2 = -imgSize.Y / 2;
GMatrix T = new GMatrix(new double[, ] {
{ 1, 0, w2 },
{ 0, 1, h2 },
{ 0, 0, 1 }
});
var e1 = e.X + w2;
var e2 = e.Y + h2;
var l = Math.Sqrt((e1 * e1) + (e2 * e2));
var alph = e1 >= 0 ? 1 : -1;
GMatrix R = new GMatrix(new double[, ] {
{ alph *e1 / l, alph *e2 / l, 0 },
{ -1 * alph * e2 / l, alph * e1 / l, 0 },
{ 0, 0, 1 }
});
GMatrix G = new GMatrix(new double[, ]
{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ -1 / e1, 0, 1 }
});
return(GMatrix.Inverse3x3(T).dot(G.dot(R.dot(T))));
}
public static PointFloat FindEpipole(PointFloat[] pts, PointSide side, GMatrix f)
{
if (side == PointSide.Right)
{
f = f.tranpose();
}
GMatrix lines = new GMatrix(pts.Length, 3);
var storage = lines.storage;
for (int i = 0; i < pts.Length; i++)
{
var cur = storage[i];
var pt = pts[i];
var gm = f.dot(new GMatrix(new double[3, 1] {
{ pt.X }, { pt.Y }, { 1 }
}));
var ms = gm.storage;
for (int j = 0; j < 3; j++)
{
cur[j] = ms[j][0];
}
}
var svdA = JacobSvd.JacobiSVD(lines);
var res = svdA.Vt.storage[2];
//Console.WriteLine($"{res[0].ToString("0.00")},{res[1].ToString("0.00")},{res[2].ToString("0.0000000")}");
//Console.WriteLine($"{(res[0]/res[2]).ToString("0.00")},{(res[1]/res[2]).ToString("0.00")}");
return(new PointFloat((float)(res[0] / res[2]), (float)(res[1] / res[2])));
}
static void MaxApply(GMatrix m, int pos)
{
double max = 0;
for (var i = 0; i < m.rows; i++)
{
var v = Math.Abs(m.storage[i][pos]);
if (v > max)
{
max = v;
}
}
for (var i = 0; i < m.rows; i++)
{
var v = m.storage[i][pos];
if (v == 0)
{
continue;
}
var scal = max / v;
for (var j = pos; j < m.cols; j++)
{
m.storage[i][j] *= scal;
}
}
}
public static double[] SVBkSb(GMatrix u, double[] w, GMatrix v, double[] b)
{
int m = u.rows, n = u.cols;
double[] x = new double[n];
double[] temp = new double[n];
for (int j = 0; j < n; j++)
{
double s = 0;
if (w[j] != 0)
{
for (var i = 0; i < m; i++)
{
s += u.storage[i][j] * b[i];
}
s /= w[j];
}
temp[j] = s;
}
for (var j = 0; j < n; j++)
{
double s = 0.0;
for (var jj = 0; jj < n; jj++)
{
s += v.storage[j][jj] * temp[jj];
}
x[j] = s;
}
return(x);
}
public GMatrix getWMat()
{
var mat = new GMatrix(U.rows, Vt.cols);
int at = 0;
for (var i = 0; i < Vt.cols; i++)
{
mat.storage[i][i] = W[at++];
}
return(mat);
}
static PointFloat[] perspectiveTransform(PointFloat[] pts, GMatrix mat)
{
List <PointFloat> res = new List <PointFloat>();
foreach (var pt in pts)
{
var npt = mat.dot(pt.ToVect());
var w = npt.storage[2][0];
res.Add(new PointFloat(npt.storage[0][0] / w, npt.storage[1][0] / w));
}
return(res.ToArray());
}
public static GMatrix GetH1(PointFloat e, PointFloat imgSize, GMatrix F, GMatrix H2,
PointFloat[] leftPts, PointFloat[] rightPts)
{
var ex = new GMatrix(new double[, ]
{
{ 0, -1, e.Y },
{ 1, 0, -e.X },
{ -e.Y, e.X, 0 }
});
var e_111 = new GMatrix(new double[, ]
{
{ e.X, e.X, e.X },
{ e.Y, e.Y, e.Y },
{ 1, 1, 1 },
});
var m = ex.dot(F).add(e_111);
var h0h = H2.dot(m);
var m1 = perspectiveTransform(leftPts, h0h);
var m2 = perspectiveTransform(rightPts, H2);
var abuf = new double[leftPts.Length, 3];
for (var i = 0; i < leftPts.Length; i++)
{
var pt = leftPts[i];
abuf[i, 0] = pt.X;
abuf[i, 1] = pt.Y;
abuf[i, 2] = 1;
}
var A = new GMatrix(abuf);
var svcr = JacobSvd.JacobiSVD(A);
double[] B = new double[rightPts.Length];
for (var i = 0; i < rightPts.Length; i++)
{
B[i] = m2[i].X;
}
var x = Util.SVBkSb(svcr.U, svcr.W, svcr.Vt, B);
var Ha = new GMatrix(new double[, ]
{
{ x[0], x[1], x[2] },
{ 0, 1, 0 },
{ 0, 0, 1 }
});
var res = Ha.dot(h0h);
return(res);
}
public GMatrix add(GMatrix m)
{
var r = rows;
var c = cols;
var newStorage = new GMatrix(r, c);
for (var i = 0; i < r; i++)
{
for (var j = 0; j < c; j++)
{
newStorage.storage[i][j] = storage[i][j] + m.storage[i][j];
}
}
return(newStorage);
}
public GMatrix div(double v)
{
var r = rows;
var c = cols;
var newStorage = new GMatrix(r, c);
for (var i = 0; i < r; i++)
{
for (var j = 0; j < c; j++)
{
newStorage.storage[i][j] = storage[i][j] / v;
}
}
return(newStorage);
}
public GMatrix tranpose()
{
var r = rows;
var c = cols;
var newStorage = new GMatrix(c, r);
for (var i = 0; i < r; i++)
{
for (var j = 0; j < c; j++)
{
newStorage.storage[j][i] = storage[i][j];
}
}
return(newStorage);
}
public static GMatrix SolveSvd3x3(GMatrix m1)
{
JacobSvd.SvdRes svdA = SolveSvd(m1);
var Fhat = new GMatrix(3, 3);
int at = 0;
var ms = svdA.Vt.storage[8];
for (var i = 0; i < 3; i++)
{
for (var j = 0; j < 3; j++)
{
Fhat.storage[i][j] = ms[at++];
}
}
return(Fhat);
}
public static GMatrix EstimateHomography(PointFloat[] points, PointFloat[] checkBoardLoc)
{
var nu = GetNormalizedMatrix(points);
var nx = GetNormalizedMatrix(checkBoardLoc);
var m1 = new GMatrix(points.Length * 2, 9);
for (var i = 0; i < points.Length; i++)
{
var pts = nu.t.dot(points[i].ToVect()).noramByLast();
var obj = nx.t.dot(checkBoardLoc[i].ToVect()).noramByLast();
var ii = i * 2;
//var xy = points[i];
var x = pts.storage[0][0];
var y = pts.storage[1][0];
var X = obj.storage[0][0];
var Y = obj.storage[1][0];
var cur = m1.storage[ii];
cur[0] = -X;
cur[1] = -Y;
cur[2] = -1;
cur[3] = 0;
cur[4] = 0;
cur[5] = 0;
cur[6] = x * X;
cur[7] = x * Y;
cur[8] = x;
cur = m1.storage[ii + 1];
cur[0] = 0;
cur[1] = 0;
cur[2] = 0;
cur[3] = -X;
cur[4] = -Y;
cur[5] = -1;
cur[6] = y * X;
cur[7] = y * Y;
cur[8] = y;
}
var res = SolveSvd3x3(m1);
var denormed = nu.ti.dot(res).dot(nx.t).noramByLast();
return(denormed);
}
protected static GMatrix Cholesky3x3(GMatrix m)
{
var l11 = Math.Sqrt(m.storage[0][0]);
var l21 = m.storage[0][1] / l11;
var l31 = m.storage[0][2] / l11;
var l22 = Math.Sqrt(m.storage[1][1] - (l21 * l21));
var l32 = (m.storage[1][2] - (l31 * l21)) / l22;
var l33 = Math.Sqrt(m.storage[2][2] - ((l31 * l31) + (l32 * l32)));
GMatrix rm = new GMatrix(3, 3);
rm.storage[0][0] = l11;
rm.storage[0][1] = l21;
rm.storage[0][2] = l31;
rm.storage[1][1] = l22;
rm.storage[1][2] = l32;
rm.storage[2][2] = l33;
return(rm);
}
public static SvdRes JacobiSVD(GMatrix mat)
{
//m rows, n cols
//orig mxn, U mxm, W mxn V nxn
//W is array of size n, Vt size nxn
var m = mat.rows;
var n = mat.cols;
var A = mat.tranpose().ToArray();
var res = SVD(A, m, n);
return(new SvdRes
{
U = new GMatrix(A, n, m).tranpose(), //or nxm or mxn?
W = res.W,
Vt = new GMatrix(res.Vt, n, n),
});
}
public static GMatrix Inverse3x3(GMatrix m)
{
double det = 0;
var mat = m.storage;
for (var i = 0; i < 3; i++)
{
det = det + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] - mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
}
var res = new GMatrix(3, 3);
for (var i = 0; i < 3; ++i)
{
for (var j = 0; j < 3; ++j)
{
res.storage[i][j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) - (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) / det;
}
}
return(res);
}
public static RectifyResult Rectify(List <StereoPoints> allPts, PointFloat imgSize, int CalibGridRow = 6, int CalibGridCol = 3)
{
var leftPts = allPts.SelectMany(x => x.Left).ToArray();
var rightPts = allPts.SelectMany(x => x.Right).ToArray();
var F = Calib.CalcFundm(leftPts, rightPts);
var epol = CalibRect.FindEpipole(leftPts, PointSide.Left, F);
var h2 = CalibRect.GetH2(epol, new PointFloat(imgSize.X, imgSize.Y));
var h1 = CalibRect.GetH1(epol, new PointFloat(imgSize.X, imgSize.Y), F, h2, allPts[0].Left, allPts[0].Right);
Func <Func <StereoPoints, PointFloat[]>, PointFloat[][]> fetch = f =>
{
PointFloat[][] res = new PointFloat[allPts.Count][];
for (var i = 0; i < allPts.Count; i++)
{
res[i] = f(allPts[i]);
}
return(res);
};
var li = Calib.EstimateIntranics(fetch(x => x.Left), CalibGridRow, CalibGridCol);
var ri = Calib.EstimateIntranics(fetch(x => x.Right), CalibGridRow, CalibGridCol);
var E = GMatrix.Inverse3x3(li).tranpose().dot(F).dot(GMatrix.Inverse3x3(ri));
return(new RectifyResult
{
H1 = h1,
H2 = h2,
F = F,
E = E,
el = epol,
LeftIntrinics = li,
RightIntrinics = ri,
});
}
public void Solve(GMatrix m)
{
MaxApply(m, 0);
}
public static GMatrix EstimateIntranics(PointFloat[][] allPoints, int w = 6, int h = 3)
{
/*
* hij
* h12 h22 h32
*
* h11 b11 b12 b13
* h21 b12 b22 b23
* h31 b13 b23 b33
*
*
* h11 h12 t1
* h21 h22 t2
* h31 h32 t3
*
*/
var allHomos = allPoints.GetLength(0);
var homopts = new List <HomoPts>();
for (var c = 0; c < allHomos; c++)
{
var p = allPoints[c];
var hm = EstimateHomography(p, w, h);
Action <int, int, double[], GMatrix> fillV = (i, j, cur, h**o) =>
{
Func <int, int, double> hval = (hrow, hcol) =>
{
return(h**o.storage[hrow][hcol]);
};
cur[0] = hval(0, i) * hval(0, j);
cur[1] = (hval(0, i) * hval(1, j)) + (hval(1, i) * hval(0, j));
cur[2] = hval(1, i) * hval(1, j);
cur[3] = (hval(2, i) * hval(0, j)) + (hval(0, i) * hval(2, j));
cur[4] = (hval(2, i) * hval(1, j)) + (hval(1, i) * hval(2, j));
cur[5] = hval(2, i) * hval(2, j);
};
homopts.Add(new HomoPts {
h**o = hm, points = p, FillV = fillV
});
}
var pointsLength = w * h;
GMatrix m = new GMatrix(allHomos * 2, 6);
int at = 0;
for (var c = 0; c < homopts.Count; c++)
{
var hp = homopts[c];
var FillV = hp.FillV;
FillV(0, 1, m.storage[at], hp.h**o);
var v00 = new double[6];
FillV(0, 0, v00, hp.h**o);
var v11 = new double[6];
FillV(1, 1, v11, hp.h**o);
var r2 = m.storage[at + 1];
for (var j = 0; j < 6; j++)
{
r2[j] = v00[j] - v11[j];
}
at += 2;
}
var svdr = SolveSvd(m);
var a = new GMatrix(3, 3);
var vtl = svdr.Vt.storage[5];
Func <int, double> gb = i => vtl[i];
var gb0_4 = gb(0) * gb(4);
var gb0_2 = gb(0) * gb(2);
var gb1x1 = gb(1) * gb(1);
var vc = ((gb(1) * gb(3)) - gb0_4) / (gb0_2 - gb1x1);
var l = gb(5) - ((gb(3) * gb(3) + vc * (gb(1) * gb(2) - gb0_4)) / gb(0));
var alpha = Math.Sqrt(l / gb(0));
var beta = Math.Sqrt((l * gb(0)) / (gb0_2 - gb1x1));
var gamma = -1 * (gb(1) * alpha * alpha) * beta / l;
var uc = gamma * vc / beta - (gb(3) * alpha * alpha / l);
a.storage[0][0] = alpha;
a.storage[1][1] = beta;
a.storage[0][1] = gamma;
a.storage[0][2] = uc;
a.storage[1][2] = vc;
a.storage[2][2] = 1;
//Console.WriteLine(a);
return(a);
// var b = new GMatrix(3, 3);
//b.storage[0][0] = svdr.Vt.storage[0][5];
//b.storage[0][1] = b.storage[1][0] = svdr.Vt.storage[1][5]; //b12
//b.storage[0][2] = b.storage[2][0] = svdr.Vt.storage[2][5]; //b13
//b.storage[1][1] = svdr.Vt.storage[3][5]; //b22
//b.storage[1][2] = b.storage[2][1] = svdr.Vt.storage[4][5]; //b23
//b.storage[2][2] = svdr.Vt.storage[5][5]; //b33
////b= K-t*K-1
//Console.WriteLine(b);
//var A1 = Cholesky3x3(b);
//Console.WriteLine(Inverse3x3(A1));
}
