public static Scene bundleAdjustSceneMultiCollection()
{
//scene bevat SObject. dit kunnen eender welke elementen zijn die hier van overerven
var s = new Scene();
//calibratieruimte 8x4x10m elke 1m een marker, returns List<CalibratieForms::Marker>
var ptn3d = createBox(8, 4, 10, 0.2);
s.objects.AddRange(ptn3d); //markers toevoegen aan scene
Random rnd = new Random();
int index = rnd.Next(ptn3d.Count);
var cameras = new List <PinholeCamera>();
for (int i = 0; i < 6; i++) //7 foto's met Huawei camera
{
var c = PinholeCamera.getTestCameraHuawei(); //Huawei bepaald via Zhang
c.Name = "huaweip9";
var pos = new Matrix <double>(new double [] { rnd.Next(2, 6), 2, rnd.Next(3, 7) }); //camera staat op x: 2-6m, y=2m, z=3-7m random gekozen
var target = ptn3d[rnd.Next(ptn3d.Count)]; //camera richt naar een random gekozen marker
var worldtocamera = MatrixExt.LookAt(pos, target.Pos, MatrixExt.UnitVectorY <double>()); //berekend de wereld->cameracoordinaten
c.WorldMat = worldtocamera.Inverted();
//camera->wereldcoordinaten.
//Deze matrix bevat dus op kolom3 de Positie van de camera in wereldcoordinten,
//hiernaast de rotatie in 3x3 ([R t])
cameras.Add(c);
}
CameraCollection coll = new CameraCollection(cameras);
CameraCollection coll2 = new CameraCollection(cameras);
//coll.SetCollectionCenter_MidCameras();
/*for (int i = 0; i < 5; i++) { //5 foto's met Casio camera
* var c = PinholeCamera.getTestCamera();//Casio bepaald via zhang
* c.Name = "casio";
* var Pos = new Vector3d(rnd.Next(2, 6), 2, rnd.Next(3, 7));
* var target = ptn3d[rnd.Next(ptn3d.Count)];
* var worldtocamera = Matrix4d.LookAt(Pos, target.Pos, Vector3d.UnitY);
* c.worldMat = worldtocamera.Inverted();
* cameras.Add(c);
* }*/
//s.objects.AddRange(cameras);
s.objects.Add(coll);
s.objects.Add(coll2);
return(s);
}
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;
}
}
