Пример #1
0
    public void pnp(Point3f[] objPts, Point2f[] imgPts, Camera cam, SolvePnPFlags type, string filename)
    {
        Debug.Log($" PNP   : {objPts.Length} : {imgPts.Length}");

        TextWriter rotFile   = new StreamWriter(filename + "-RotPnP.txt");
        TextWriter transFile = new StreamWriter(filename + "-TransPnP.txt");

        using (var objPtsMat = new Mat(objPts.Length, 1, MatType.CV_32FC3, objPts))
            using (var imgPtsMat = new Mat(imgPts.Length, 1, MatType.CV_32FC2, imgPts))
                using (var cameraMatrixMat = new Mat(3, 3, MatType.CV_64FC1, getCameraMatrix(cam)))
                    using (var distMat = Mat.Zeros(5, 0, MatType.CV_64FC1))
                        using (var rvecMat = new Mat())
                            using (var tvecMat = new Mat())
                            {
                                Cv2.SolvePnP(objPtsMat, imgPtsMat, cameraMatrixMat, distMat, rvecMat, tvecMat, flags: type);
                                //Debug.Log("Solved " + rvecMat + "  :  "+tvecMat);

                                Mat rot_matrix  = Mat.Zeros(3, 3, MatType.CV_64FC1);
                                Mat tr_matrix   = Mat.Zeros(3, 1, MatType.CV_64FC1);
                                Mat proj_matrix = Mat.Zeros(3, 4, MatType.CV_64FC1);

                                Cv2.Rodrigues(rvecMat, rot_matrix);
                                tr_matrix = tvecMat;

                                rotFile.WriteLine(rot_matrix.At <double>(0, 0) + " " + rot_matrix.At <double>(0, 1) + " " + rot_matrix.At <double>(0, 2));
                                rotFile.WriteLine(rot_matrix.At <double>(1, 0) + " " + rot_matrix.At <double>(1, 1) + " " + rot_matrix.At <double>(1, 2));
                                rotFile.WriteLine(rot_matrix.At <double>(2, 0) + " " + rot_matrix.At <double>(2, 1) + " " + rot_matrix.At <double>(2, 2));
                                transFile.WriteLine(tr_matrix.At <double>(0) + " " + tr_matrix.At <double>(1) + " " + tr_matrix.At <double>(2));

                                proj_matrix.Set <double>(0, 0, rot_matrix.At <double>(0, 0));
                                proj_matrix.Set <double>(0, 1, rot_matrix.At <double>(0, 1));
                                proj_matrix.Set <double>(0, 2, rot_matrix.At <double>(0, 2));

                                proj_matrix.Set <double>(1, 0, rot_matrix.At <double>(1, 0));
                                proj_matrix.Set <double>(1, 1, rot_matrix.At <double>(1, 1));
                                proj_matrix.Set <double>(1, 2, rot_matrix.At <double>(1, 2));

                                proj_matrix.Set <double>(2, 0, rot_matrix.At <double>(2, 0));
                                proj_matrix.Set <double>(2, 1, rot_matrix.At <double>(2, 1));
                                proj_matrix.Set <double>(2, 2, rot_matrix.At <double>(2, 2));

                                proj_matrix.Set <double>(0, 3, tr_matrix.At <double>(0));
                                proj_matrix.Set <double>(1, 3, tr_matrix.At <double>(1));
                                proj_matrix.Set <double>(2, 3, tr_matrix.At <double>(2));

                                //rot = eulerAngle(rot_matrix);

                                // Debug.Log($"1  : Matrix4x4.TRS(translation, rotation, scale) {Matrix4x4.TRS(cam.transform.position, cam.transform.rotation, new Vector3(10, 10, 10))}");
                                // Debug.Log($"1  : Matrix4x4.TRS(translation, rotation, scale) {Matrix4x4.TRS(cam.transform.position, cam.transform.rotation, new Vector3(10, 10, 10))}");

                                //Debug.Log($"rot1 {cam.transform.eulerAngles} rot2 { rot * Mathf.Rad2Deg}");
                                //Debug.Log($"tr1: {cam.transform.position} tr2: {tr_matrix.At<double>(0)}, {tr_matrix.At<double>(1)}, {tr_matrix.At<double>(2)}");
                                projectionMatrix = proj_matrix;
                            }

        rotFile.Flush();
        transFile.Flush();
    }
