예제 #1
0
      /*
      /// <summary>
      /// Create a LevMarqSparse solver
      /// </summary>
      public LevMarqSparse()
      {
         _ptr = CvInvoke.CvCreateLevMarqSparse();
      }*/

      /// <summary>
      /// Useful function to do simple bundle adjustment tasks
      /// </summary>
      /// <param name="points">Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment</param>
      /// <param name="imagePoints">Projections of 3d points for every camera</param>
      /// <param name="visibility">Visibility of 3d points for every camera</param>
      /// <param name="cameraMatrix">Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="R">rotation matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="T">translation vector of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="distCoeffcients">distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="termCrit">Termination criteria, a reasonable value will be (30, 1.0e-12) </param>
      public static void BundleAdjust(
         MCvPoint3D64f[] points, MCvPoint2D64f[][] imagePoints, int[][] visibility,
         Matrix<double>[] cameraMatrix, Matrix<double>[] R, Matrix<double>[] T, Matrix<double>[] distCoeffcients, MCvTermCriteria termCrit)
      {
         using (Matrix<double> imagePointsMat = CvToolbox.GetMatrixFromPoints(imagePoints))
         using (Matrix<int> visibilityMat = CvToolbox.GetMatrixFromArrays(visibility))
         using (VectorOfMat cameraMatVec = new VectorOfMat())
         using (VectorOfMat rMatVec = new VectorOfMat())
         using (VectorOfMat tMatVec = new VectorOfMat())
         using (VectorOfMat distorMatVec = new VectorOfMat())
         {
            cameraMatVec.Push(cameraMatrix);
            rMatVec.Push(R);
            tMatVec.Push(T);
            distorMatVec.Push(distCoeffcients);


            GCHandle handlePoints = GCHandle.Alloc(points, GCHandleType.Pinned);

            CvInvoke.CvLevMarqSparseAdjustBundle(
               cameraMatrix.Length,
               points.Length, handlePoints.AddrOfPinnedObject(),
               imagePointsMat, visibilityMat, cameraMatVec, rMatVec, tMatVec, distorMatVec, ref termCrit);

            handlePoints.Free();

         }
      }
예제 #2
0
        public static double CalculateAngle3D(MCvPoint3D64f p1, MCvPoint3D64f p2, MCvPoint3D64f norm)
        {
            var ip = GetDistance(p1, p2);
            var cat1 = GetDistance(p1, norm);
            var cat2 = GetDistance(p2, norm);

            return CalculateAngle2D(new PointF(0, 0), new PointF((float)cat1, (float)cat2));
        }
