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