Пример #2
0
    void pnp(Point3f[] objPoints, Point2f[] imgPts, double[, ] cameraMatrix, SolvePnPFlags type)
    {
        using (var objPtsMat = new Mat(objPoints.Length, 1, MatType.CV_32FC3, objPoints))
            using (var imgPtsMat = new Mat(imgPts.Length, 1, MatType.CV_32FC2, imgPts))
                using (var cameraMatrixMat = new Mat(3, 3, MatType.CV_64FC1, cameraMatrix))
                    using (var distMat = Mat.Zeros(5, 0, MatType.CV_64FC1))
                        using (var rvecMat = new Mat())
                            using (var tvecMat = new Mat())
                            {
                                Cv2.SolvePnP(objPtsMat, imgPtsMat, cameraMatrixMat, distMat, rvecMat, tvecMat, flags: type);
                                //Debug.Log("Solved " + rvecMat + "  :  "+tvecMat);

                                Mat rot_matrix  = Mat.Zeros(3, 3, MatType.CV_64FC1);
                                Mat tr_matrix   = Mat.Zeros(3, 1, MatType.CV_64FC1);
                                Mat proj_matrix = Mat.Zeros(3, 4, MatType.CV_64FC1);


                                Cv2.Rodrigues(rvecMat, rot_matrix);
                                tr_matrix = tvecMat;

                                proj_matrix.Set <double>(0, 0, rot_matrix.At <double>(0, 0));
                                proj_matrix.Set <double>(0, 1, rot_matrix.At <double>(0, 1));
                                proj_matrix.Set <double>(0, 2, rot_matrix.At <double>(0, 2));

                                proj_matrix.Set <double>(1, 0, rot_matrix.At <double>(1, 0));
                                proj_matrix.Set <double>(1, 1, rot_matrix.At <double>(1, 1));
                                proj_matrix.Set <double>(1, 2, rot_matrix.At <double>(1, 2));

                                proj_matrix.Set <double>(2, 0, rot_matrix.At <double>(2, 0));
                                proj_matrix.Set <double>(2, 1, rot_matrix.At <double>(2, 1));
                                proj_matrix.Set <double>(2, 2, rot_matrix.At <double>(2, 2));

                                proj_matrix.Set <double>(0, 3, tr_matrix.At <double>(0));
                                proj_matrix.Set <double>(1, 3, tr_matrix.At <double>(1));
                                proj_matrix.Set <double>(2, 3, tr_matrix.At <double>(2));

                                projMatrix = proj_matrix;

                                // Debug.Log(" rvecMat.Height  " +rvecMat.Height);
                                // for (int y = 0; y < rvecMat.Height; y++){
                                //     Debug.Log(" r-double : "+rvecMat.At<double>(y));
                                //     Debug.Log(" r-float : "+(float)rvecMat.At<double>(y));

                                //     Debug.Log(" t-double : "+tvecMat.At<double>(y));
                                //     Debug.Log(" t-float : "+(float)tvecMat.At<double>(y));
                                // }

                                // Debug.Log(" Quaternion.LookRotation(relativePos, Vector3.up) " + Camera.main.transform.position);
                                // Debug.Log(" transform.rotation " + Camera.main.transform.eulerAngles);

                                // Debug.Log(" Rotation matrix FLOAT : "
                                //         + new Vector3(rvecMat.Get<float>(0), rvecMat.Get<float>(1), rvecMat.Get<float>(2)));
                                // Debug.Log(" ScreenToWorldPoint() for Rotation matrix FLOAT : "
                                //         + Camera.main.ScreenToWorldPoint(new Vector3(rvecMat.Get<float>(0), rvecMat.Get<float>(1), rvecMat.Get<float>(2))));

                                // Debug.Log(" Rotation matrix DOUBLE : "
                                //         + new Vector3((float)rvecMat.Get<double>(0), (float)rvecMat.Get<double>(1), (float)rvecMat.Get<double>(2)));
                                // Debug.Log(" ScreenToWorldPoint() for Rotation matrix DOUBLE : "
                                //         + Camera.main.ScreenToWorldPoint(new Vector3((float)rvecMat.Get<double>(0), (float)rvecMat.Get<double>(1), (float)rvecMat.Get<double>(2))));

                                // Debug.Log(" Translation matrix FLOAT : "
                                //         + new Vector3(tvecMat.Get<float>(0), tvecMat.Get<float>(1), tvecMat.Get<float>(2)));
                                // Debug.Log(" ScreenToWorldPoint() for Translation matrix FLOAT : "
                                //         + Camera.main.ScreenToWorldPoint(new Vector3(tvecMat.Get<float>(0), tvecMat.Get<float>(1), tvecMat.Get<float>(2))));

                                // Debug.Log(" Translation matrix DOUBLE : "
                                //         + new Vector3((float)tvecMat.Get<double>(0), (float)tvecMat.Get<double>(1), (float)tvecMat.Get<double>(2)));
                                // Debug.Log(" ScreenToWorldPoint() for Translation matrix DOUBLE : "
                                //         + Camera.main.ScreenToWorldPoint(new Vector3((float)tvecMat.Get<double>(0), (float)tvecMat.Get<double>(1), (float)tvecMat.Get<double>(2))));


                                // Vector3 m = new Vector3 ((float)rvecMat.Get<double>(0), (float)rvecMat.Get<double>(1), (float)rvecMat.Get<double>(2));
                                //Vector3 m = new Vector3 (rvecMat.Get<float>(0), rvecMat.Get<float>(1), rvecMat.Get<float>(2));
                                // float theta = (float)(Math.Sqrt(rvecMat.Get<double>(0)*rvecMat.Get<double>(0)
                                //     + rvecMat.Get<double>(1)*rvecMat.Get<double>(1)
                                //     + rvecMat.Get<double>(2)*rvecMat.Get<double>(2))*180/Math.PI);

                                // Vector3 axis = Camera.main.ScreenToWorldPoint(new Vector3 (-(float)rvecMat.Get<double>(0), (float)rvecMat.Get<double>(1), -(float)rvecMat.Get<double>(2)));

                                // float theta = (float)(Math.Sqrt(m.x*m.x + m.y*m.y + m.z*m.z)*180/Math.PI);
                                // Vector3 axis = new Vector3 (m.x, -m.y, m.z);
                                // Quaternion rot = Quaternion.AngleAxis(theta, axis);

                                // Quaternion rot = Quaternion.LookRotation(new Vector3(-(float)rot_matrix.At<double>(0,2), (float)rot_matrix.At<double>(1,2), -(float)rot_matrix.At<double>(2,2))
                                //     , new Vector3(-(float)rot_matrix.At<double>(0,1), (float)rot_matrix.At<double>(1,1), -(float)rot_matrix.At<double>(2,1)));

                                // // Quaternion rot = Quaternion.LookRotation(new Vector3(rot_matrix.At<float>(0,2), -rot_matrix.At<float>(1,2), rot_matrix.At<float>(2,2))
                                // //     , new Vector3(rot_matrix.At<float>(0,1), -rot_matrix.At<float>(1,1), rot_matrix.At<float>(2,1)));

                                // Debug.Log(" Rotation " + rot.eulerAngles);

                                // double sy = Math.Sqrt(rot_matrix.At<double>(2,1) * rot_matrix.At<double>(2,1) +  rot_matrix.At<double>(2,2) * rot_matrix.At<double>(2,2) );
                                // var x = Math.Atan2(rot_matrix.At<double>(1,0) , rot_matrix.At<double>(0,0));
                                // var y = Math.Atan2(-rot_matrix.At<double>(2,0), sy);
                                // var z = Math.Atan2(rot_matrix.At<double>(2,1), rot_matrix.At<double>(2,2));

                                // double theta = Math.Acos((rot_matrix.At<double>(0,0) + rot_matrix.At<double>(1,1) + rot_matrix.At<double>(2,2) - 1)/2);
                                // var x = (rot_matrix.At<double>(2,1) - rot_matrix.At<double>(1,2))/(2 *  Math.Sin(theta));
                                // var y = (rot_matrix.At<double>(0,2) - rot_matrix.At<double>(2,0))/(2 *  Math.Sin(theta));
                                // var z = (rot_matrix.At<double>(1,0) - rot_matrix.At<double>(0,1))/(2 *  Math.Sin(theta));

                                // Debug.Log(" Rotation screen : "+ x + " : "+y+" : "+ z);
                                // Debug.Log(" Rotation screen : "+ new Vector3((float)x,(float)y,(float)z));
                                // Debug.Log(" Rotation : "+ Camera.main.ScreenToWorldPoint(new Vector3((float)x,(float)y,(float)z)));

                                // Mat cameraMatrix1 = new Mat();
                                // Mat rotMatrix =  new Mat();
                                // Mat transVect = new Mat();
                                // Mat rotMatrixX = new Mat();
                                // Mat rotMatrixY = new Mat();
                                // Mat rotMatrixZ = new Mat();
                                // Mat eulerAngles = new Mat();

                                // Cv2.DecomposeProjectionMatrix(proj_matrix,
                                //                    cameraMatrix1,
                                //                    rotMatrix,
                                //                    transVect,
                                //                    rotMatrixX,
                                //                    rotMatrixY,
                                //                    rotMatrixZ,
                                //                    eulerAngles);

                                // Debug.Log(" Camera matrix "+ cameraMatrix[0, 0] + " : "+ cameraMatrix[0, 1] +" : "+ cameraMatrix[0, 2] );
                                // Debug.Log(" cameraMatrix1 "+cameraMatrix1.At<double>(0, 0) + " : "+cameraMatrix1.At<double>(0,1)+ " : "+cameraMatrix1.At<double>(0,2) );
                                // Debug.Log(" eulerAngles "+eulerAngles.At<double>(0) + " : "+eulerAngles.At<double>(1)+ " : "+eulerAngles.At<double>(2) );

                                //Debug.Log($"Projection Matrix   {proj_matrix}");

                                // foreach (var p3D in objPoints){

                                //     Mat point3d_vec = new Mat(4, 1, MatType.CV_64FC1);
                                //     point3d_vec.Set<double>(0, p3D.X);
                                //     point3d_vec.Set<double>(1, p3D.Y);
                                //     point3d_vec.Set<double>(2, p3D.Z);
                                //     point3d_vec.Set<double>(3, 1);


                                //     Mat point2d_vec = new Mat(4, 1, MatType.CV_64FC1);
                                //     point2d_vec = cameraMatrixMat * proj_matrix * point3d_vec;
                                //     //Debug.Log(" 3d Points " + p3D.X + " " + p3D.Y + " " + p3D.Z);
                                //     //Debug.Log(" 2d point  x : " + point2d_vec.At<double>(0)/point2d_vec.At<double>(2) + " y " + point2d_vec.At<double>(1)/point2d_vec.At<double>(2));
                                // }
                            }
    }
