public void CalibrateExtrinsics(PointF[] imagePoints, MCvPoint3D32f[] worldPoints)
        {
            if (Intrinsics == null)
                throw new Exception("Intrinsics of camera are still unknown, unable to calibrate extrinsic paramters.");
            
            Extrinsics = CameraCalibration.FindExtrinsicCameraParams2(worldPoints, imagePoints, Intrinsics);

            InitializeWorldAndViewMatrices();
            BoundingFrustum = new BoundingFrustum(View * Projection);

            using (var img = new Image<Bgr, byte>(ImageWidth, ImageHeight))
            {
                foreach (var p in imagePoints)
                    img.Draw(new Cross2DF(p, 20, 20), new Bgr(255, 0, 255), 1);

                var projectedCorners = CameraCalibration.ProjectPoints(worldPoints, Extrinsics, Intrinsics);
                foreach (var p in projectedCorners)
                    img.Draw(new Cross2DF(p, 6, 6), new Bgr(255, 255, 0), 1);

                var und = Intrinsics.Undistort(img);

                img.Save(Path.Combine(Global.TmpDir, "reproject.png"));
                und.Save(Path.Combine(Global.TmpDir, "undistorted.png"));
            }
        }

        public static ExtrinsicCameraParameters Calibrate(PointF[] locals, MCvPoint3D32f[] globals, Size pattern)
        {
            PointF[] pl = new PointF[]
            {
                locals[0 + (pattern.Height -1) * pattern.Width],
                locals[0 + 0 * pattern.Width],
                locals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
            };

            MCvPoint3D32f[] pg = new MCvPoint3D32f[]
            {
                globals[0 + (pattern.Height -1) * pattern.Width],
                globals[0 + 0 * pattern.Width],
                globals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width],
            };

            //f2 = (p1 - p0).Normalize(1);
            //f1 = (p2 - p0).Normalize(1);
            //f1 = (f1 - (f1.DotProduct(f2) * f2)).Normalize(1);
            //f3 = new DenseVector(new double[] { f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0] });
            //f3 = f3.Normalize(1);

            var plv = pl.Select(row => new DenseVector(new double[] { row.X, row.Y })).ToArray();

            return null;
        }

		public double Calibrate(Size resolution)
		{
			MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[1][];
			PointF[][] imagePoints = new PointF[1][];

			int count = this.FObjectPoints.Count;

			objectPoints[0] = new MCvPoint3D32f[count];
			imagePoints[0] = new PointF[count];

			for (int i = 0; i < count; i++)
			{
				objectPoints[0][i] = FObjectPoints[i];
				imagePoints[0][i] = FImagePoints[i];
			}

			IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
			ExtrinsicCameraParameters[] extrinsicParams;

			Matrix<double> mat = intrinsicParam.IntrinsicMatrix;
			mat[0, 0] = resolution.Width;
			mat[1, 1] = resolution.Height;
			mat[0, 2] = resolution.Width / 2.0d;
			mat[1, 2] = resolution.Height / 2.0d;
			mat[2, 2] = 1;

			CALIB_TYPE flags;
			flags = CALIB_TYPE.CV_CALIB_FIX_K1 | CALIB_TYPE.CV_CALIB_FIX_K2 | CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_FIX_K4 | CALIB_TYPE.CV_CALIB_FIX_K5 | CALIB_TYPE.CV_CALIB_FIX_K6 | CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;

			double error = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, resolution, intrinsicParam, flags, out extrinsicParams);
			this.FIntrinsics = new Intrinsics(intrinsicParam, resolution);
			this.FExtrinsics = new Extrinsics(extrinsicParams[0]);
			return error;
		}

 public static MCvPoint3D32f[] GetObjectPointsCopy()
 {
     var ops = new MCvPoint3D32f[M * N];
     for (int y = 0; y < N; ++y)
         for (int x = 0; x < M; ++x)
             ops[y * M + x] = new MCvPoint3D32f(Origin.X + x * Size.X, Origin.Y - (y * Size.Y), 0);
     return ops;
 }

      /// <summary>
      /// Find the approximate nearest position in 3D
      /// </summary>
      /// <param name="position">The position to start the search from</param>
      /// <param name="squareDist">The square distance of the nearest neighbour</param>
      /// <returns>The index with the nearest 3D position</returns>
      public int ApproximateNearestNeighbour(MCvPoint3D32f position, out double squareDist)
      {
         _query.Data[0, 0] = position.x;
         _query.Data[0, 1] = position.y;
         _query.Data[0, 2] = position.z;
         _flannIndex.KnnSearch(_query, _index, _distance, 1, 1);

         squareDist = _distance.Data[0, 0];
         return _index.Data[0, 0];
      }

        /// <summary>
        /// Estimates intrinsic camera parameters and extrinsic parameters for each of the views
        /// </summary>
        /// <param name="objectPoints">The 3D location of the object points. The first index is the index of image, second index is the index of the point</param>
        /// <param name="imagePoints">The 2D image location of the points. The first index is the index of the image, second index is the index of the point</param>
        /// <param name="imageSize">The size of the image, used only to initialize intrinsic camera matrix</param>
        /// <param name="intrinsicParam">The intrisinc parameters, might contains some initial values. The values will be modified by this function.</param>
        /// <param name="flags">Flags</param>
        /// <param name="extrinsicParams">The output array of extrinsic parameters.</param>
        /// <returns>The final reprojection error</returns>
        public static double CalibrateCamera(
         MCvPoint3D32f[][] objectPoints,
         PointF[][] imagePoints,
         Size imageSize,
         IntrinsicCameraParameters intrinsicParam,
         CvEnum.CALIB_TYPE flags,
         out ExtrinsicCameraParameters[] extrinsicParams)
        {
            Debug.Assert(objectPoints.Length == imagePoints.Length, "The number of images for objects points should be equal to the number of images for image points");
             int imageCount = objectPoints.Length;

             #region get the array that represent the point counts
             int[] pointCounts = new int[objectPoints.Length];
             for (int i = 0; i < objectPoints.Length; i++)
             {
            Debug.Assert(objectPoints[i].Length == imagePoints[i].Length, String.Format("Number of 3D points and image points should be equal in the {0}th image", i));
            pointCounts[i] = objectPoints[i].Length;
             }
             #endregion

             double reprojectionError = -1;
             using (Matrix<float> objectPointMatrix = ToMatrix(objectPoints))
             using (Matrix<float> imagePointMatrix = ToMatrix(imagePoints))
             using (Matrix<int> pointCountsMatrix = new Matrix<int>(pointCounts))
             using (Matrix<double> rotationVectors = new Matrix<double>(imageCount, 3))
             using (Matrix<double> translationVectors = new Matrix<double>(imageCount, 3))
             {
            reprojectionError = CvInvoke.cvCalibrateCamera2(
                objectPointMatrix.Ptr,
                imagePointMatrix.Ptr,
                pointCountsMatrix.Ptr,
                imageSize,
                intrinsicParam.IntrinsicMatrix,
                intrinsicParam.DistortionCoeffs,
                rotationVectors,
                translationVectors,
                flags);

            extrinsicParams = new ExtrinsicCameraParameters[imageCount];
            IntPtr matPtr = Marshal.AllocHGlobal(StructSize.MCvMat);
            for (int i = 0; i < imageCount; i++)
            {
               ExtrinsicCameraParameters p = new ExtrinsicCameraParameters();
               CvInvoke.cvGetRow(rotationVectors.Ptr, matPtr, i);
               CvInvoke.cvTranspose(matPtr, p.RotationVector.Ptr);
               CvInvoke.cvGetRow(translationVectors.Ptr, matPtr, i);
               CvInvoke.cvTranspose(matPtr, p.TranslationVector.Ptr);
               extrinsicParams[i] = p;
            }
            Marshal.FreeHGlobal(matPtr);
             }
             return reprojectionError;
        }

        public MCvPoint3D32f[] GetWorldPointsCopy()
        {
            var m = Saddles.Width;
            var n = Saddles.Height;

            var ops = new MCvPoint3D32f[m * n];
            for (int y = 0; y < n; ++y)
                for (int x = 0; x < m; ++x)
                    ops[y * m + x] = new MCvPoint3D32f(Board.Left + (x + 1) * Square.Width, Board.Top + (y + 1) * Square.Height, 0);

            return ops;
        }

        /// <summary>
        /// Create a linear flann index for 3D points
        /// </summary>
        /// <param name="points">The IPosition3D array</param>
        public Index3D(MCvPoint3D32f[] points)
        {
            _points = points;

             _dataHandle = GCHandle.Alloc(_points, GCHandleType.Pinned);
             _dataMatrix = new Matrix<float>(_points.Length, 3, _dataHandle.AddrOfPinnedObject());

            _flannIndex = new Index(_dataMatrix);

             _query = new Matrix<float>(1, 3);
             _distance = new Matrix<float>(1, 1);
             _index = new Matrix<int>(1, 1);
        }

		public static MCvPoint3D32f[] ObjectPoints(ISpread<Vector3D> input, bool toVVVV)
		{
			MCvPoint3D32f[] objectPoints = new MCvPoint3D32f[input.SliceCount];

			for (int i = 0; i < input.SliceCount; i++)
			{
				objectPoints[i].x = (float)input[i].x;
				objectPoints[i].y = toVVVV ? - (float)input[i].y : (float) input[i].y;
				objectPoints[i].z = toVVVV ? - (float)input[i].z : (float) input[i].z;
			}

			return objectPoints;
		}

        public void CalibrateIntrinsics(PointF[][] imagePoints, MCvPoint3D32f[][] worldPoints)
        {
            if (Intrinsics == null)
                Intrinsics = new IntrinsicCameraParameters();

            var calibType = CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;
            ExtrinsicCameraParameters[] extrinsics;

            var totalErr = CameraCalibration.CalibrateCamera(worldPoints, imagePoints, new Size(ImageWidth, ImageHeight), Intrinsics, calibType, out extrinsics);
            var err = Math.Sqrt(totalErr / (imagePoints.Length + imagePoints[0].Length));

            Console.WriteLine("Calibration Finished, Reprojection Error: {0}", err);

            Save(IntrinsicsFile, Intrinsics, ImageWidth, ImageHeight);            
        }

		public ChessBoard(int xCount, int yCount)
		{
			this.XCount = xCount;
			this.YCount = yCount;

			this.PatternSize = new Size(this.XCount - 1, this.YCount - 1);
			this.CornerCount = this.PatternSize.Width * this.PatternSize.Height;

			List<MCvPoint3D32f> cornerPoints = new List<MCvPoint3D32f>(this.CornerCount);
			for (int x = 0; x < this.XCount - 1; x++)
			{
				for (int y = 0; y < this.YCount - 1; y++)
				{
					MCvPoint3D32f cornerPoint = new MCvPoint3D32f(x * 25, y * 25, 0);
					cornerPoints.Add(cornerPoint);
				}
			}
			this.CornerPoints = cornerPoints.ToArray();
		}

		//called when data for any output pin is requested
		public void Evaluate(int SpreadMax)
		{
			FPinOutOutput.SliceCount = FPinInInput.SliceCount;

			if (FPinInIntrinsics[0] == null || FPinInExtrinsics[0] == null)
				return;

			MCvPoint3D32f[] o = new MCvPoint3D32f[1];
			PointF[] im = new PointF[1];

			for (int i=0; i<FPinInInput.SliceCount; i++)
			{
				o[0].x = (float)FPinInInput[i].x;
				o[0].y = (float)FPinInInput[i].y;
				o[0].z = (float)FPinInInput[i].z;

				im = CameraCalibration.ProjectPoints(o, FPinInExtrinsics[0], FPinInIntrinsics[0]);
				FPinOutOutput[i] = new Vector2D((double)im[0].X, (double)im[0].Y);
			}
		}

      /// <summary>
      /// Given the left and right image, computer the disparity map and the 3D point cloud.
      /// </summary>
      /// <param name="left">The left image</param>
      /// <param name="right">The right image</param>
      /// <param name="outputDisparityMap">The left disparity map</param>
      /// <param name="points">The 3D point cloud within a [-0.5, 0.5] cube</param>
      private static void Computer3DPointsFromStereoPair(IInputArray left, IInputArray right, Mat outputDisparityMap, out MCvPoint3D32f[] points)
      {
         Size size;
         using (InputArray ia = left.GetInputArray())
            size = ia.GetSize();
         
         using (StereoBM stereoSolver = new StereoBM())
         {
            stereoSolver.Compute(left, right, outputDisparityMap);

            float scale = Math.Max(size.Width, size.Height);

            //Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
            using (Matrix<double> q = new Matrix<double>(
               new double[,] {
                  {1.0, 0.0, 0.0, -size.Width/2}, //shift the x origin to image center
                  {0.0, -1.0, 0.0, size.Height/2}, //shift the y origin to image center and flip it upside down
                  {0.0, 0.0, -1.0, 0.0}, //Multiply the z value by -1.0, 
                  {0.0, 0.0, 0.0, scale}})) //scale the object's coordinate to within a [-0.5, 0.5] cube
               points = PointCollection.ReprojectImageTo3D(outputDisparityMap, q);
         }
      }

            public Camera(string path, int width, int height, PointF[] imageCorners, MCvPoint3D32f[] worldCorners)
            {
                Width = width;
                Height = height;

                Intrinsics = Markers.Chessboard.Load(path, width, height, out UndistortX, out UndistortY);
                Extrinsics = CameraCalibration.FindExtrinsicCameraParams2(worldCorners, imageCorners, Intrinsics);

                var ext = Extrinsics;
                View = new Matrix((float)ext.ExtrinsicMatrix[0, 0], -(float)ext.ExtrinsicMatrix[1, 0], -(float)ext.ExtrinsicMatrix[2, 0], 0,
                                         (float)ext.ExtrinsicMatrix[0, 1], -(float)ext.ExtrinsicMatrix[1, 1], -(float)ext.ExtrinsicMatrix[2, 1], 0,
                                         (float)ext.ExtrinsicMatrix[0, 2], -(float)ext.ExtrinsicMatrix[1, 2], -(float)ext.ExtrinsicMatrix[2, 2], 0,
                                         (float)ext.ExtrinsicMatrix[0, 3], -(float)ext.ExtrinsicMatrix[1, 3], -(float)ext.ExtrinsicMatrix[2, 3], 1);
                Intrinsics.GetIntrinsicMatrixValues(width, height, 0, 0,
                    out FovX, out FovY,
                    out FocalLength, out PrincipalPoint,
                    out PixelAspectRatio);
                World = Matrix.Invert(View);
                Projection = Matrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians((float)FovY), ((float)width) / ((float)height), 0.1f, 4.0f);
                BoundingFrustum = new BoundingFrustum(View * Projection);

                if (Global.No)
                    using (var img = new Image<Bgr, byte>(@"C:\Users\Jaap\My Dropbox\Data\Pentacorn.Vision\Offline\Casio Ex S5\Tape on Melamine\CIMG0606.JPG"))
                    {
                        foreach (var p in imageCorners)
                            img.Draw(new Cross2DF(p, 20, 20), new Bgr(255, 0, 255), 1);

                        var projectedCorners = CameraCalibration.ProjectPoints(worldCorners, Extrinsics, Intrinsics);
                        foreach (var p in projectedCorners)
                            img.Draw(new Cross2DF(p, 6, 6), new Bgr(255, 255, 0), 1);

                        var und = Intrinsics.Undistort(img);

                        img.Save(@"C:\Users\Jaap\Temp\img.png");
                        und.Save(@"C:\Users\Jaap\Temp\und.png");
                    }
            }

      /// <summary>
      /// Given the left and right image, computer the disparity map and the 3D point cloud.
      /// </summary>
      /// <param name="left">The left image</param>
      /// <param name="right">The right image</param>
      /// <param name="leftDisparityMap">The left disparity map</param>
      /// <param name="points">The 3D point cloud within a [-0.5, 0.5] cube</param>
      private static void Computer3DPointsFromImages(Image<Gray, Byte> left, Image<Gray, Byte> right, out Image<Gray, Int16> leftDisparityMap, out MCvPoint3D32f[] points)
      {
         Size size = left.Size;

         using (Image<Gray, Int16> leftDisparity = new Image<Gray, Int16>(size))
         using (Image<Gray, Int16> rightDisparity = new Image<Gray, Int16>(size))
         using (StereoGC gc = new StereoGC(16, 2))
         {
            gc.FindStereoCorrespondence(left, right, leftDisparity, rightDisparity);

            leftDisparityMap = leftDisparity * (-16);

            float scale = Math.Max(size.Width, size.Height);

            //Construct a simple Q matrix, if you have a matrix from cvStereoRectify, you should use that instead
            using (Matrix<double> q = new Matrix<double>(
               new double[,] {
                  {1.0, 0.0, 0.0, -size.Width/2}, //shift the x origin to image center
                  {0.0, -1.0, 0.0, size.Height/2}, //shift the y origin to image center and flip it upside down
                  {0.0, 0.0, 16.0, 0.0}, //Multiply the z value by 16, 
                  {0.0, 0.0, 0.0, scale}})) //scale the object's corrdinate to within a [-0.5, 0.5] cube
               points = PointCollection.ReprojectImageTo3D(leftDisparity, q);
         }
      }

      /// <summary>
      /// Reproject pixels on a 1-channel disparity map to array of 3D points.
      /// </summary>
      /// <param name="disparity">Disparity map</param>
      /// <param name="Q">The reprojection 4x4 matrix, can be arbitrary, e.g. the one, computed by cvStereoRectify</param>
      /// <returns>The reprojected 3D points</returns>
      public static MCvPoint3D32f[] ReprojectImageTo3D(Image<Gray, Byte> disparity, Matrix<double> Q)
      {
         Size size = disparity.Size;
         MCvPoint3D32f[] points3D = new MCvPoint3D32f[size.Width * size.Height];
         GCHandle handle = GCHandle.Alloc(points3D, GCHandleType.Pinned);

         using (Matrix<float> pts = new Matrix<float>(size.Height, size.Width, 3, handle.AddrOfPinnedObject(), 0))
            CvInvoke.cvReprojectImageTo3D(disparity.Ptr, pts.Ptr, Q);

         handle.Free();
         return points3D;
      }

 public void TestCrossProduct()
 {
     MCvPoint3D32f p1 = new MCvPoint3D32f(1.0f, 0.0f, 0.0f);
      MCvPoint3D32f p2 = new MCvPoint3D32f(0.0f, 1.0f, 0.0f);
      MCvPoint3D32f p3 = p1.CrossProduct(p2);
      Assert.IsTrue(new MCvPoint3D32f(0.0f, 0.0f, 1.0f).Equals(p3));
 }

        public void TestProjectPoints()
        {
            IntrinsicCameraParameters intrin = new IntrinsicCameraParameters();
             intrin.IntrinsicMatrix.SetIdentity();
             ExtrinsicCameraParameters extrin = new ExtrinsicCameraParameters();
             MCvPoint3D32f point = new MCvPoint3D32f();

             PointF[] points = CameraCalibration.ProjectPoints(new MCvPoint3D32f[] { point }, extrin, intrin);
        }

        public void TestOctTree()
        {
            MCvPoint3D32f[] pts = new MCvPoint3D32f[]
             {
            new MCvPoint3D32f(1, 2, 3),
            new MCvPoint3D32f(2, 3, 4),
            new MCvPoint3D32f(4, 5, 6),
            new MCvPoint3D32f(2, 2, 2)
             };

             using (Octree tree = new Octree(pts, 10, 10))
             {
            MCvPoint3D32f[] p = tree.GetPointsWithinSphere(new MCvPoint3D32f(0, 0, 0), 5);
            int i = p.Length;
             }
        }

 /// <summary> 
 /// Create a line segment with the specific start point and end point 
 /// </summary>
 /// <param name="p1">The first point on the line segment</param>
 /// <param name="p2">The second point on the line segment</param>
 public LineSegment3DF(MCvPoint3D32f p1, MCvPoint3D32f p2)
 {
    _p1 = p1;
    _p2 = p2;
 }