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