velocity of a joint between two frames using kinect sdk - kinect

I'm stack for along time in this problem and i will really appreciate if any one could help me in that.
I asked many times in many forums, i've searched alot but no answer that really helped me.
i'm developping an application where i have to calculate the velocity of a joint of skeleton body using vs c# 2012 and kinect sdk 1.7
i have first to be sure of the logic of things before asking this question so,
if I understood correctly, the delta_time i'm looking for to calculate velocity, is not the duration of one frame (1/30s) but it must be calculated from two instants:
1- the instant when detecting and saving the "joint point" in the first frame
2- the instant when detecting and saving the same "joint point" in the next frame
if it's not true, thank you for clarifying things.
starting from this hypothesis, i wrote a code to :
detectiong a person
tracking the spine joint ==> if it's is tracked then saving its coordinates into a list (I reduced the work for the moment on the Y axis to simplify)
pick up the time when saving the coordinates
increment the framecounter (initially equal to zero)
if the frame counter is > 1 calculate velocity ( x2 - x1)/(T2 - T1) and save it
here is a piece of the code:
System.Diagnostics.Stopwatch stopWatch = new System.Diagnostics.Stopwatch();
double msNow;
double msPast;
double diff;
TimeSpan currentTime;
TimeSpan lastTime = new TimeSpan(0);
List<double> Sylist = new List<double>();
private int framecounter = 0;
private void KinectSensorOnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
{
Skeleton first = GetFirstSkeleton(allFramesReadyEventArgs);
if (first == null) // if there is no skeleton
{
txtP.Text = "No person detected"; // (Idle mode)
return;
}
else
{
txtP.Text = "A person is detected";
skeletonDetected = true;
/// look if the person is totally detected
find_coordinates(first);
/*******************************
* time computing *
/*******************************/
currentTime = stopWatch.Elapsed;
msNow = currentTime.Seconds * 1000 + currentTime.Milliseconds;
if (lastTime.Ticks != 0)
{
msPast = lastTime.Seconds * 1000 + lastTime.Milliseconds;
diff = msNow - msPast;
}
lastTime = currentTime;
}
//framecounter++;
}
void find_coordinates(Skeleton first)
{
//*modification 07052014 *****/
Joint Spine = first.Joints[JointType.Spine];
if (Spine.TrackingState == JointTrackingState.Tracked)
{
double Sy = Spine.Position.Y;
/*******************************
* time starting *
/*******************************/
stopWatch.Start();
Sylist.Add(Sy);
framecounter++;
}
else
return;
if (framecounter > 1)
{
double delta_Distance = Sylist[Sylist.Count] - Sylist[Sylist.Count - 1];
}
}
to be honnest, i dont really know how ti use timespan and stopwatch in this context ( i mean when there are frames to process many times/s)
i will be thankfull for any help !

First:
The SkeletonFrame has a property called Timespamp that you can use. It's better to use that one than to create your own timesystem because the timestamp is directly generated by the Kinect.
Second:
Keep track of the previous Timestamp and location.
Then it's just a matter of calculation.
(CurrentLocation - PreviousLocation) = Distance difference
(CurrentTimestamp - PreviousTimestamp) = Time taken to travel the distance.
For example you would get 0.1 meter per 33 miliseconds.
So you can get the meters per seconds like this = (1 second / time taken to travel) * distance difference. In the example this is = (1000/33)*0.1 = 3.03 meter per second.

Related

Unwanted click when using SoXR Library to do variable rate resampling