Пример #3
0
        /// <summary>
        /// computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible.
        /// </summary>
        /// <param name="objectPoints">Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, 
        /// where N is the number of points. List&lt;Point3f&gt; can be also passed here.</param>
        /// <param name="imagePoints">Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. 
        /// List&lt;Point2f&gt; can be also passed here.</param>
        /// <param name="cameraMatrix">Input 3x3 camera matrix</param>
        /// <param name="distCoeffs">Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. 
        /// If the vector is null, the zero distortion coefficients are assumed.</param>
        /// <param name="rvec">Output rotation vector that, together with tvec , brings points from the model coordinate system 
        /// to the camera coordinate system.</param>
        /// <param name="tvec">Output translation vector.</param>
        /// <param name="useExtrinsicGuess">If true, the function uses the provided rvec and tvec values as initial approximations 
        /// of the rotation and translation vectors, respectively, and further optimizes them.</param>
        /// <param name="iterationsCount">Number of iterations.</param>
        /// <param name="reprojectionError">Inlier threshold value used by the RANSAC procedure. 
        /// The parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier.</param>
        /// <param name="minInliersCount">Number of inliers. If the algorithm at some stage finds more inliers than minInliersCount , it finishes.</param>
        /// <param name="inliers">Output vector that contains indices of inliers in objectPoints and imagePoints .</param>
        /// <param name="flags">Method for solving a PnP problem</param>
        public static void SolvePnPRansac(
            IEnumerable<Point3f> objectPoints,
            IEnumerable<Point2f> imagePoints,
            double[,] cameraMatrix,
            IEnumerable<double> distCoeffs,
            out double[] rvec, out double[] tvec,
            out int[] inliers,
            bool useExtrinsicGuess = false,
            int iterationsCount = 100,
            float reprojectionError = 8.0f,
            int minInliersCount = 100,
            SolvePnPFlags flags = SolvePnPFlags.Iterative)
        {
            if (objectPoints == null)
                throw new ArgumentNullException("objectPoints");
            if (imagePoints == null)
                throw new ArgumentNullException("imagePoints");
            if (cameraMatrix == null)
                throw new ArgumentNullException("cameraMatrix");

            if (cameraMatrix.GetLength(0) != 3 || cameraMatrix.GetLength(1) != 3)
                throw new ArgumentException("");

            Point3f[] objectPointsArray = EnumerableEx.ToArray(objectPoints);
            Point2f[] imagePointsArray = EnumerableEx.ToArray(imagePoints);
            double[] distCoeffsArray = EnumerableEx.ToArray(distCoeffs);
            int distCoeffsLength = (distCoeffs == null) ? 0 : distCoeffsArray.Length;
            rvec = new double[3];
            tvec = new double[3];

            using (var inliersVec = new VectorOfInt32())
            {
                NativeMethods.calib3d_solvePnPRansac_vector(
                    objectPointsArray, objectPointsArray.Length,
                    imagePointsArray, imagePointsArray.Length,
                    cameraMatrix, distCoeffsArray, distCoeffsLength,
                    rvec, tvec, useExtrinsicGuess ? 1 : 0, iterationsCount,
                    reprojectionError, minInliersCount, inliersVec.CvPtr, (int)flags);
                inliers = inliersVec.ToArray();
            }
        }
