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);
}