예제 #3
0
        public void FilterAccMagnet(ref MCvPoint3D64f newAcc, ref MCvPoint3D64f newMagnet, double newCoeff)
        {
            double oldCoeff = 1 - newCoeff;
            if (this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
            {
                return;
            }

            MCvPoint3D64f ta = new MCvPoint3D64f((this.OldAMGFusedAcc.Value.x * oldCoeff + newAcc.x * newCoeff), (this.OldAMGFusedAcc.Value.y * oldCoeff + newAcc.y * newCoeff), (this.OldAMGFusedAcc.Value.z * oldCoeff + newAcc.z * newCoeff));
            MCvPoint3D64f tm = new MCvPoint3D64f((this.OldAMGFusedMagnet.Value.x * oldCoeff + newMagnet.x * newCoeff), (this.OldAMGFusedMagnet.Value.y * oldCoeff + newMagnet.y * newCoeff), (this.OldAMGFusedMagnet.Value.z * oldCoeff + newMagnet.z * newCoeff));

            newAcc = ta;
            newMagnet = tm;
        }
예제 #4
0
 public static Matrix<double> MCvPoint3D64fToMatrix(MCvPoint3D64f point, bool vertical = false)
 {
     if (vertical)
     {
         return new Matrix<double>(new double[,]
         {
             {point.x},
             {point.y},
             {point.z}
         });
     }
     return new Matrix<double>(new double[,]
     {
         {point.x, point.y, point.z}
     });
 }
예제 #5
0
      /// <summary>
      /// Convert geodetic coordinate to ECEF coordinate
      /// </summary>
      /// <param name="coordinate">the geodetic coordinate</param>
      /// <returns>The ECEF coordinate</returns>
      public static MCvPoint3D64f Geodetic2ECEF(GeodeticCoordinate coordinate)
      {
#if PINVOKE
         MCvPoint3D64f res = new MCvPoint3D64f();
         transformGeodetic2ECEF(ref coordinate, ref res);
         return res;
#else
         double sinPhi = Math.Sin(coordinate.Latitude);

         double N = A / Math.Sqrt(1.0 - E * E * sinPhi * sinPhi);

         double tmp1 = (N + coordinate.Altitude) * Math.Cos(coordinate.Latitude);

         return new MCvPoint3D64f(
            tmp1 * Math.Cos(coordinate.Longitude), 
            tmp1 * Math.Sin(coordinate.Longitude),
            ((B * B) / (A * A) * N + coordinate.Altitude) * sinPhi);
#endif
      }
예제 #6
0
 private extern static void transformGeodetic2ECEF(IntPtr datum, ref GeodeticCoordinate coordinate, ref MCvPoint3D64f ecef);
예제 #7
0
 public MCvPoint3D64f GetNormalizedPoint(MCvPoint3D64f point)
 {
     var norm = this.GetPointNorm(point);
     var res = new MCvPoint3D64f(point.x / norm, point.y / norm, point.z / norm);
     return res;
 }
예제 #8
0
        public void TestQuaternion2()
        {
            Random r = new Random();
             Quaternions q1 = new Quaternions();
             q1.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());

             Quaternions q2 = new Quaternions();
             q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());

             MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);

             MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
             double epsilon = 1.0e-8;
             Assert.Less(delta.x, epsilon);
             Assert.Less(delta.y, epsilon);
             Assert.Less(delta.z, epsilon);
        }
예제 #9
0
 /// <summary>
 /// Check if the specific point is in the Cuboid
 /// </summary>
 /// <param name="point">The point to be checked</param>
 /// <returns>True if the point is in the cuboid</returns>
 public bool Contains(MCvPoint3D64f point)
 {
     return point.x >= Min.x && point.y >= Min.y && point.z >= Min.z
     && point.x <= Max.x && point.y <= Max.y && point.z <= Max.z;
 }
예제 #10
0
 /// <summary>
 /// Check if the specific point is in the Cuboid
 /// </summary>
 /// <param name="point">The point to be checked</param>
 /// <returns>True if the point is in the cuboid</returns>
 public bool Contains(MCvPoint3D64f point)
 {
     return(point.X >= Min.X && point.Y >= Min.Y && point.Z >= Min.Z &&
            point.X <= Max.X && point.Y <= Max.Y && point.Z <= Max.Z);
 }
예제 #11
0
 public static double GetDistanceToPoint(MCvPoint3D64f origin, MCvPoint3D64f destination)
 {
     return GetPoint3dMagnitude(destination - origin);
 }
예제 #12
0
 public Matrix<double> MCvPoint3D64fToMatrix(MCvPoint3D64f point)
 {
     return new Matrix<double>(new double[,]
     {
         {point.x, point.y, point.z}
     });
 }
예제 #13
0
 private extern static void transformECEF2Geodetic(IntPtr datum, ref MCvPoint3D64f ecef, ref GeodeticCoordinate coordinate);
