public static void dlt(Matrix x, Matrix xp)
{
var n = x.Rows;
if (n < 4)
{
throw new Exception();
}
Matrix A = null;
for (int i = 0; i < x.Rows; i++)
{
var xip = xp[i, 0];
var yip = xp[i, 1];
var wip = xp[i, 2];
var xi = x.GetRow(i);
var test = -wip * xi;
double te = (-wip * xi)[0, 0];
var xit = xi.Transpose();
var row1 = new Matrix(new float[, ] {
{ 0, 0, 0 }
});
var row2 = (wip * xit);
row1 = row1.ConcateHorizontal((-wip * xit)).ConcateHorizontal((yip * xit));
row2 = row2.ConcateHorizontal(new Matrix(new float[, ] {
{ 0, 0, 0 }
})).ConcateHorizontal((-xip * xit));
if (A == null)
{
A = row1.ConcateVertical(row2);
}
else
{
A.ConcateVertical(row1);
A.ConcateVertical(row2);
}
}
var svd = new SVD <float>(A);
Matrix H = svd.V.Reshape(3, 3);
H = H / H[2, 2];
}
public static unsafe void ceresSolveAruco()
{
var phc = PinholeCamera.getTestCameraHuawei();
string dir = @"C:\Users\jens\Desktop\calibratie\Huawei p9\aruco\stereo test\";
List <CeresMarker> ceresmarkers = new List <CeresMarker>();
List <CeresCamera> cerescameras = new List <CeresCamera>();
var files = Directory.GetFiles(dir).ToList();
//8 punten nodig
var markerDictionary = Aruco.findArucoMarkers(files, Path.Combine(dir, "aruco_detected\\"), 1);
var pairs = findImagePairsMinMarkers(markerDictionary, 8);
Matrix K = new Matrix(phc.Intrinsics.Mat);
var W = new Matrix(new double[] {
0.0D, -1.0D, 0.0D,
1.0D, 0.0D, 0.0D,
0.0D, 0.0D, 1.0D
});
var Wt = new Matrix(new double[] {
0.0D, 1.0D, 0.0D,
-1.0D, 0.0D, 0.0D,
0.0D, 0.0D, 1.0D
});
var Z = new Matrix(new double[] {
0.0D, 1.0D, 0.0D,
-1.0D, 0.0D, 0.0D,
0.0D, 0.0D, 0D
});
var diag = new Matrix(new double[] {
1.0D, 0.0D, 0.0D,
0.0D, 1.0D, 0.0D,
0.0D, 0.0D, 0.0D
});
foreach (var stereoPair in pairs)
{
var points_count = stereoPair.intersection.Count;
VectorOfPointF punten1px, punten2px;
{
int i = 0;
List <PointF> p1 = new List <PointF>();
List <PointF> p2 = new List <PointF>();
foreach (KeyValuePair <ArucoMarker, ArucoMarker> kvp in stereoPair.intersection)
{
p1.Add(kvp.Key.Corner1);
p2.Add(kvp.Value.Corner1);
i++;
}
punten1px = new VectorOfPointF(p1.ToArray());
punten2px = new VectorOfPointF(p2.ToArray());
}
Matrix F = new Matrix(3, 3);
CVI.FindFundamentalMat(punten1px, punten2px, F);
Matrix essential = K.Transpose() * F * K;
var decomp = new SVD <double>(essential);
var U = decomp.U;
var Vt = decomp.Vt;
var R1 = U * W * Vt;
var R2 = U * W.Transpose() * Vt;
var T1 = U.GetCol(2);
var T2 = -1 * U.GetCol(2);
Matrix[] Ps = new Matrix[4];
for (int i = 0; i < 4; i++)
{
Ps[i] = new Matrix(3, 4);
}
CVI.HConcat(R1, T1, Ps[0]);
CVI.HConcat(R1, T2, Ps[1]);
CVI.HConcat(R2, T1, Ps[2]);
CVI.HConcat(R2, T2, Ps[3]);
var KPs = new Matrix[4];
KPs[0] = K * Ps[0];
KPs[1] = K * Ps[1];
KPs[2] = K * Ps[2];
KPs[3] = K * Ps[3];
var KP0 = K * new Matrix(new double [, ] {
{ 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }
});
for (int i = 0; i < 4; i++)
{
Matrix <float> output_hom = new Matrix <float>(4, punten1px.Size);
VectorOfPoint3D32F output_3d = new VectorOfPoint3D32F();
CVI.TriangulatePoints(KP0, KPs[i], punten1px, punten2px, output_hom);
CVI.ConvertPointsFromHomogeneous(output_hom, output_3d);
}
Matrix S = U * diag * W * U.Transpose();
Matrix R = U * W * decomp.Vt;
}
}
