public void SolveMultiCollection()
{
SetUniqueValues();
var collections = scene.get <CameraCollection>();
var markers3d = scene.get <Marker>();
var cameras = scene.get <PinholeCamera>();
var collec = new CameraCollection(cameras);
collections = new[] { collec };
var ccoll = new CeresCameraCollection();
ccoll.Cameras = new List <CeresCamera>();
ccoll.Cameras.AddRange(cameras.Select(x => {
var cc = x.toCeresCamera();
return(cc);
}));
// ccoll.CreateSecondPositionCopy();
var bundler = new ceresdotnet.CeresCameraMultiCollectionBundler();
Dictionary <CeresCameraCollection, Dictionary <CeresCamera, List <CeresMarker> > > observations =
new Dictionary <CeresCameraCollection, Dictionary <CeresCamera, List <CeresMarker> > >();
List <CeresCamera> cerescameras = new List <CeresCamera>();
List <CeresCameraCollection> cerescameracollections = new List <CeresCameraCollection>();
int cameraID = -1;
var collectionobservations2 = new Dictionary <CeresCamera, List <CeresMarker> >();
foreach (var camera in UniqueCameras)
{
//voor ruis kopie maken
var cameracopy = new PinholeCamera();
var cc = camera.toCeresCamera();
cameracopy.toCeresCamera();
cameracopy.updateFromCeres(cc.Internal);
cameracopy.updateFromCeres(cc.External);
this.OriginalValues.Add(camera, cameracopy);
}
int totaalfotos = 0;
foreach (var collection in collections)
{
var cerescollection = new CeresCameraCollection();
var collectionobservations = new Dictionary <CeresCamera, List <CeresMarker> >();
foreach (var camera in collection)
{
totaalfotos++;
List <CeresMarker> ceresmarkers = new List <CeresMarker>();
cameraID++;
var puntenCv =
markers3d.ToDictionary(m => new MCvPoint3D32f((float)m.X, (float)m.Y, (float)m.Z));
var cc = camera.toCeresCamera();
ceresdotnet.CeresTestFunctions.ProjectPoint(cc.Internal, cc.External, markers3d[0].Pos);
var visible_proj = camera.ProjectPointd2D_Manually(markers3d, out Marker[] visible3d);
GemFeaturesPerFoto += visible_proj.Length;
//Pixel ruis
for (int i = 0; i < visible_proj.Length; i++)
{
PixelRuisProvider?.Apply(ref visible_proj[i]);
}
//in een cerescamera worden interne parameters opgeslaan volgens array v doubles
//Per interne parameters kan men bepalen wat dient gebundeld te worden
//ook combinatie zijn mogelijk
//3D ruis
foreach (var marker in markers3d)
{
//WorldRuisProvider?.Apply(marker);
}
for (int i = 0; i < visible3d.Length; i++)
{
var proj = visible_proj[i];
ceresmarkers.Add(new CeresMarker()
{
id = visible3d[i].ID,
Location = visible3d[i].toCeresParameter(),
x = proj.X,
y = proj.Y
});
}
cerescameras.Add(cc);
collectionobservations.Add(cc, ceresmarkers);
//gesimuleerde foto weergeven
/*var window2 = new CameraSimulationFrm(string.Format("Camera {0}: {1}", cameraID, camera.Name)) {
* Camera = camera
* };
* window2.Show();
* window2.drawChessboard(visible_proj);*/
}
observations.Add(cerescollection, collectionobservations);
collectionobservations2 = collectionobservations;
}
GemFeaturesPerFoto /= totaalfotos;
foreach (var intr in UniqueIntr)
{
//Camera ruis/modifier
CameraModifier?.Apply(intr);
intr.toCeresParameter();
}
foreach (var marker in markers3d)
{
WorldRuisProvider?.Apply(marker);
marker.toCeresParameter();
}
CeresCameraMultiCollectionBundler.MarkersFromCameraDelegate findObservationsFunc = (camera, coll) => {
var r = collectionobservations2[camera];
double Allreproj = 0;
double totalmrkrcout = 0;
//foreach (var ceresCamera in bundler.StandaloneCameraList) {
double reprojections = 0;
foreach (var ceresMarker in r)
{
var reproj = CeresTestFunctions.ReprojectPoint(camera.Internal, camera.External,
ceresMarker.toPointF(), ceresMarker.Location.toMatrix());
reprojections += Math.Sqrt(reproj.X * reproj.X + reproj.Y * reproj.Y);
}
Allreproj += reprojections;
totalmrkrcout += r.Count;
reprojections /= r.Count;
//}
return(r);
};
bundler.MarkersFromCamera = findObservationsFunc;
bundler.CollectionList = cerescameracollections;
bundler.StandaloneCameraList = cerescameras;
bundler.bundleCollections(iterationCallbackHandler);
CeresCameraMultiCollectionBundler b = bundler;
double AllReprojections = 0;
double totalmarkercount = 0;
double meanPosX = 0;
double meanPosY = 0;
double meanPosZ = 0;
foreach (var ceresCamera in b.StandaloneCameraList)
{
double reprojections = 0;
var markerlist = b.MarkersFromCamera(ceresCamera, null);
foreach (var ceresMarker in markerlist)
{
var reproj = CeresTestFunctions.ReprojectPoint(ceresCamera.Internal, ceresCamera.External, ceresMarker.toPointF(), ceresMarker.Location.toMatrix());
reprojections += Math.Sqrt(reproj.X * reproj.X + reproj.Y * reproj.Y);
}
AllReprojections += reprojections;
totalmarkercount += markerlist.Count;
reprojections /= markerlist.Count;
//mean cam pos;
var pos = ceresCamera.External.t;
meanPosX += pos[0];
meanPosY += pos[1];
meanPosZ += pos[2];
}
meanPosX /= b.StandaloneCameraList.Count;
meanPosY /= b.StandaloneCameraList.Count;
meanPosZ /= b.StandaloneCameraList.Count;
AllReprojections /= totalmarkercount;
reporjectionsstring += String.Format("({0}) Error: {1}", "final", AllReprojections) + Environment.NewLine;
meancamereapos += String.Format("({0}) pos: {1} {2} {3}", "final", AllReprojections, meanPosX, meanPosY, meanPosZ) + Environment.NewLine;
Console.WriteLine("({0}) reprojerror: {1} mean cam pos: x({2}) y({3}) z({4})", "final", AllReprojections, meanPosX, meanPosY, meanPosZ);
lastReproj = AllReprojections;
foreach (var collection in collections)
{
foreach (var camera in collection)
{
camera.updateFromCeres();
}
}
}
public static PointF[] ProjectPointd2D_Manually <T>(this PinholeCamera cam, T[] points3d, out T[] visible) where T : SPoint
{
var r = new List <PointF>();
var vis = new List <T>();
//var tt = cam.worldMat.Inverted();
//var transf = tt.tocv();
var transf = cam.WorldMat.Inverted();
var worldmatinv = cam.worldMat.Inverted();
var mcvpoints3d = points3d.Select(x => x.toMCvPoint3D32f());
VectorOfPoint3D32F points3dvec = new VectorOfPoint3D32F(mcvpoints3d.ToArray());
//VectorOfPoint3D32F points3dvectransf = new VectorOfPoint3D32F(points3dvec.Size);
var camcoords = new Matrix <double>(3, points3d.Length);
var phm_mat = new Mat();
var camcoords_mat = new Mat();
var camcoords_mat2 = new VectorOfPoint3D32F();
CvInvoke.ConvertPointsToHomogeneous(points3dvec, phm_mat);
//var phm = new Matrix<float>(4, points3d.Length, phm_mat.DataPointer);
//var camcoord = cam.WorldMat.Inverted() * phm;
CvInvoke.Transform(phm_mat, camcoords_mat, transf);
CvInvoke.ConvertPointsFromHomogeneous(camcoords_mat, camcoords_mat2);
var cc = cam.toCeresCamera();
for (int i = 0; i < camcoords_mat2.Size; i++)
{
var phmm = new Matrix <double>(3, 1);
var camcoord = Vector3d.TransformPerspective(new Vector3d(points3d[i].X, points3d[i].Y, points3d[i].Z), worldmatinv);
var x = camcoord.X / camcoord.Z;
var y = camcoord.Y / camcoord.Z;
var r2 = x * x + y * y;
var r4 = r2 * r2;
var r6 = r4 * r2;
var r_coeff = ((1) + cam.DistortionR1 * r2 + cam.DistortionR2 * r4 + cam.DistortionR3 * r6);
var tdistx = 2 * cam.DistortionT1 * x * y + cam.DistortionT2 * (r2 + 2 * x * x);
var tdisty = 2 * cam.DistortionT2 * x * y + cam.DistortionT1 * (r2 + 2 * y * y);
var xd = x * r_coeff + tdistx;
var yd = y * r_coeff + tdisty;
var im_x2 = cam.Intrinsics.fx * xd + cam.Intrinsics.cx;
var im_y2 = cam.Intrinsics.fy * yd + cam.Intrinsics.cy;
if (camcoord.Z < 0) //camcoords_mat2[i].Z < 0) {
{
continue;
}
var pointf = ceresdotnet.CeresTestFunctions.ProjectPoint(cc.Internal, cc.External, points3d[i].Pos);
var im_x = pointf.X;
var im_y = pointf.Y;
if (im_x >= 0 && im_x <= cam.PictureSize.Width && im_y >= 0 && im_y <= cam.PictureSize.Height)
{
vis.Add(points3d[i]);
r.Add(pointf);
}
if (im_x2 >= 0 && im_x2 <= cam.PictureSize.Width && im_y2 >= 0 && im_y2 <= cam.PictureSize.Height)
{
//vis.Add(points3d[i]);
//pointf.X = (float)im_x2;
//pointf.Y = (float)im_y2;
//r.Add(pointf);
}
}
visible = vis.ToArray();
return(r.ToArray());
}