Пример #4
0
        /// <summary>
        /// computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible.
        /// </summary>
        /// <param name="objectPoints">Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, 
        /// where N is the number of points. List&lt;Point3f&gt; can be also passed here.</param>
        /// <param name="imagePoints">Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. 
        /// List&lt;Point2f&gt; can be also passed here.</param>
        /// <param name="cameraMatrix">Input 3x3 camera matrix</param>
        /// <param name="distCoeffs">Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. 
        /// If the vector is null, the zero distortion coefficients are assumed.</param>
        /// <param name="rvec">Output rotation vector that, together with tvec , brings points from the model coordinate system 
        /// to the camera coordinate system.</param>
        /// <param name="tvec">Output translation vector.</param>
        /// <param name="useExtrinsicGuess">If true, the function uses the provided rvec and tvec values as initial approximations 
        /// of the rotation and translation vectors, respectively, and further optimizes them.</param>
        /// <param name="iterationsCount">Number of iterations.</param>
        /// <param name="reprojectionError">Inlier threshold value used by the RANSAC procedure. 
        /// The parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier.</param>
        /// <param name="minInliersCount">Number of inliers. If the algorithm at some stage finds more inliers than minInliersCount , it finishes.</param>
        /// <param name="inliers">Output vector that contains indices of inliers in objectPoints and imagePoints .</param>
        /// <param name="flags">Method for solving a PnP problem</param>
        public static void SolvePnPRansac(
            InputArray objectPoints,
            InputArray imagePoints,
            InputArray cameraMatrix,
            InputArray distCoeffs,
            OutputArray rvec,
            OutputArray tvec,
            bool useExtrinsicGuess = false,
            int iterationsCount = 100,
            float reprojectionError = 8.0f,
            int minInliersCount = 100,
            OutputArray inliers = null,
            SolvePnPFlags flags = SolvePnPFlags.Iterative)
        {
            if (objectPoints == null)
                throw new ArgumentNullException("objectPoints");
            if (imagePoints == null)
                throw new ArgumentNullException("imagePoints");
            if (cameraMatrix == null)
                throw new ArgumentNullException("cameraMatrix");
            if (rvec == null)
                throw new ArgumentNullException("rvec");
            if (tvec == null)
                throw new ArgumentNullException("tvec");
            objectPoints.ThrowIfDisposed();
            imagePoints.ThrowIfDisposed();
            cameraMatrix.ThrowIfDisposed();
            distCoeffs.ThrowIfDisposed();
            rvec.ThrowIfDisposed();
            tvec.ThrowIfDisposed();
            IntPtr distCoeffsPtr = ToPtr(distCoeffs);

            NativeMethods.calib3d_solvePnPRansac_InputArray(
                objectPoints.CvPtr, imagePoints.CvPtr, cameraMatrix.CvPtr, distCoeffsPtr,
                rvec.CvPtr, tvec.CvPtr, useExtrinsicGuess ? 1 : 0, iterationsCount,
                reprojectionError, minInliersCount, ToPtr(inliers), (int)flags);

            rvec.Fix();
            tvec.Fix();
            if (inliers != null)
                inliers.Fix();
        }
