Kinect: Calculate head's pitch, yaw, roll angles - c#

I am trying to use Kinect sensor and SDK to calculate the orientation of user's head but I wasn't able to find any good help for this on google. Does anybody have any good sample, tutorial or something like that that might help me?

I think that I have found a solution although it's limited, it works only if the Kinect SDK can detect the face since I'm using the FaceTrackFrame object.
If somebody finds a solution to track more extreme angles when Kinect SDK is unable to detect a face I'd be more then happy to see it.
My solution looks something like this:
FaceTrackFrame faceFrame = faceTracker.Track(
kinectSensor.ColorStream.Format, colorPixelData,
kinectSensor.DepthStream.Format, depthPixelData, skeleton);
// Only works if face is detected
if (faceFrame.TrackSuccessful)
{
txtTracked.Content = "TRACKED";
txtRoll.Content = faceFrame.Rotation.Z;
txtPitch.Content = faceFrame.Rotation.X;
txtYaw.Content = faceFrame.Rotation.Y;
}

I managed to calculate these manually using the depth data from the image.
First you need to get the depth point
private EnumIndexableCollection<FeaturePoint, Vector3DF> depthPoints;
Then if you look at the FaceTracking viewer code that comes with the SDK and search for the DrawFaceModel function. You can extract the code like this in the 1st for loop.
faceModelPts3D.Add(new Point3D(this.depthPoints[i].X + 0.5f, this.depthPoints[i].Y + 0.5f, this.depthPoints[i].Z + 0.5f));
FaceDataPoints.DepthXPointInfo[i] = this.depthPoints[i].X;
FaceDataPoints.DepthYPointInfo[i] = this.depthPoints[i].Y;
FaceDataPoints.DepthZPointInfo[i] = this.depthPoints[i].Z;
I then placed point 0 and point 9 into the following function to obtain the Pitch. I then put the Points 120 and 116 in to obtain the yawn angle.
public static double FacePitch(double FirstXPos, double FirstYPos, double FirstZPos, double SecXPos, double SecYPos, double SecZPos)
{
double PitchAngle = 0;
double r = 0;
double XDifference, YDifference, ZDifference = 0;
double DifferenceSquared = 0;
XDifference = FirstXPos - SecXPos;//Calculates distance from Points
YDifference = FirstYPos - SecYPos;
ZDifference = FirstZPos - SecZPos;
DifferenceSquared = Math.Pow(XDifference, 2) + Math.Pow(YDifference, 2) + Math.Pow(ZDifference, 2);
r = Math.Sqrt(DifferenceSquared);
PitchAngle = (Math.Acos(ZDifference / r));
PitchAngle = ((PitchAngle * 180 / Math.PI) - 90) * -1; //Converts to Degrees as easier to recognise visually
return PitchAngle;
}
for the roll i placed point 0 and 9 in again and used the above function again But i changed
PitchAngle = (Math.Acos(ZDifference / r));
to
RollAngle = Math.Acos(XDifference / r);

Related

Making a Pen's LineCap go more into the drawn Line/Curve