예제 #14
0
        public void GetOrthoNormalAccMagnetBasis(MCvPoint3D64f accPoint, MCvPoint3D64f magnetPoint, out MCvPoint3D64f x, out MCvPoint3D64f y, out MCvPoint3D64f z)
        {
            var accMagnetCross = accPoint.CrossProduct(magnetPoint);

            var magnetNormal = accMagnetCross.CrossProduct(accPoint);

            var aNorm = this.GetNormalizedPoint(accPoint);
            var mNorm = this.GetNormalizedPoint(magnetNormal);
            var amNorm = this.GetNormalizedPoint(accMagnetCross);

            x = this.GetPointNorm(mNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(1, 0, 0) : mNorm;
            //y = aNorm.Norm.CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 1, 0) : new MCvPoint3D64f(-aNorm.x, -aNorm.y, -aNorm.z);
            y = this.GetPointNorm(aNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 1, 0) : aNorm;
            z = this.GetPointNorm(amNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 0, 1) : amNorm;
        }
예제 #15
0
 /// <summary>
 /// Convert ECEF coordinate to geodetic coordinate
 /// </summary>
 /// <param name="ecef">The ecef coordinate</param>
 /// <returns>The geodetic coordinate</returns>
 public GeodeticCoordinate ECEF2Geodetic(MCvPoint3D64f ecef)
 {
    GeodeticCoordinate res = new GeodeticCoordinate();
    transformECEF2Geodetic(_ptr, ref ecef, ref res);
    return res;
 }
예제 #16
0
 /// <summary>
 /// Convert geodetic coordinate to ECEF coordinate
 /// </summary>
 /// <param name="coordinate">the geodetic coordinate</param>
 /// <returns>The ECEF coordinate</returns>
 public MCvPoint3D64f Geodetic2ECEF(GeodeticCoordinate coordinate)
 {
    MCvPoint3D64f res = new MCvPoint3D64f();
    transformGeodetic2ECEF(_ptr, ref coordinate, ref res);
    return res;
 }
예제 #17
0
 private extern static void transformNED2Geodetic(IntPtr datum, ref MCvPoint3D64f ned, ref GeodeticCoordinate refCoor, ref MCvPoint3D64f refEcef, ref GeodeticCoordinate coor);
예제 #18
0
 private extern static void transformGeodetic2ENU(IntPtr datum, ref GeodeticCoordinate coor, ref GeodeticCoordinate refCoor, ref MCvPoint3D64f refEcef, ref MCvPoint3D64f enu);
예제 #19
0
 public double GetPointNorm(MCvPoint3D64f point)
 {
     return Math.Sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
 }
예제 #20
0
 public static double GetDistance(MCvPoint3D64f p1, MCvPoint3D64f p2)
 {
     return Math.Sqrt(Math.Pow(p1.x - p2.x, 2) + Math.Pow(p1.y - p2.y, 2) + Math.Pow(p1.z - p2.z, 2));
 }
예제 #21
0
 public MCvPoint3D64f ScalePoint(MCvPoint3D64f point, double scale)
 {
     return new MCvPoint3D64f(point.x * scale, point.y * scale, point.z * scale);
 }
예제 #22
0
파일: Form1.cs 프로젝트: JimmHub/odoStuff
        private void RenderTranslatoin(MCvPoint3D64f position)
        {
            double xScale = this.GetTranslationRenderCoeffX();
            double yScale = this.GetTranslationRenderCoeffY();
            Bitmap bmp = new Bitmap(1000, 1000);
            double xOffset = bmp.Width / 2;
            double yOffset = bmp.Height / 2;
            var pointSize = new Size(bmp.Width / 100, bmp.Height / 100);
            var g = Graphics.FromImage(bmp);
            g.Clear(Color.White);

            g.DrawRectangle(Pens.Red, new Rectangle(new Point((int)(position.x / xScale + xOffset), (int)(position.z / yScale + yOffset)), pointSize));

            this.videoForm.RenderToStuffPictureBox2(bmp);
        }
예제 #23
0
        private void ProcessFrameFindFaces()
        {
            if (Options.StereoCalibrationOptions == null)
            {
                return;
            }

            var leftImageR = new Image<Gray, byte>(new Size(_cameras[0].Image.Width, _cameras[0].Image.Height));
            var rightImageR = new Image<Gray, byte>(new Size(_cameras[1].Image.Width, _cameras[1].Image.Height));

            CvInvoke.cvRemap(_cameras[0].Image.Ptr, leftImageR.Ptr,
                Options.StereoCalibrationOptions.MapXLeft, Options.StereoCalibrationOptions.MapYLeft, 0, new MCvScalar(0));

            CvInvoke.cvRemap(_cameras[1].Image.Ptr, rightImageR.Ptr,
                Options.StereoCalibrationOptions.MapXRight, Options.StereoCalibrationOptions.MapYRight, 0, new MCvScalar(0));

            //PointCollection.ReprojectImageTo3D()

            // find first face points
            var leftFaceRegions = Helper2D.GetFaceRegion2Ds(leftImageR, FaceWidth, FaceHeight, true, true);
            var rightFaceRegions = Helper2D.GetFaceRegion2Ds(rightImageR, FaceWidth, FaceHeight, true, true);

            FaceRegion2D leftFace;
            FaceRegion2D rightFace;

            if (leftFaceRegions != null
                && rightFaceRegions != null
                && (leftFace = leftFaceRegions.FirstOrDefault()) != null
                && (rightFace = rightFaceRegions.FirstOrDefault()) != null)
            {
                var leftPoints = new List<Point>();
                var rightPoints = new List<Point>();

                #region Points

                // face
                leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Face.Width / 2, leftFace.Face.Location.Y + leftFace.Face.Height / 2));
                rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Face.Width / 2, rightFace.Face.Location.Y + rightFace.Face.Height / 2));

                // left eye
                if (leftFace.LeftEye != null && rightFace.LeftEye != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.LeftEye.Location.X + leftFace.LeftEye.Width / 2,
                        leftFace.Face.Location.Y + leftFace.LeftEye.Location.Y + leftFace.LeftEye.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.LeftEye.Location.X + rightFace.LeftEye.Width / 2,
                        rightFace.Face.Location.Y + rightFace.LeftEye.Location.Y + rightFace.LeftEye.Height / 2));
                }

                // right eye
                if (leftFace.RightEye != null && rightFace.RightEye != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.RightEye.Location.X + leftFace.RightEye.Width / 2,
                        leftFace.Face.Location.Y + leftFace.RightEye.Location.Y + leftFace.RightEye.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.RightEye.Location.X + rightFace.RightEye.Width / 2,
                        rightFace.Face.Location.Y + rightFace.RightEye.Location.Y + rightFace.RightEye.Height / 2));
                }

                // mouth
                if (leftFace.Mouth != null && rightFace.Mouth != null)
                {
                    leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Mouth.Location.X + leftFace.Mouth.Width / 2,
                        leftFace.Face.Location.Y + leftFace.Mouth.Location.Y + leftFace.Mouth.Height / 2));

                    rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Mouth.Location.X + rightFace.Mouth.Width / 2,
                        rightFace.Face.Location.Y + rightFace.Mouth.Location.Y + rightFace.Mouth.Height / 2));
                }

                #endregion

                var pointCloud = new MCvPoint3D64f[leftPoints.Count];

                #region Calculate Point Cloud

                for (int i = 0; i < leftPoints.Count; i++)
                {
                    var d = rightPoints[i].X - leftPoints[i].X;

                    var X = leftPoints[i].X * Options.StereoCalibrationOptions.Q[0, 0] + Options.StereoCalibrationOptions.Q[0, 3];
                    var Y = leftPoints[i].Y * Options.StereoCalibrationOptions.Q[1, 1] + Options.StereoCalibrationOptions.Q[1, 3];
                    var Z = Options.StereoCalibrationOptions.Q[2, 3];
                    var W = d * Options.StereoCalibrationOptions.Q[3, 2] + Options.StereoCalibrationOptions.Q[3, 3];

                    X = X / W;
                    Y = Y / W;
                    Z = Z / W;

                    leftImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, leftPoints[i], new Gray(255));
                    rightImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, rightPoints[i], new Gray(255));

                    pointCloud[i] = new MCvPoint3D64f(X, Y, Z);
                }

                #endregion

                if (pointCloud.Length >= 4)
                {
                    var srcPoints = new Matrix<float>(pointCloud.Length, 3);
                    var dstPoints = new Matrix<float>(pointCloud.Length, 3);

                    for (int i = 0; i < pointCloud.Length; i++)
                    {
                        srcPoints[i, 0] = (float)pointCloud[i].x;
                        srcPoints[i, 1] = (float)pointCloud[i].y;
                        srcPoints[i, 2] = (float)pointCloud[i].z;

                        dstPoints[i, 0] = (float)pointCloud[i].x;
                        dstPoints[i, 1] = (float)pointCloud[i].y;
                        dstPoints[i, 2] = 0;
                    }

                    var mapMatrix = new Matrix<double>(3,3);
                    CvInvoke.cvGetPerspectiveTransform(srcPoints.Ptr, dstPoints.Ptr, mapMatrix.Ptr);

                    try
                    {
                        if (_transformed == null)
                        {
                            _transformed = new Image<Gray, byte>(leftImageR.Width, leftImageR.Height);
                        }

                        CvInvoke.cvPerspectiveTransform(leftImageR.Ptr, _transformed.Ptr, mapMatrix.Ptr);

                        //_transformed = leftImageR.WarpAffine(mapMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
                    }
                    catch(Exception ex)
                    {

                    }

                    //HomographyMatrix homographyMatrix = CameraCalibration.FindHomography(srcPoints, dstPoints, HOMOGRAPHY_METHOD.RANSAC, 2);

                    //_transformed = leftImageR.WarpPerspective(homographyMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
                }

            }

            var oldLeft = _cameras[0].Image;
            var oldRight = _cameras[1].Image;

            _cameras[0].Image = leftImageR;
            _cameras[1].Image = rightImageR;

            oldLeft.Dispose();
            oldRight.Dispose();
        }
        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);
            }
        }
