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(); }
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)); // } } }
/// <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<Point3f> 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<Point2f> 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(); } }
/// <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<Point3f> 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<Point2f> 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(); }
/// <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<Point3f> 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<Point2f> 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); }
/// <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<Point3f> 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<Point2f> 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(); }