Currently I have the following code:
AdjustableArrowCap arrow = new AdjustableArrowCap(10, 15, false);
penStateOutline.CustomEndCap = arrow;
And it draws this:
I have tried all day to make the arrow point to the ellipse itself rather than the center of it..
Update (I was wrong about the cap extending the line; it doesn't!)
To let the line and its cap end at the cirlce's outside you need to make the line shorter by the radius of the circle.
There are two approaches:
You can find a new endpoint that sits on the circle by calculating it, either with Pythagoras or by trigonometry. Then replace the endpoint, i.e. the circle's center, when drawing the line or curve by that new point.
Or put the other way round: You need to calculate a point on the circle as the new endpoint of the line.
It requires a little math, unless the line is horizontal or vertical...
This will work well for straight lines but for curves it may cause considerable changes in the shape, depending on how close the points get and how curved the shape is.
This may or may not be a problem.
To avoid it you can replace the curve points by a series of line points that are close enough to look like a curve when drawn. From the list of points we subtract all those that do not lie inside the circle.
This sounds more complicated than it is as there is a nice class called GraphicsPath that will allow you to add a curve and then flatten it. The result is a more or less large number of points. The same class also allows you to determine whether a point lies inside a shape, i.e. our case inside the circle.
To implement the latter approach, here is a routine that transforms a list of curve points to a list of line points that will end close to the circle..:
void FlattenCurveOutside(List<Point> points, float radius)//, int count)
{
using (GraphicsPath gp = new GraphicsPath())
using (GraphicsPath gpc = new GraphicsPath())
{
// firt create a path that looks like our circle:
PointF l = points.Last();
gpc.AddEllipse(l.X - radius, l.Y - radius, radius * 2, radius* 2);
// next one that holds the curve:
gp.AddCurve(points.ToArray());
// now we flatten it to a not too large number of line segments:
Matrix m = new Matrix(); // dummy matrix
gp.Flatten(m, 0.75f); // <== play with this param!!
// now we test the pathpoints from bach to front until we have left the circle:
int k = -1;
for (int i = gp.PathPoints.Length - 1; i >= 0; i--)
{
if ( !gpc.IsVisible(gp.PathPoints[i])) k = i;
if (k > 0) break;
}
// now we know how many pathpoints we want to retain:
points.Clear();
for (int i = 1; i <= k; i++)
points.Add(Point.Round(gp.PathPoints[i]));
}
}
Note that when the last part of the curve is too straight the result may look a little jagged..
Update 2
To implement the former approach here is a function that returns a PointF on a circle of radius r and a line connecting a Point b with the circle center c:
PointF Intersect(Point c, Point a, int rad)
{
float dx = c.X - a.X;
float dy = c.Y - a.Y;
var radians = Math.Atan2(dy, dx);
var angle = 180 - radians * (180 / Math.PI);
float alpha = (float)(angle * Math.PI / 180f);
float ry = (float)(Math.Sin(alpha) * rad );
float rx = (float)(Math.Cos(alpha) * rad );
return new PointF(c.X + rx, c.Y - ry);
}
The result thankfully looks rather similar:
The math can be simplified a little..
To apply it you can get the new endpoint and replace the old one:
PointF I = Intersect(c, b, r);
points2[points2.Count - 1] = Point.Round(I);
Thank you so much to #TaW for helping.
Unfortunately the answer he provided did not match my stupid OCD..
You made me think about the problem from a different perspective and I now have found a solution that I'll share if anyone else needs, note that the solution is not perfect.
public static Point ClosestPointOnCircle(int rad, PointF circle, PointF other)
{
if (other.X == circle.X && other.Y == circle.Y) // dealing with division by 0
other.Y--;
return new Point((int)Math.Floor(circle.X + rad * ((other.X - circle.X) / (Math.Sqrt(Math.Pow(other.X - circle.X, 2) + Math.Pow(other.Y - circle.Y, 2))))), (int)Math.Floor(circle.Y + rad * ((other.Y - circle.Y) / (Math.Sqrt(Math.Pow(other.X - circle.X, 2) + Math.Pow(other.Y - circle.Y, 2))))));
}
What the code does is return a point on the circle that is the closest to the another point, then, I used the curve middle point as the other point to change the end point of the curve.
Then I used the arrow cap as normal and got this:image
Which is good enough for my project.

GPS lap & Segment timer

I've been searching for a while but haven't found exactly what I'm looking for.
I'm working on an app that will go in a race car. It will give the driver the ability to press a button to mark a Start/Finish line. It will also have a button to allow a driver to set segment times.
Keep in mind a track can be an oval which I'm working on first. It could be a road course or it could be an auto cross where the start and finish line aren't the exact same location. They could be with 50 feet of each other or so but the car never crosses where it starts.
I have my gps data coming in and I convert the NMea messages to my classes and I store Lat, Lon, Speed, Course etc. In my research I've ran across this which is interesting. The GPS will be mounted outside the roof for better signal. It generates 10 hits per second. (Garmin Glo)
http://www.drdobbs.com/windows/gps-programming-net/184405690?pgno=1
It's old but it talks about UTM and the Cartesian coordinate system. So using the DecDeg2UTM, I convert Lat & Lon to X & coordinates as well.
I've also been trying to use the Intersect formula I found Here I took the intersect and tried to convert it to C# which I'll post at the end. However, feeding coordinates of an oval track, it doesn't seem to be working. Also, I'm not sure exactly what it's supposed to be doing. But the coordinates it returns when it does somethign like -35.xxx & 98.xxxx which out in an ocean somewhere 1000's of miles from where the track is.
I looking for answers to the following.
I assume I need to take the location recorded when a button is pressed for Start/Finish or Segment and calculate a line perpendicular to the direction the car in able to be able to do some sort of Line Intersection calculation. The Cartesian coordinates seems to calculate the bearing fairly well. But the question here is how do you get the "left and right coordinates". Also, keep in mind, an oval track may be 60 feet wide. But as mentioned an auto cross track may only be 20 ft wide and part of the track may be with 50 ft. Note I'm fine with indicating to set the points, the car needs to be going slow or stopped at the points to get an accurate coordinate. Some tracks they will have to be set while walking the track.
Based on this, should I be trying to use decimal lat lon or would utilizing the Cartesian coordinate system based on UTM be a more accurate method for what I'm trying to do?
Either one is there a .Net library or C based library with source code that has methods for making these calculations?
How can this be accurately handled. (Not that great with Math, links to code samples would help tremendously.)
Next, after I have the lines or whatever is needed for start/finish and segments, as I get GPS sign from the car racing, I need to figure out the most accurate way to tell when a car has crossed those segments. again if I'm lucky I'll get 10 hits per second but it will probably be lower. Then the vehicle speeds could vary significantly depending on the type of track and vehicle. So the GPS hit could be many feet "left or right" of a segment. Also, it could be many feet before or after a segment.
Again, if there is a GIS library out there I can feed coordinates and all this is calculated, that's would work as well as long as it's performant. If not again I'm trying to decide if it's best to break down coordinates to X Y or some geometry formulas for coordinates in decimal format. Mods, I assume there is hard data to support an answer of either way and this isn't responses aren't fully subjective to opinions.
Here is the C# code I came up with from the Script page above. I'm starting to feel UTM and the Cartesian Coordinate system would be better for accuracy and performance. But again I'm open to evidence to the contrary if it exists.
Thanks
P.S. Note GeoCoordinate is from the .Net System.Device.Location assemble. GpsData is just a class I use to convert NMEA messages into Lat, Lon, Course, NumSats, DateTime etc.
The degree Radian methods are extensions as as follows.
public static double DegreeToRadians(this double angle)
{
return Math.PI * angle / 180.0;
}
public static double RadianToDegree(this double angle)
{
return angle * (180.0 / Math.PI);
}
}
public static GeoCoordinate CalculateIntersection(GpsData p1, double brng1, GpsData p2, double brng2)
{
// see http://williams.best.vwh.net/avform.htm#Intersection
// Not sure I need to use Cosine
double _p1LatRadians = p1.Latitude.DegreeToRadians();
double _p1LonToRadians = p1.Longitude.DegreeToRadians();
double _p2LatToRadians = p2.Latitude.DegreeToRadians();
double _p2LonToRadians = p2.Longitude.DegreeToRadians();
double _brng1ToRadians = brng1.DegreeToRadians();
double _brng2ToRadians = brng2.DegreeToRadians();
double _deltaLat = _p2LatToRadians - _p1LatRadians;
double _deltaLon = _p2LonToRadians - _p1LonToRadians;
var _var1 = 2 * Math.Asin(Math.Sqrt(Math.Sin(_deltaLat / 2) * Math.Sin(_deltaLat / 2)
+ Math.Cos(_p1LatRadians) * Math.Cos(_p2LatToRadians) * Math.Sin(_deltaLon / 2) * Math.Sin(_deltaLon / 2)));
if (_var1 == 0) return null;
// initial/final bearings between points
var _finalBrng = Math.Acos((Math.Sin(_p2LatToRadians) - Math.Sin(_p1LatRadians) * Math.Cos(_var1)) / (Math.Sin(_var1) * Math.Cos(_p1LatRadians)));
//if (isNaN(θa)) θa = 0; // protect against rounding
var θb = Math.Acos((Math.Sin(_p1LatRadians) - Math.Sin(_p2LatToRadians) * Math.Cos(_var1)) / (Math.Sin(_var1) * Math.Cos(_p2LatToRadians)));
var θ12 = Math.Sin(_p2LonToRadians - _p1LonToRadians) > 0 ? _finalBrng : 2 * Math.PI - _finalBrng;
var θ21 = Math.Sin(_p2LonToRadians - _p1LonToRadians) > 0 ? 2 * Math.PI - θb : θb;
var α1 = (_brng1ToRadians - θ12 + Math.PI) % (2 * Math.PI) - Math.PI; // angle 2-1-3
var α2 = (θ21 - _brng2ToRadians + Math.PI) % (2 * Math.PI) - Math.PI; // angle 1-2-3
if (Math.Sin(α1) == 0 && Math.Sin(α2) == 0) return null; // infinite intersections
if (Math.Sin(α1) * Math.Sin(α2) < 0) return null; // ambiguous intersection
α1 = Math.Abs(α1);
α2 = Math.Abs(α2);
// ... Ed Williams takes abs of α1/α2, but seems to break calculation?
var α3 = Math.Acos(-Math.Cos(α1) * Math.Cos(α2) + Math.Sin(α1) * Math.Sin(α2) * Math.Cos(_var1));
var δ13 = Math.Atan2(Math.Sin(_var1) * Math.Sin(α1) * Math.Sin(α2), Math.Cos(α2) + Math.Cos(α1) * Math.Cos(α3));
var _finalLatRadians = Math.Asin(Math.Sin(_p1LatRadians) * Math.Cos(δ13) + Math.Cos(_p1LatRadians) * Math.Sin(δ13) * Math.Cos(_brng1ToRadians));
var _lonBearing = Math.Atan2(Math.Sin(_brng1ToRadians) * Math.Sin(δ13) * Math.Cos(_p1LatRadians), Math.Cos(δ13) - Math.Sin(_p1LatRadians) * Math.Sin(_finalLatRadians));
var _finalLon = _p1LonToRadians + _lonBearing;
var _returnLat = _finalLatRadians.RadianToDegree();
var _latToDegree = _finalLon.RadianToDegree();
var _returnLon = ( _latToDegree + 540) % 360 - 180;
return new GeoCoordinate(_returnLat, _returnLon);
//return new LatLon(φ3.toDegrees(), (λ3.toDegrees() + 540) % 360 - 180); // normalise to −180..+180°
}

