/// <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 MatOfDouble GetEulerMatrix(Mat rotation)
{
// convert the 1x3 rotation vector to a full 3x3 matrix
var r = new MatOfDouble(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
Euler euler_out = new Euler();
Euler euler_out2 = new Euler();
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 MatOfDouble(1, 3, new double[] { euler_out.yaw, euler_out.roll, euler_out.pitch }));
}
/// <summary>
/// Check if the driver is facing forward.
/// </summary>
/// <param name="headRotation">The head rotation angles.</param>
/// <returns>True if the driver is facing forward, false if not.</returns>
private bool IsDriverFacingForward(MatOfDouble headRotation)
{
// calculate head rotation in degrees
var leftRight = 180 * headRotation.At <double>(0, 2) / Math.PI;
var upDown = 180 * headRotation.At <double>(0, 1) / Math.PI;
var rotation = 180 * headRotation.At <double>(0, 0) / Math.PI;
// looking straight ahead wraps at -180/180, so make the range smooth
upDown = Math.Sign(upDown) * 180 - upDown;
// calculate if the driver is facing forward
// the left/right angle must be in the -25..25 range
// the up/down angle must be in the -10..10 range
return
(leftRight >= -25 && leftRight <= 25 &&
upDown >= -10 && upDown <= 10);
}
/// <summary>
/// Timer event to update the user interface.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void timer_Tick(object sender, EventArgs e)
{
if (headRotation == null)
{
return;
}
// calculate head pose in degrees
var leftRight = 180 * headRotation.At <double>(0, 2) / Math.PI;
var upDown = 180 * headRotation.At <double>(0, 1) / Math.PI;
var rotation = 180 * headRotation.At <double>(0, 0) / Math.PI;
// looking straight ahead wraps at -180/180, so make the range smooth
upDown = Math.Sign(upDown) * 180 - upDown;
// update sliders
if (leftRight >= -45 && leftRight <= 45)
{
leftRightAngle.Value = (int)leftRight;
}
if (upDown >= -45 && upDown <= 45)
{
upDownAngle.Value = (int)upDown;
}
if (rotation >= -45 && rotation <= 45)
{
rotationAngle.Value = (int)rotation;
}
// pause the video player if we're not looking at it
// the left/right angle must be in the -25..25 range
// the up/down angle must be in the -10..10 range
bool facingForward = (
leftRight >= -25 && leftRight <= 25 &&
upDown >= -10 && upDown <= 10);
// start and stop the video player and show the paused label
pausedLabel.Visible = !facingForward;
/*
* if (facingForward && !videoPlayer.IsRunning)
* videoPlayer.Start();
* else if (!facingForward && videoPlayer.IsRunning)
* videoPlayer.Stop();
*/
}