예제 #25
0
 public static MCvPoint3D64f GetCubeCenter(MCvPoint3D64f[] cubeObjectPoints)
 {
     double mx = 0, my = 0, mz = 0;
     for (int i = 0; i < cubeObjectPoints.Length; i++)
     {
         mx += cubeObjectPoints[i].x / cubeObjectPoints.Length;
         my += cubeObjectPoints[i].y / cubeObjectPoints.Length;
         mz += cubeObjectPoints[i].z / cubeObjectPoints.Length;
     }
     return new MCvPoint3D64f(mx, my, mz);
 }
예제 #26
0
      public void TestQuaternionEulerAngleAndRotate()
      {
         double epsilon = 1.0e-12;
         Random r = new Random();
         Quaternions q1 = new Quaternions();
         double roll1 = r.NextDouble(), pitch1 = r.NextDouble(), yaw1 = r.NextDouble();
         double roll2 = 0, pitch2 = 0, yaw2 = 0;
         q1.SetEuler(roll1, pitch1, yaw1);
         q1.GetEuler(ref roll2, ref pitch2, ref yaw2);
         EmguAssert.IsTrue(Math.Abs(roll1 - roll2) < epsilon);
         EmguAssert.IsTrue(Math.Abs(pitch1 - pitch2) < epsilon);
         EmguAssert.IsTrue(Math.Abs(yaw1 - yaw2) < epsilon);

         Quaternions q2 = new Quaternions();
         q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());

         MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);

         MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));

         EmguAssert.IsTrue(delta.X < epsilon);
         EmguAssert.IsTrue(delta.Y < epsilon);
         EmguAssert.IsTrue(delta.Z < epsilon);

      }
