public Angles GetAnglesAndPoints(Mat <Point2d> points, int width, int height) { using (var objPtsMat = InputArray.Create(Model_points, MatType.CV_32FC3)) //new Mat(objPts.Count, 1, MatType.CV_32FC3, objPts)) using (var imgPtsMat = InputArray.Create(points, MatType.CV_32FC2)) //new Mat(imgPts.Length, 1, MatType.CV_32FC2, imgPts)) using (var cameraMatrixMat = Mat.Eye(3, 3, MatType.CV_64FC1)) 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); using (Mat resultPoints = new Mat()) { Cv2.ProjectPoints(objPtsMat, rvecMat, tvecMat, cameraMatrixMat, distMat, resultPoints); } // 根据旋转矩阵求解坐标旋转角 double theta_x = Math.Atan2((float)rvecMat.At <double>(2, 1), (float)rvecMat.At <double>(2, 2)); double theta_y = Math.Atan2((float)-rvecMat.At <double>(2, 0), Math.Sqrt((float)((rvecMat.At <double>(2, 1) * rvecMat.At <double>(2, 1)) + ((float)rvecMat.At <double>(2, 2) * rvecMat.At <double>(2, 2))))); double theta_z = Math.Atan2((float)rvecMat.At <double>(1, 0), (float)rvecMat.At <double>(0, 0)); // 将弧度转为角度 Angles angles = new Angles(); angles.Roll = theta_x * (180 / Math.PI); angles.Pitch = theta_y * (180 / Math.PI); angles.Yaw = theta_z * (180 / Math.PI); // 将映射的点的坐标保存下来 // outarray类型的resultpoints如何转存到list中? return(angles); } }
/// <summary> /// Find the Euler matrix from the output of SolvePnP. /// </summary> /// <param name="rotation">The rotation matrix returned by SolvePnp.</param> /// <returns>The Euler matrix containing pitch, roll, and yaw angles.</returns> public static Mat <double> GetEulerMatrix(Mat rotation) { // convert the 1x3 rotation vector to a full 3x3 matrix var r = new Mat <double>(3, 3); Cv2.Rodrigues(rotation, r); // set up some shortcuts to rotation matrix double m00 = r.At <double>(0, 0); double m01 = r.At <double>(0, 1); double m02 = r.At <double>(0, 2); double m10 = r.At <double>(1, 0); double m11 = r.At <double>(1, 1); double m12 = r.At <double>(1, 2); double m20 = r.At <double>(2, 0); double m21 = r.At <double>(2, 1); double m22 = r.At <double>(2, 2); // set up output variables Angles euler_out = new Angles(); Angles euler_out2 = new Angles(); if (Math.Abs(m20) >= 1) { euler_out.Yaw = 0; euler_out2.Yaw = 0; // From difference of angles formula if (m20 < 0) //gimbal locked down { double delta = Math.Atan2(m01, m02); euler_out.Pitch = Math.PI / 2f; euler_out2.Pitch = Math.PI / 2f; euler_out.Roll = delta; euler_out2.Roll = delta; } else // gimbal locked up { double delta = Math.Atan2(-m01, -m02); euler_out.Pitch = -Math.PI / 2f; euler_out2.Pitch = -Math.PI / 2f; euler_out.Roll = delta; euler_out2.Roll = delta; } } else { euler_out.Pitch = -Math.Asin(m20); euler_out2.Pitch = Math.PI - euler_out.Pitch; euler_out.Roll = Math.Atan2(m21 / Math.Cos(euler_out.Pitch), m22 / Math.Cos(euler_out.Pitch)); euler_out2.Roll = Math.Atan2(m21 / Math.Cos(euler_out2.Pitch), m22 / Math.Cos(euler_out2.Pitch)); euler_out.Yaw = Math.Atan2(m10 / Math.Cos(euler_out.Pitch), m00 / Math.Cos(euler_out.Pitch)); euler_out2.Yaw = Math.Atan2(m10 / Math.Cos(euler_out2.Pitch), m00 / Math.Cos(euler_out2.Pitch)); } // return result return(new Mat <double>(1, 3, new double[] { euler_out.Yaw, euler_out.Roll, euler_out.Pitch })); }
/// 旋转向量转化为欧拉角 public Angles GetEulerAngle(Mat <double> rotation_vector) { //var X = rotation_vector[0]; //var Y = rotation_vector[1]; //var Z = rotation_vector[2]; //var x = Math.Sin(Y / 2)*Math.Sin(Z / 2)*Math.Cos(X / 2) + Math.Cos(Y / 2)*Math.Cos(Z / 2)*Math.Sin(X / 2); //var y = Math.Sin(Y / 2)*Math.Cos(Z / 2)*Math.Cos(X / 2) + Math.Cos(Y / 2)*Math.Sin(Z / 2)*Math.Sin(X / 2); //var z = Math.Cos(Y / 2)*Math.Sin(Z / 2)*Math.Cos(X / 2) - Math.Sin(Y / 2)*Math.Cos(Z / 2)*Math.Sin(X / 2); //var w = Math.Cos(Y / 2)*Math.Cos(Z / 2)*Math.Cos(X / 2) - Math.Sin(Y / 2)*Math.Sin(Z / 2)*Math.Sin(X / 2); var rotArray = rotation_vector.ToArray(); Mat mat = new Mat(3, 1, MatType.CV_64FC1, rotArray); var theta = Cv2.Norm(mat, NormTypes.L2); var w = Math.Cos(theta / 2); var x = Math.Sin(theta / 2) * rotArray[0] / theta; var y = Math.Sin(theta / 2) * rotArray[1] / theta; var z = Math.Sin(theta / 2) * rotArray[2] / theta; var ysqr = y * y; // pitch (x-axis rotation) var t0 = 2.0 * (w * x + y * z); var t1 = 1.0 - 2.0 * (x * x + ysqr); var pitch = Math.Atan2(t0, t1);//反正切(给坐标轴,x,y) // yaw (y-axis rotation) var t2 = 2.0 * (w * y - z * x); if (t2 > 1.0) { t2 = 1.0; } if (t2 < -1.0) { t2 = -1.0; } var yaw = Math.Asin(t2); //反正弦函数 // roll (z-axis rotation) var t3 = 2.0 * (w * z + x * y); var t4 = 1.0 - 2.0 * (ysqr + z * z); var roll = Math.Atan2(t3, t4); // 单位转换:将弧度转换为度 var Y = (pitch / Math.PI) * 180; Y = Math.Sign(Y) * 180 - Y; var X = (yaw / Math.PI) * 180; var Z = (roll / Math.PI) * 180; Angles angles = new Angles() { Pitch = Y, Roll = Z, Yaw = X }; return(angles); }