public void DoBundleAdjust() { // N = cameras // M = point count //public static void BundleAdjust(MCvPoint3D64f[M] points, // Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment // MCvPoint2D64f[M][N] imagePoints, // Projections of 3d points for every camera // int[M][N] visibility, // Visibility of 3d points for every camera // Matrix<double>[N] cameraMatrix, // Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment // Matrix<double>[N] R, // rotation matrices of all cameras (input and output), values will be modified by bundle adjustment // Matrix<double>[N] T, // translation vector of all cameras (input and output), values will be modified by bundle adjustment // Matrix<double>[N] distCoefficients, // distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment // MCvTermCriteria termCrit) // Termination criteria, a reasonable value will be (30, 1.0e-12) _stopwatchGet.Restart(); if (_cameras.Cameras.Count == 0) { return; } IEnumerable <CameraModel> orderedCameras = _cameras.Cameras.OrderBy(camera => camera.Calibration.Index); ObservableCollection <MotionControllerModel> controllers = _cameras.Cameras[0].Controllers; if (controllers.Count == 0) { return; } float radius = CameraCalibrationModel.SPHERE_RADIUS_CM; int cameraCount = _cameras.Cameras.Count; int pointCount = 8; MCvPoint3D64f[] objectPoints = new MCvPoint3D64f[controllers.Count * pointCount]; MCvPoint2D64f[][] imagePoints = new MCvPoint2D64f[cameraCount][]; int[][] visibility = new int[cameraCount][]; Matrix <double>[] cameraMatrix = new Matrix <double> [cameraCount]; Matrix <double>[] R = new Matrix <double> [cameraCount]; Matrix <double>[] T = new Matrix <double> [cameraCount]; Matrix <double>[] distCoefficients = new Matrix <double> [cameraCount]; MCvTermCriteria termCrit = new MCvTermCriteria(30, 1.0e-12); int visible = 0; foreach (CameraModel camera in orderedCameras) { visibility[camera.Calibration.Index] = new int[controllers.Count * pointCount]; cameraMatrix[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.IntrinsicMatrix.Clone(); distCoefficients[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.DistortionCoeffs.Clone(); imagePoints[camera.Calibration.Index] = new MCvPoint2D64f[controllers.Count * pointCount]; R[camera.Calibration.Index] = camera.Calibration.RotationToWorld.Clone(); T[camera.Calibration.Index] = camera.Calibration.TranslationToWorld.Clone(); foreach (MotionControllerModel controller in controllers) { float x = controller.RawPosition[camera].x; float y = controller.RawPosition[camera].y; //if (x == 0 && y == 0) return; // controller is not visible if (controller.TrackerStatus[camera] != PSMoveTrackerStatus.Tracking) { for (int i = 0; i < pointCount; i++) { visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0; } } // controller is visible else { Vector3[] history = controller.PositionHistory[camera]; float avgMagnitude = 0f; for (int i = 1; i < history.Length; i++) { avgMagnitude += history[i].magnitude / (history.Length - 1); } // check deviation of newest position if ((Math.Abs(((history[0].magnitude * 100) / avgMagnitude)) - 100) > 5) { for (int i = 0; i < pointCount; i++) { visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0; } continue; } visible++; //double distance = 0.0; int startIndex = controller.Id * pointCount; //MCvPoint3D64f cameraPositionInWorld = new MCvPoint3D64f //{ // x = camera.Calibration.TranslationToWorld[0, 0], // y = camera.Calibration.TranslationToWorld[1, 0], // z = camera.Calibration.TranslationToWorld[2, 0] //}; // set visibility and calculate distance of the controller relative to the camera for (int i = startIndex; i < pointCount * controllers.Count; i++) { visibility[camera.Calibration.Index][i] = 1; //double d = CvHelper.GetDistanceToPoint(cameraPositionInWorld,objectPoints[i]); //distance += d / pointCount; } // initialize object's world coordinates // calculate as the average of each camera's transformed world coordinate float wx = controller.WorldPosition[camera].x; float wy = controller.WorldPosition[camera].y; float wz = controller.WorldPosition[camera].z; objectPoints[startIndex] += new MCvPoint3D32f(wx - radius, wy - radius, wz - radius); objectPoints[startIndex + 1] += new MCvPoint3D32f(wx + radius, wy - radius, wz - radius); objectPoints[startIndex + 2] += new MCvPoint3D32f(wx + radius, wy + radius, wz - radius); objectPoints[startIndex + 3] += new MCvPoint3D32f(wx - radius, wy + radius, wz - radius); objectPoints[startIndex + 4] += new MCvPoint3D32f(wx - radius, wy + radius, wz + radius); objectPoints[startIndex + 5] += new MCvPoint3D32f(wx + radius, wy + radius, wz + radius); objectPoints[startIndex + 6] += new MCvPoint3D32f(wx + radius, wy - radius, wz + radius); objectPoints[startIndex + 7] += new MCvPoint3D32f(wx - radius, wy - radius, wz + radius); //imagePoints[scvm.Camera.Calibration.Index] = Utils.GetImagePoints(mcvm.MotionController.RawPosition[scvm.Camera]); imagePoints[camera.Calibration.Index] = Array.ConvertAll(camera.Calibration.ObjectPointsProjected, CvHelper.PointFtoPoint2D); } } // foreach controller } // foreach camera if (visible == 0) { return; } // average object points for (int i = 0; i < objectPoints.Length; i++) { objectPoints[i].x /= visible; objectPoints[i].y /= visible; objectPoints[i].z /= visible; } // calculate object's middle float prex = 0, prey = 0, prez = 0; for (int i = 0; i < objectPoints.Length; i++) { prex += (float)objectPoints[i].x / objectPoints.Length; prey += (float)objectPoints[i].y / objectPoints.Length; prez += (float)objectPoints[i].z / objectPoints.Length; } _stopwatchBA.Restart(); //LevMarqSparse.BundleAdjust(objectPoints, imagePoints, visibility, cameraMatrix, R, T, distCoefficients, termCrit); _stopwatchBA.Stop(); _stopwatchSet.Restart(); // check for calucation error for (int i = 0; i < objectPoints.Length; i++) { if (objectPoints[i].x.ToString().Equals("NaN")) { return; } if (objectPoints[i].y.ToString().Equals("NaN")) { return; } if (objectPoints[i].z.ToString().Equals("NaN")) { return; } } // save changed matrices foreach (CameraModel camera in orderedCameras) { if (visibility[camera.Calibration.Index][0] == 1) { //RotationVector3D rot1 = new RotationVector3D(); //rot1.RotationMatrix = camera.Calibration.RotationToWorld; //RotationVector3D rot2 = new RotationVector3D(); //rot2.RotationMatrix = R[camera.Calibration.Index]; //Console.WriteLine((int)(rot1[0, 0] * (180 / Math.PI)) + " " + (int)(rot2[0, 0] * (180 / Math.PI))); //Console.WriteLine((int)(rot1[1, 0] * (180 / Math.PI)) + " " + (int)(rot2[1, 0] * (180 / Math.PI))); //Console.WriteLine((int)(rot1[2, 0] * (180 / Math.PI)) + " " + (int)(rot2[2, 0] * (180 / Math.PI)) + Environment.NewLine); //camera.Calibration.IntrinsicParameters.IntrinsicMatrix = cameraMatrix[camera.Calibration.Index]; //camera.Calibration.RotationToWorld = R[camera.Calibration.Index]; //camera.Calibration.TranslationToWorld = T[camera.Calibration.Index]; //camera.Calibration.IntrinsicParameters.DistortionCoeffs = distCoefficients[camera.Calibration.Index]; //camera.Calibration.XAngle = (int)(rot2[0, 0] * (180 / Math.PI)); //camera.Calibration.YAngle = (int)(rot2[1, 0] * (180 / Math.PI)); //camera.Calibration.ZAngle = (int)(rot2[2, 0] * (180 / Math.PI)); } } // calculate object's middle float preCenterX = 0, preCenterY = 0, preCenterZ = 0; for (int i = 0; i < objectPoints.Length; i++) { preCenterX += (float)objectPoints[i].x / objectPoints.Length; preCenterY += (float)objectPoints[i].y / objectPoints.Length; preCenterZ += (float)objectPoints[i].z / objectPoints.Length; } Vector3 prePosition = new Vector3(preCenterX, -preCenterY, preCenterZ); if (prePosition != _positionHistory[0]) { for (int i = _positionHistory.Length - 1; i > 0; --i) { _positionHistory[i] = _positionHistory[i - 1]; } _positionHistory[0] = prePosition; } //Vector3 avgPosition = Vector3.zero; //for (int i = 0; i < _positionHistory.Length; i++) //{ // avgPosition += _positionHistory[i] / _positionHistory.Length; //} // 0 predition, 1 correction / estimated Matrix <float> kalmanResults = FilterPoints(_kalmanXYZ, prePosition.x, prePosition.y, prePosition.z); Vector3 kalmanPosition = new Vector3(kalmanResults[1, 0], kalmanResults[1, 1], kalmanResults[1, 2]); _cameras.Position = kalmanPosition; _stopwatchGet.Stop(); _stopwatchSet.Stop(); for (int i = 0; i < 4; i++) { CameraModel camera = _cameras.Cameras[i]; float xr = controllers[0].RawPosition[camera].x; float yr = controllers[0].RawPosition[camera].y; float zr = controllers[0].RawPosition[camera].z; float xc = controllers[0].CameraPosition[camera].x; float yc = controllers[0].CameraPosition[camera].y; float zc = controllers[0].CameraPosition[camera].z; string str = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}", iteration, xr, yr, zr, PsMoveApi.psmove_tracker_distance_from_radius(camera.Handle, controllers[0].RawPosition[camera].z), xc, yc, zc, Math.Sqrt(xc * xc + yc * yc + zc * zc) ); if (camera.Calibration.Index == 0) { if (csv0.Count > 0 && csv0[csv0.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { } else { csv0.Add(str); } } else if (camera.Calibration.Index == 1) { if (csv1.Count > 0 && csv1[csv1.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { } else { csv1.Add(str); } } else if (camera.Calibration.Index == 2) { if (csv2.Count > 0 && csv2[csv2.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { } else { csv2.Add(str); } } else if (camera.Calibration.Index == 3) { if (csv3.Count > 0 && csv3[csv3.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { } else { csv3.Add(str); } } } csvTime.Add(String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3}", iteration, _stopwatchGet.ElapsedMilliseconds, _stopwatchBA.ElapsedMilliseconds, _stopwatchSet.ElapsedMilliseconds)); string strBA = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}", iteration, prePosition.x, prePosition.y, prePosition.z, Math.Sqrt(prePosition.x * prePosition.x + prePosition.y * prePosition.y + prePosition.z * prePosition.z), kalmanPosition.x, kalmanPosition.y, kalmanPosition.z, Math.Sqrt(kalmanPosition.x * kalmanPosition.x + kalmanPosition.y * kalmanPosition.y + kalmanPosition.z * kalmanPosition.z)); if (csvBA.Count > 0 && csvBA[csvBA.Count - 1].Contains(prePosition.x.ToString(new CultureInfo("en-US")))) { } else { csvBA.Add(strBA); } iteration++; if (csvBA.Count == 100) { File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\avg_time.csv", csvTime); File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance.csv", csvBA); File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance0.csv", csv0); File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance1.csv", csv1); File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance2.csv", csv2); File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance3.csv", csv3); } }