Calculate angle required to hit coordinate (x,y) from position other than (0,0) with varying elevation

I am attempting to calculate the angle required to fire a projectile in order to hit a specific coordinate.
My projectile is located a random coordinate and my target coordinate at a static coordinate.
I ended up running across the following equation on Wikipedia for calculating the angle required to hit a coordinate at (x,y) from (0,0):
I have made some attempts to understand this and other formula and attempted the following implementation (I am using c# and XNA).
double y = source.Y - target.Y;
double x = Vector2.Distance(source, target);
double v = 1440; //velocity
double g = 25; //gravity
double sqrt = (v*v*v*v) - (g*(g*(x*x) + 2*y*(v*v)));
sqrt = Math.Sqrt(sqrt);
double angleInRadians = Math.Atan(((v*v) + sqrt)/(g*x));
I have also attempted the following, which resulted in an identical angle where the values of v and g remain the same.
double targetX = target.X - source.X;
double targetY = -(target.Y - source.Y);
double r1 = Math.Sqrt((v*v*v*v) - g*(g*(target.X*target.X) + ((2*target.Y)*(v*v))));
double a1 = ((v*v) + r1)/(g*target.X);
angleInRadians = -Math.Atan(a1);
if (targetX < 0)
{
angleInRadians -= 180/180*Math.PI;
}
My conjecture is that even in my (assumed) attempt to zero out the source coordinate, that I am still not performing the calculation correctly for coordinates with a non (0,0) source and different elevations.
Below is an image that depicts my coordinate system. It is the default for XNA.
Thanks to the help in the comments the solution to find this angle ended up requiring that the positions be translated to a (0,0) based system. For anyone looking for the same scenario the final working solution was:
double x = -(source.x - target.x);
double y = (source.y - target.y);
double v = 1440; //m/s
double g = 25; //m/s
double sqrt = (v*v*v*v) - (g*(g*(x*x) + 2*y*(v*v)));
sqrt = Math.Sqrt(sqrt);
angleInRadians = Math.Atan(((v*v) + sqrt)/(g*x));
Then to convert the radians into a vector that works with XNA, perform the following conversion:
Vector2 angleVector = new Vector2(-(float)Math.Cos(angleInRadians), (float)Math.Sin(angleInRadians));
I think the real problem lies in the use of arctan. Because the range is limited to -pi/2..pi/2 results are only in the right half plane.
Use arctan2 to get the proper coordinates:
angleInRadians = Math.Atan2(((v*v) + tmp), (g*x));

How to find the distance between a point and a parabola in code

I am trying to find the closest point on a parabola to an arbitrary point in 2d, for a DirectX pixel shader.
A great amount of googling has revealed to me that this is a common pre-calculus homework problem. Unfortunately, the hundreds of relevant answers all say things like "Once you have this equation, use your graphing calculator's minimum function and it will tell you the answer is 6."
I confess that I recall nothing of pre-calculus. I recognize that the equation I seek is probably sitting right there on wikipedia, but I can't figure out how to convert these greek symbols into an HLSL function. A solution in C, C++, C#, or any other language would also be greatly appreciated.
edit: Per a request to see the format of the input curve:
//Equation of parabola being y = ax^2 + bx + c
//p is the arbitrary point we're trying to find the closest point on the parabola for.
float2 GetClosestPointOnParabola(float a, float b, float c, float2 p)
{
//Something involving the distance formula...
//Something involving "minimization"...
return float2(x, y);
}
You could make use of this:
Pmin = (xmin, ymin) ~ point on a parabola
P = (px, py) ~ point in 2d
y = a*x^2 + bx + c ~ parabola
P(x) = (x-px)^2 + (y-py)^2 = (x-px)^2 + (a*x^2 + bx + c - py)^2
You need to calculate the P(x) derivative, it's not that difficult. E.g.
If you get: P(x) = x^4 + 4x^2 - 3x + 10 the derivative would be:
P'(x) = 4x^3 + 8x - 3
I think you get how to calculate that. Then compare P'(x) to zero to find where it crossess an X-axis. You find an xmin from that and then you have ymin from:
y = a*x^2 + bx + c
That's it.
I assume what you want is the point on a parabola that is closest to another point in the plane. Let's assume the parabola is given by y = a * x^2 + b * x + c and that you want to find the point on it closest to the point A(xa, ya).
I would propose you use hill climbing. It finds a local minimum in a function with a logarithmic complexity. I will write example c++ code assuming there is a function h(x) that calculates the distance from A to the point with the point with x coordinate equal to x on the parabola.
double minDist() {
const double epsylon = 1e-9; // used to avoid double prescision errors
double current = 0.0;
double step = 1e+6;
while (step > 1e-5) { // change this with the accuracy you need
double left_neighbour = current - step;
double right_neighbour = current + step;
double cval = h(current);
double lval = h(left_neighbour);
double rval = h(right_neighbour);
if (cval < rval + epsylon && cval < lval + epsylon) {
step *= 0.5;
continue;
}
if (lval < rval) {
current = left_neighbour;
} else {
current = right_neighbour;
}
}
return current;
}
In most case you will have a single local mimimum that is the answer you need, but maybe there are cases where you have two(I believe they can not be more then 2). In these cases you need to start the function twice with different initial points.
Hope this helps.

3D Math / 2D Rotation Calculation: Split/cut 3D model?

I'm trying to rotate a 3D object on its Z axis (up/down).
public void RotateY(float angle)
{
foreach (CoordinateVertices cv in this.GetAll<CoordinateVertices>())
{
for (int i = 0; i < cv.Coordinates.Length; i++)
{
Vector3 old = cv.Coordinates[i];
float theta = Math.Atan2(old.Y, old.X) + angle;
float rayon = Math.Sqrt(Math.Pow(old.X, 2) + Math.Pow(old.Y, 2));
cv.Coordinates[i] = new Vector3(Math.Cos(theta) * rayon, Math.Sin(theta) * rayon, old.Z);
}
}
}
The trigonometry is fairly simple, and it seems to work fine, but for some reason, my 3D object gets cut in half.
Does anybody have an idea of what's going on? I would have posted this on the maths StackExchange, but it might be a problem with my programming too, and the trigonometry is really simple.
Edit: The following is an alternative for the doing the same as the above. It took me a few minutes to realize the following solution is identical to the code initially posted.
It should look like this:
double Xnew = X * cos(theta) + Y * sin(theta);
double Ynew = Y * cos(theta) - X * sin(theta);
Or in your code:
public void RotateY(float angle)
{
foreach (CoordinateVertices cv in this.GetAll<CoordinateVertices>())
{
for (int i = 0; i < cv.Coordinates.Length; i++)
{
Vector3 old = cv.Coordinates[i];
float xnew = old.X * Math.Cos(angle) + old.Y * Math.Sin(angle);
float ynew = old.Y * Math.Cos(angle) - old.X * Math.Sin(angle);
cv.Coordinates[i] = new Vector3(xnew, ynew, old.Z);
}
}
}
The above code assumes you're rotating about the origin. If you're not rotating about the origin, you just need to translate to the origin, rotate, then translate back.
See here for more details: http://en.wikipedia.org/wiki/Transformation_matrix#Rotation
As has been noted, nothing wrong with your code. However, you may also be interested in using the Transform function (which can operate on your entire array of coordinates at once). Vector3.Transform (Vector3[], Matrix). You can get your rotation with a rotation matrix calculated for a given angle, theta, about any axis. I would expect this to be significantly faster for large numbers of points. (Less trig calculations, and possibly hardware acceleration)
Actually, the bug disappeared, out of nowhere. I went on to test some more values, and they worked. I went back to the same value as before, and it worked. This is ridiculous, it always happens to me.
What's the name for that? Bugs that disappear by themselves.

Categories

Resources