I am using the SoXR library's variable rate feature to dynamically change the sampling rate of an audio stream in real time. Unfortunately I have have noticed that an unwanted clicking noise is present when changing the rate from 1.0 to a larger value (ex: 1.01) when testing with a sine wave. I have not noticed any unwanted artifacts when changing from a value larger than 1.0 to 1.0. I looked at the wave form it was producing and it appeared as if a few samples right at rate change are transposed incorrectly.
Here's a picture of an example of a stereo 440Hz sinewave stored using signed 16bit interleaved samples:
I also was unable to find any documentation covering the variable rate feature beyond the fifth code example. Here's is my initialization code:
bool DynamicRateAudioFrameQueue::intialize(uint32_t sampleRate, uint32_t numChannels)
{
mSampleRate = sampleRate;
mNumChannels = numChannels;
mRate = 1.0;
mGlideTimeInMs = 0;
// Intialize buffer
size_t intialBufferSize = 100 * sampleRate * numChannels / 1000; // 100 ms
pFifoSampleBuffer = new FiFoBuffer<int16_t>(intialBufferSize);
soxr_error_t error;
// Use signed int16 with interleaved channels
soxr_io_spec_t ioSpec = soxr_io_spec(SOXR_INT16_I, SOXR_INT16_I);
// "When creating a var-rate resampler, q_spec must be set as follows:" - example code
// Using SOXR_VR makes sense, but I'm not sure if the quality can be altered when using var-rate
soxr_quality_spec_t qualitySpec = soxr_quality_spec(SOXR_HQ, SOXR_VR);
// Using the var-rate io-spec is undocumented beyond a single code example which states
// "The ratio of the given input rate and ouput rates must equate to the
// maximum I/O ratio that will be used: "
// My tests show this is not true
double inRate = 1.0;
double outRate = 1.0;
mSoxrHandle = soxr_create(inRate, outRate, mNumChannels, &error, &ioSpec, &qualitySpec, NULL);
if (error == 0) // soxr_error_t == 0; no error
{
mIntialized = true;
return true;
}
else
{
return false;
}
}
Any idea what may be causing this to happen? Or have a suggestion for an alternative library that is capable of variable rate audio resampling in real time?
After speaking with the developer of the SoXR library I was able to resolve this issue by adjusting the maximum ratio parameters in the soxr_create method call. The developer's response can be found here.

How to overlap a buffer in Objective-C

