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