Пример #5
0
        /// <summary>
        /// Finds an object pose from 3D-2D point correspondences.
        /// </summary>
        /// <param name="objectPoints"> Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, 
        /// where N is the number of points. vector&lt;Point3f&gt; can be also passed here.</param>
        /// <param name="imagePoints">Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, 
        /// where N is the number of points. vector&lt;Point2f&gt; can be also passed here.</param>
        /// <param name="cameraMatrix">Input camera matrix</param>
        /// <param name="distCoeffs">Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. 
        /// If the vector is null, the zero distortion coefficients are assumed.</param>
        /// <param name="rvec">Output rotation vector that, together with tvec , brings points from the model coordinate system to the 
        /// camera coordinate system.</param>
        /// <param name="tvec">Output translation vector.</param>
        /// <param name="useExtrinsicGuess">If true, the function uses the provided rvec and tvec values as initial approximations of 
        /// the rotation and translation vectors, respectively, and further optimizes them.</param>
        /// <param name="flags">Method for solving a PnP problem:</param>
        public static void SolvePnP(
            IEnumerable<Point3f> objectPoints,
            IEnumerable<Point2f> imagePoints,
            double[,] cameraMatrix,
            IEnumerable<double> distCoeffs,
            out double[] rvec, out double[] tvec,
            bool useExtrinsicGuess = false,
            SolvePnPFlags flags = SolvePnPFlags.Iterative)
        {
            if (objectPoints == null)
                throw new ArgumentNullException("objectPoints");
            if (imagePoints == null)
                throw new ArgumentNullException("imagePoints");
            if (cameraMatrix == null)
                throw new ArgumentNullException("cameraMatrix");
            if (cameraMatrix.GetLength(0) != 3 || cameraMatrix.GetLength(1) != 3)
                throw new ArgumentException("");

            Point3f[] objectPointsArray = EnumerableEx.ToArray(objectPoints);
            Point2f[] imagePointsArray = EnumerableEx.ToArray(imagePoints);
            double[] distCoeffsArray = EnumerableEx.ToArray(distCoeffs);
            int distCoeffsLength = (distCoeffs == null) ? 0 : distCoeffsArray.Length;
            rvec = new double[3];
            tvec = new double[3];

            NativeMethods.calib3d_solvePnP_vector(
                    objectPointsArray, objectPointsArray.Length,
                    imagePointsArray, imagePointsArray.Length,
                    cameraMatrix, distCoeffsArray, distCoeffsLength,
                    rvec, tvec, useExtrinsicGuess ? 1 : 0, (int)flags);
        }