I'm trying to do FFT on the iPhone, and I realised that I had not overlapped my input prior to windowing. I was wondering if anyone could give me some insight on to how to properly overlap my input buffer.
I am wanting to overlap bufferSamples by a factor of 4, and I understand that it is to be done using memove functions, but I can't figure out how to get it to work in regard to overlapping.
enum
{
frameSize = 2048,
overlap = 4,
range = 8192,
step = frameSize/overlap,
};
static COMPLEX_SPLIT A;
// For each sample in buffer...
for (int j = 0; j < audioBufferList.mNumberBuffers; j++)
{
// Declaring samples from audio buffer list
SInt16 *bufferSamples = (SInt16*)audioBufferList.mBuffers[j].mData;
// Overlapping here?
////////////////////////
//// vDSP FUNCTIONS ////
////////////////////////
// Creating Hann window function
for (int i = 0; i < frameSize; i++)
{
double window = 0.5 * (1.0 - cos((2.0 * M_PI * i) / (frameSize - 1)));
// Applying window to each sample
A.realp[i] = window * bufferSamples[i];
A.imagp[i] = 0;
}
// Further DSP...
To get an overlap factor of 4, you need to save the last 75% of the data that, before windowing, was input to the previous FFT. Then use that saved data as the first 75% of the current FFT, with only the last 25% from current or not yet used data. memmove can be used to copy data to and from the temporary save data buffers. Repeat as necessary to use up the data available.

Calculating Center of mass of body being tracked using kinect?

I am working on Kinect for my research project . I have worked previously to calculate the joint angle of kinect and the joint coordinates. I would like to calculate the center of mass of the body which is being tracked.
Any idea would be appreciated and code snippets would be immensely helpful.
I owe a lot to stack overflow without the community help it would had not been possible to do such a thing.
Thanks in Advance
Please find the code where i want to include this center of mass function. This function tracks the skeleton.
Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
{
using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
{
if (skeletonFrameData == null)
{
return null;
}
skeletonFrameData.CopySkeletonDataTo(allSkeletons);
//get the first tracked skeleton
Skeleton first = (from s in allSkeletons
where s.TrackingState == SkeletonTrackingState.Tracked
select s).FirstOrDefault();
return first;
}
I have tried using this code in my code but its not getting accustomed , can any one please help me include the center of mass code.
oreach (SkeletonData data in skeletonFrame.Skeletons) {
SkeletonFrame allskeleton = e.SkeletonFrame;
// Count passive and active person up to six in the group
int numberOfSkeletonsT = (from s in allskeleton.Skeletons
where s.TrackingState == SkeletonTrackingState.Tracked select s).Count();
int numberOfSkeletonsP = (from s in allskeleton.Skeletons
where s.TrackingState == SkeletonTrackingState.PositionOnly select s).Count();
// Count passive and active person up to six in the group
int totalSkeletons = numberOfSkeletonsP + numberOfSkeletonsT;
//Console.WriteLine("TotalSkeletons = " + totalSkeletons);
//======================================================
if (data.TrackingState == SkeletonTrackingState.PositionOnly)
{
foreach (Joint joint in data.Joints)
{
if (joint.Position.Z != 0)
{
double centerofmassX = com.Position.X;
double centerofmassY = com.Position.Y;
double centerofmassZ = com.Position.Z;
Console.WriteLine( centerofmassX + centerofmassY + centerofmassZ );
}
}
See a couple of resources here:
http://mathwiki.ucdavis.edu/Calculus/Vector_Calculus/Multiple_Integrals/Moments_and_Centers_of_Mass#Three-Dimensional_Solids
http://www.slideshare.net/GillianWinters/center-of-mass-presentation
http://en.wikipedia.org/wiki/Locating_the_center_of_mass
Basically no matter what, you are going to need to find the mass of your user. This can be a simple input, then you can determine how much weight the person puts on each foot and use the equations described at all of these sources. Another option may be to use plumb lines on a planar shape representation of the user in 2D, However that won't be the actually accurate 3D center of mass.
Here is an example of how to find what amount of mass is on each foot. using the equation found on http://www.vitutor.com/geometry/distance/line_plane.html
Vector3 v = new Vector3(skeleton.Joints[JointType.Head].Position.X, skeleton.Joints[JointType.Head].Position.Y, skeleton.Joints[JointType.Head].Position.Z);
double mass;
double leftM, rightM;
double A = sFrame.FloorClipPlane.X,
B = sFrame.FloorClipPlane.Y,
C = sFrame.FloorClipPlane.Z;
//find angle
double angle = Math.ASin(Math.Abs(A * v.X + B * v.Y * C * v.Z)/(Math.Sqrt(A * A + B * B + C * C) * Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z)));
if (angle == 90.0)
{
leftM = mass / 2.0;
rightM = mass / 2.0;
}
double distanceFrom90 = 90.0 - angle;
if (distanceFrom90 > 0)
{
double leftMultiple = distanceFrom90 / 90.0;
leftM = mass * leftMultiple;
rightM = mass - leftM;
}
else
{
double rightMultiple = distanceFrom90 / 90.0;
rightM = rightMultiple * mass;
leftM = mass - rightMultiple;
}
This is of course assuming that the user is on both feet, but you could modify the code to create a new plane based off the users feet instead of the automatic one generated by Kinect.
The code to then find the center of mass you have to choose a datum. I would choose the head as that is the top of the person, and you can measure down from it easily. Using the steps found here:
double distanceFromDatumLeft = Math.Sqrt(Math.Pow(headX - footLeftX, 2) + Math.Pow(headY - footLeftY, 2) + Math.Pow(headZ - footLeftZ, 2));
double distanceFromDatumLeft = Math.Sqrt(Math.Pow(headX - footRightX, 2) + Math.Pow(headY - footRightY, 2) + Math.Pow(headZ - footRightZ, 2));
double momentLeft = distanceFromDatumLeft * leftM;
double momentRight = distanceFromDatumRight * rightM;
double momentSum = momentLeft + momentRight;
//measured in units from the datum
double centerOfGravity = momentSum / mass;
You then can of course show this on the screen by passing a point to plot that is centerOfGravity points below the head.

A* Pathfinding - how to modify G and H to include rough terrain movement cost?

I have A* pathfinding implemented in my 2D game and it works well on a plain map with obstacles. Now I'm trying to understand how to modify the algorithm, so it counts rough terrain (hills, forest, etc) as 2 moves instead of 1.
With the 1 movement cost, the algorithm uses integers 10 and 14 in the move cost function. Im interested in how to modify these values if one cell actually has a movement cost of 2? will it be 20:17?
Here's how my current algorithm currently computes G and H (adopted from Ray Wenderleich):
// Compute the H score from a position to another (from the current position to the final desired position
- (int)computeHScoreFromCoord:(CGPoint)fromCoord toCoord:(CGPoint)toCoord
{
// Here we use the Manhattan method, which calculates the total number of step moved horizontally and vertically to reach the
// final desired step from the current step, ignoring any obstacles that may be in the way
return abs(toCoord.x - fromCoord.x) + abs(toCoord.y - fromCoord.y);
}
// Compute the cost of moving from a step to an adjecent one
- (int)costToMoveFromStep:(ShortestPathStep *)fromStep toAdjacentStep:(ShortestPathStep *)toStep
{
return ((fromStep.position.x != toStep.position.x)
&& (fromStep.position.y != toStep.position.y))
? 14 : 10;
}
If some of the edges have movement cost 2, you will simply add 2 to the G of the parent node, rather than 1.
As for H: it doesn't need to change. The resulting heuristic will still be admissible/consistent.
I think I got it, with this line the tutorial author checks if the move is 1 square or 2 squares(diagonal) from the move that is currently being considered.
return ((fromStep.position.x != toStep.position.x)
&& (fromStep.position.y != toStep.position.y))
? 14 : 10;
Unfortunately, this is a really simple case and does not really explain what has to be done. Number 10 is used to make calculations easier (10 = 1 move cost), and (14 = 1 diagonal move) is an approximation of sqrt(10*10).
I attempted to introduce terrain cost below, and this requires extra information - I need to know which cell I'm going through to reach the destination. This turned out to be really annoying, and the code below is clearly not my best, but I attempted to spell out what's going on at each step.
If I'm making a diagonal move, I need to know it's move cost AND the move cost of 2 squares that can be used to get there. I can then pick the lowest movement cost among two squares and plug it into the equation of the form:
moveCost = (int)sqrt(lowestMoveCost*lowestMoveCost + (stepNode.moveCost*10) * (stepNode.moveCost*10));
Here's the entire loop that checks adjacent steps and creates new steps out of them with the move cost. It finds tile in my map array and returns it's terrain cost.
NSArray *adjSteps = [self walkableAdjacentTilesCoordForTileCoord:currentStep.position];
for (NSValue *v in adjSteps) {
ShortestPathStep *step = [[ShortestPathStep alloc] initWithPosition:[v CGPointValue]];
// Check if the step isn't already in the closed set
if ([self.spClosedSteps containsObject:step]) {
continue; // Ignore it
}
tileIndex = [MapOfTiles tileIndexForCoordinate:step.position];
DLog(#"point (x%.0f y%.0f):%i",step.position.x,step.position.y,tileIndex);
stepNode = [[MapOfTiles sharedInstance] mapTiles] [tileIndex];
// int moveCost = [self costToMoveFromStep:currentStep toAdjacentStep:step];
//in my case 0,0 is bottom left, y points up x points right
if((currentStep.position.x != step.position.x) && (currentStep.position.y != step.position.y))
{
//move one step away - easy, multiply move cost by 10
moveCost = stepNode.moveCost*10;
}else
{
possibleMove1 = 0;
possibleMove2 = 0;
//we are moving diagonally, figure out in which direction
if(step.position.y > currentStep.position.y)
{
//moving up
possibleMove1 = tileIndex + 1;
if(step.position.x > currentStep.position.x)
{
//moving right and up
possibleMove2 = tileIndex + tileCountTall;
}else
{
//moving left and up
possibleMove2 = tileIndex - tileCountTall;
}
}else
{
//moving down
possibleMove1 = tileIndex - 1;
if(step.position.x > currentStep.position.x)
{
//moving right and down
possibleMove2 = tileIndex + tileCountTall;
}else
{
//moving left and down
possibleMove2 = tileIndex - tileCountTall;
}
}
moveNode1 = nil;
moveNode2 = nil;
CGPoint coordinate1 = [MapOfTiles tileCoordForIndex:possibleMove1];
CGPoint coordinate2 = [MapOfTiles tileCoordForIndex:possibleMove2];
if([adjSteps containsObject:[NSValue valueWithCGPoint:coordinate1]])
{
//we know that possible move to reach destination has been deemed walkable, get it's move cost from the map
moveNode1 = [[MapOfTiles sharedInstance] mapTiles] [possibleMove1];
}
if([adjSteps containsObject:[NSValue valueWithCGPoint:coordinate2]])
{
//we know that the second possible move is walkable
moveNode2 = [[MapOfTiles sharedInstance] mapTiles] [possibleMove2];
}
#warning not sure about this one if the algorithm has to backtrack really far back
//find out which square has the lowest move cost
lowestMoveCost = fminf(moveNode1.moveCost, moveNode2.moveCost) * 10;
moveCost = (int)sqrt(lowestMoveCost*lowestMoveCost + (stepNode.moveCost*10) * (stepNode.moveCost*10));
}
// Compute the cost form the current step to that step
// Check if the step is already in the open list
NSUInteger index = [self.spOpenSteps indexOfObject:step];
if (index == NSNotFound) { // Not on the open list, so add it
// Set the current step as the parent
step.parent = currentStep;
// The G score is equal to the parent G score + the cost to move from the parent to it
step.gScore = currentStep.gScore + moveCost;
// Compute the H score which is the estimated movement cost to move from that step to the desired tile coordinate
step.hScore = [self computeHScoreFromCoord:step.position toCoord:toTileCoord];
// Adding it with the function which is preserving the list ordered by F score
[self insertInOpenSteps:step];
}
else { // Already in the open list
step = (self.spOpenSteps)[index]; // To retrieve the old one (which has its scores already computed ;-)
// Check to see if the G score for that step is lower if we use the current step to get there
if ((currentStep.gScore + moveCost) < step.gScore) {
// The G score is equal to the parent G score + the cost to move from the parent to it
step.gScore = currentStep.gScore + moveCost;
// Because the G Score has changed, the F score may have changed too
// So to keep the open list ordered we have to remove the step, and re-insert it with
// the insert function which is preserving the list ordered by F score
// Now we can removing it from the list without be afraid that it can be released
[self.spOpenSteps removeObjectAtIndex:index];
// Re-insert it with the function which is preserving the list ordered by F score
[self insertInOpenSteps:step];
}
}
}
These types of problems are quite common in, say, chip routing and, yes, gamedev.
Standard approach is to have your graph (in C++ I would say you have Boost "grid graph" or similar structure). If you can afford to have an object each vertex, then the solution is quite easy.
You connect two vertices (neighbors or diagonally adjacent) by an edge, unless there is an obstacle between them. You assign this edge a weight equal to edge length (10 or 14) times terrain cost. Sometimes people prefer not to exclude obstacle edges but assign extremely high weights to them (an advantage is that with such approach you are guaranteed to find at least some path, even when object is stuck at an island).
Then you apply A* algorithm. Your heuristic function (H) can be "pessimistic" (equal to Euclidean distance times the max move cost) or "optimistic" (Euclidean distance times min move cost) or anything in between. Different heuristics will result in slightly different "personalities" of your search but usually do not matter much.

How to program smooth movement with the accelerometer like a labyrinth game on iPhone OS?

I want to be able to make image move realistically with the accelerometer controlling it, like any labyrinth game. Below shows what I have so far but it seems very jittery and isnt realistic at all. The ball images seems to never be able to stop and does lots of jittery movements around everywhere.
- (void)accelerometer:(UIAccelerometer *)accelerometer didAccelerate:(UIAcceleration *)acceleration {
deviceTilt.x = 0.01 * deviceTilt.x + (1.0 - 0.01) * acceleration.x;
deviceTilt.y = 0.01 * deviceTilt.y + (1.0 - 0.01) * acceleration.y;
}
-(void)onTimer {
ballImage.center = CGPointMake(ballImage.center.x + (deviceTilt.x * 50), ballImage.center.y + (deviceTilt.y * 50));
if (ballImage.center.x > 279) {
ballImage.center = CGPointMake(279, ballImage.center.y);
}
if (ballImage.center.x < 42) {
ballImage.center = CGPointMake(42, ballImage.center.y);
}
if (ballImage.center.y > 419) {
ballImage.center = CGPointMake(ballImage.center.x, 419);
}
if (ballImage.center.y < 181) {
ballImage.center = CGPointMake(ballImage.center.x, 181);
}
Is there some reason why you can not use the smoothing filter provided in response to your previous question: How do you use a moving average to filter out accelerometer values in iPhone OS ?
You need to calculate the running average of the values. To do this you need to store the last n values in an array, and then push and pop values off the array when ever you read the accelerometer data. Here is some pseudocode:
const SIZE = 10;
float[] xVals = new float[SIZE];
float xAvg = 0;
function runAverage(float newX){
xAvg += newX/SIZE;
xVals.push(newX);
if(xVals.length > SIZE){
xAvg -= xVals.pop()/SIZE;
}
}
You need to do this for all three axis. Play around with the value of SIZE; the larger it is, the smoother the value, but the slower things will seem to respond. It really depends on how often you read the accelerometer value. If it is read 10 times per second, then SIZE = 10 might be too large.