protected CeresCallbackReturnType iterationCallbackHandler(object sender, ceresdotnet.IterationSummary summary)
{
//return CeresCallbackReturnType.SOLVER_CONTINUE;
int iterationNr = summary.iteration;
CeresCameraMultiCollectionBundler b = sender as CeresCameraMultiCollectionBundler;
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}", iterationNr, AllReprojections) + Environment.NewLine;
meancamereapos += String.Format("({0}) pos: {1} {2} {3}", iterationNr, AllReprojections, meanPosX, meanPosY, meanPosZ) + Environment.NewLine;
//Console.WriteLine("({0}) reprojerror: {1} mean cam pos: x({2}) y({3}) z({4})", iterationNr, AllReprojections, meanPosX, meanPosY, meanPosZ);
lastReproj = AllReprojections;
endCost = summary.cost;
var r = CeresCallbackReturnType.SOLVER_CONTINUE;
return(r);
}
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();
}
}
}