Пример #6
0
 /// <summary>
 /// Finds an object pose from 3D-2D point correspondences.
 /// </summary>
 /// <param name="objectPoints"> Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, 
 /// where N is the number of points. vector&lt;Point3f&gt; can be also passed here.</param>
 /// <param name="imagePoints">Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, 
 /// where N is the number of points. vector&lt;Point2f&gt; can be also passed here.</param>
 /// <param name="cameraMatrix">Input camera matrix</param>
 /// <param name="distCoeffs">Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements. 
 /// If the vector is null, the zero distortion coefficients are assumed.</param>
 /// <param name="rvec">Output rotation vector that, together with tvec , brings points from the model coordinate system to the 
 /// camera coordinate system.</param>
 /// <param name="tvec">Output translation vector.</param>
 /// <param name="useExtrinsicGuess">If true, the function uses the provided rvec and tvec values as initial approximations of 
 /// the rotation and translation vectors, respectively, and further optimizes them.</param>
 /// <param name="flags">Method for solving a PnP problem:</param>
 public static void SolvePnP(
     InputArray objectPoints,
     InputArray imagePoints,
     InputArray cameraMatrix,
     InputArray distCoeffs,
     OutputArray rvec, OutputArray tvec,
     bool useExtrinsicGuess = false,
     SolvePnPFlags flags = SolvePnPFlags.Iterative)
 {
     if (objectPoints == null)
         throw new ArgumentNullException("objectPoints");
     if (imagePoints == null)
         throw new ArgumentNullException("imagePoints");
     if (cameraMatrix == null)
         throw new ArgumentNullException("cameraMatrix");
     if (rvec == null)
         throw new ArgumentNullException("rvec");
     if (tvec == null)
         throw new ArgumentNullException("tvec");
     objectPoints.ThrowIfDisposed();
     imagePoints.ThrowIfDisposed();
     cameraMatrix.ThrowIfDisposed();
     distCoeffs.ThrowIfDisposed();
     rvec.ThrowIfDisposed();
     tvec.ThrowIfDisposed();
     IntPtr distCoeffsPtr = ToPtr(distCoeffs);
     NativeMethods.calib3d_solvePnP_InputArray(
         objectPoints.CvPtr, imagePoints.CvPtr, cameraMatrix.CvPtr, distCoeffsPtr,
         rvec.CvPtr, tvec.CvPtr, useExtrinsicGuess ? 1 : 0, (int)flags);
     rvec.Fix();
     tvec.Fix();
 }