예제 #27
0
 public static double GetPoint3dMagnitude(MCvPoint3D64f point)
 {
     return Math.Sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
 }
예제 #28
0
      public void TestAxisAngleCompose()
      {
         MCvPoint3D64f angle1 = new MCvPoint3D64f(4.1652539565753417e-022, -9.4229054916424228e-022, 5.1619136559035708e-008);
         MCvPoint3D64f angle2 = new MCvPoint3D64f(4.3209729769679014e-023, 3.2042397847543764e-023, -6.4083339340765912e-008);
         Quaternions q1 = new Quaternions();
         q1.AxisAngle = angle1;
         Quaternions q2 = new Quaternions();
         q2.AxisAngle = angle2;
         Quaternions q = q1 * q2;

         MCvPoint3D64f angle = q.AxisAngle;
         EmguAssert.AreNotEqual(double.NaN, angle.X, "Invalid value x");
         EmguAssert.AreNotEqual(double.NaN, angle.Y, "Invalid value y");
         EmguAssert.AreNotEqual(double.NaN, angle.Z, "Invalid value z");
      }
        /// <summary>
        /// Compute the minimum and maximum value from the points
        /// </summary>
        /// <param name="points">The points</param>
        /// <param name="min">The minimum x,y,z values</param>
        /// <param name="max">The maximum x,y,z values</param>
        public static void GetMinMax(IEnumerable<MCvPoint3D64f> points, out MCvPoint3D64f min, out MCvPoint3D64f max)
        {
            min = new MCvPoint3D64f();
             min.x = min.y = min.z = double.MaxValue;
             max = new MCvPoint3D64f();
             max.x = max.y = max.z = double.MinValue;

             foreach (MCvPoint3D64f p in points)
             {
            min.x = Math.Min(min.x, p.x);
            min.y = Math.Min(min.y, p.y);
            min.z = Math.Min(min.z, p.z);
            max.x = Math.Max(max.x, p.x);
            max.y = Math.Max(max.y, p.y);
            max.z = Math.Max(max.z, p.z);
             }
        }
예제 #30
0
 /// <summary>
 /// Check if the specific point is in the Cuboid
 /// </summary>
 /// <param name="point">The point to be checked</param>
 /// <returns>True if the point is in the cuboid</returns>
 public bool Contains(MCvPoint3D64f point)
 {
     return(point.x >= Min.x && point.y >= Min.y && point.z >= Min.z &&
            point.x <= Max.x && point.y <= Max.y && point.z <= Max.z);
 }
예제 #31
0
파일: Cuboid.cs 프로젝트: neutmute/emgucv
 /// <summary>
 /// Check if the specific point is in the Cuboid
 /// </summary>
 /// <param name="point">The point to be checked</param>
 /// <returns>True if the point is in the cuboid</returns>
 public bool Contains(MCvPoint3D64f point)
 {
    return point.X >= Min.X && point.Y >= Min.Y && point.Z >= Min.Z
       && point.X <= Max.X && point.Y <= Max.Y && point.Z <= Max.Z;
 }
예제 #32
0
        public double[][] GetAccMagnetOrientationMatrix(MEMSReadingsSet3f newReadings, bool useAccMagnet, bool useGyroscope, bool useLowpassFilter, bool useAdaptiveFiltering, double accMagnetFilterCoeff, double gyroFilterCoeff)
        {
            if (useAdaptiveFiltering)
            {
                this.CalcFilterCoeffs(newReadings, out gyroFilterCoeff, out accMagnetFilterCoeff, accMagnetFilterCoeff, gyroFilterCoeff);
            }

            double[][] res = new double[3][];
            for (int i = 0; i < 3; ++i)
            {
                res[i] = new double[3];
            }

            MCvPoint3D64f rawAccPoint = new MCvPoint3D64f(newReadings.AccVector3f.Values[0], newReadings.AccVector3f.Values[1], newReadings.AccVector3f.Values[2]);
            MCvPoint3D64f rawMagnetPoint = new MCvPoint3D64f(newReadings.MagnetVector3f.Values[0], newReadings.MagnetVector3f.Values[1], newReadings.MagnetVector3f.Values[2]);
            double gyroSpeedMul = 1;
            if (useAdaptiveFiltering)
            {
                if(useAccMagnet)
                {
                    gyroSpeedMul = this.GyroSpeedMul;
                }
            }
            var gyroDiffMatr = this.GetGyroRotationMatrix(newReadings.GyroVector3f, gyroSpeedMul);

            MCvPoint3D64f accFilteredPoint;
            MCvPoint3D64f magnetFilteredPoint;

            if (useLowpassFilter && this.OldAMGFusedAcc != null && this.OldAMGFusedMagnet != null)
            {
                if (useAccMagnet)
                {
                    //this.FilterAccMagnet(ref resAccPoint, ref resMagnetPoint, accMagnetFilterCoeff);
                    accFilteredPoint = new MCvPoint3D64f(
                        rawAccPoint.x * accMagnetFilterCoeff + OldAMGFusedAcc.Value.x * (1 - accMagnetFilterCoeff),
                        rawAccPoint.y * accMagnetFilterCoeff + OldAMGFusedAcc.Value.y * (1 - accMagnetFilterCoeff),
                        rawAccPoint.z * accMagnetFilterCoeff + OldAMGFusedAcc.Value.z * (1 - accMagnetFilterCoeff)
                        );

                    magnetFilteredPoint = new MCvPoint3D64f(
                        rawMagnetPoint.x * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.x * (1 - accMagnetFilterCoeff),
                        rawMagnetPoint.y * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.y * (1 - accMagnetFilterCoeff),
                        rawMagnetPoint.z * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.z * (1 - accMagnetFilterCoeff)
                        );
                }
                else
                {
                    accFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedAcc.Value.x, this.OldAMGFusedAcc.Value.y, this.OldAMGFusedAcc.Value.z);
                    magnetFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedMagnet.Value.x, this.OldAMGFusedMagnet.Value.y, this.OldAMGFusedMagnet.Value.z);
                }
            }
            else
            {
                accFilteredPoint = new MCvPoint3D64f(rawAccPoint.x, rawAccPoint.y, rawAccPoint.z);
                magnetFilteredPoint = new MCvPoint3D64f(rawMagnetPoint.x, rawMagnetPoint.y, rawMagnetPoint.z);
            }

            MCvPoint3D64f resAccPoint = new MCvPoint3D64f(0, 9, 0);
            MCvPoint3D64f resMagnetPoint = new MCvPoint3D64f(48, 2, 0);

            if(this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
            {
                resAccPoint = accFilteredPoint;
                resMagnetPoint = magnetFilteredPoint;
            }
            else
            {
                if (useGyroscope && useAccMagnet)
                {
                    resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(accFilteredPoint).Mul(1 - gyroFilterCoeff)));
                    resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(magnetFilteredPoint).Mul(1 - gyroFilterCoeff)));
                }
                else if (useAccMagnet)
                {
                    resAccPoint = accFilteredPoint;
                    resMagnetPoint = magnetFilteredPoint;
                }
                else if(useGyroscope)
                {
                    resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr));
                    resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr));
                }
            }

            this.OldAMGFusedAcc = resAccPoint;
            this.OldAMGFusedMagnet = resMagnetPoint;

            this.OldRawAcc = rawAccPoint;
            this.OldRawMagnet = rawMagnetPoint;

            this.OldFilteredAcc = accFilteredPoint;
            this.OldFilteredMagnet = magnetFilteredPoint;

            this.OldGyroReadings = newReadings.GyroVector3f;

            MCvPoint3D64f x;
            MCvPoint3D64f y;
            MCvPoint3D64f z;

            this.GetOrthoNormalAccMagnetBasis(resAccPoint, resMagnetPoint, out x, out y, out z);

            Matrix<double> accMagnetMatrix = new Matrix<double>(3, 3);

            accMagnetMatrix[0, 0] = x.x; accMagnetMatrix[0, 1] = x.y; accMagnetMatrix[0, 2] = x.z;
            accMagnetMatrix[1, 0] = y.x; accMagnetMatrix[1, 1] = y.y; accMagnetMatrix[1, 2] = y.z;
            accMagnetMatrix[2, 0] = z.x; accMagnetMatrix[2, 1] = z.y; accMagnetMatrix[2, 2] = z.z;

            //AccMagnetGyro fuse
            Matrix<double> newGAMFusedMatrix = null;
            //old amg fusion
            //if (this.OldGAMFusedMatrix == null)
            //{
            //    newGAMFusedMatrix = accMagnetMatrix;
            //}
            //else
            //{
            //    var newGyroMatrix = this.OldGAMFusedMatrix.Mul(gyroDiffMatr);
            //    newGAMFusedMatrix = this.FuseAccMagnetGyro(accMagnetMatrix, newGyroMatrix, gyroFilterCoeff);
            //}
            newGAMFusedMatrix = accMagnetMatrix;
            OldGAMFusedMatrix = newGAMFusedMatrix;

            ////

            var resMatrix = newGAMFusedMatrix.Transpose();

            //res = this.ConvertMatrix(m);
            //gyro test
            //m = this.OldGyroMatrix;
            //m = m.Transpose();
            //
            //auto res set up
            //for (int i = 0; i < 3; ++i)
            //{
            //    for (int j = 0; j < 3; ++j)
            //    {
            //        res[i][j] = resMatrix[i, j];
            //    }
            //}
            ////
            //manual res set up
            res[0][0] = -resMatrix[0, 0]; res[0][1] = -resMatrix[0, 1]; res[0][2] = resMatrix[0, 2];
            res[1][0] = -resMatrix[1, 0]; res[1][1] = -resMatrix[1, 1]; res[1][2] = resMatrix[1, 2];
            res[2][0] = -resMatrix[2, 0]; res[2][1] = -resMatrix[2, 1]; res[2][2] = resMatrix[2, 2];
            ////

            return res;
        }