camera:realsense D435
c++
how can i call corresponding point of joint(x,y,z) and mapping data to corresponding joint.
i have an example code in C# but pls write in C++ i am enginnering student and struct this problem for a while
/*This function is used to get the angle between 3 joints input to the function
Vector algebra in 3D is used for the purpose.
The variable joint2 is used a fulcrum.
The absolute x, y and z coordinates of the joints obtainted.
their relative distance is calculated.
using dot product concept
angle cos(x) = (vect(A).vect(B)) / (mag(vect(A)) x mag(vect(B)))
*/
double GetAngle(Joint joint1, Joint joint2, Joint joint3)
{
double x1 = joint1.Position.X;
// x coordinate of joint1
double y1 = joint1.Position.Y;
// y coordinate of joint1
double z1 = joint1.Position.Z;
// z coordinate of joint1
double x2 = joint2.Position.X;
// x coordinate of joint2
double y2 = joint2.Position.Y;
// y coordinate of joint2
double z2 = joint2.Position.Z;
// z coordinate of joint2
double x3 = joint3.Position.X;
// x coordinate of joint3
double y3 = joint3.Position.Y;
// y coordinate of joint3
double z3 = joint3.Position.Z;
// z coordinate of joint3
double shiftx1 = x1 - x2;
// x coordinate - Relative position of joint1 with respect to joint2
double shifty1 = y1 - y2;
// y coordinate - Relative position of joint1 with respect to joint2
double shiftz1 = z1 - z2;
// z coordinate - Relative position of joint1 with respect to joint2
double shiftx2 = x3 - x2;
// x coordinate - Relative position of joint3 with respect to joint2
double shifty2 = y3 - y2;
// y coordinate - Relative position of joint3 with respect to joint2
double shiftz2 = z3 - z2;
// z coordinate - Relative position of joint3 with respect to joint2
double product = shiftx1 * shiftx2 + shifty1 * shifty2 + shiftz1 * shiftz2;
// dot product
double mag1 = Math.Abs(Math.Sqrt(Math.Pow(shiftx1, 2) + Math.Pow(shifty1, 2) + Math.Pow(shiftz1, 2)));
// magnitude of vector 1
double mag2 = Math.Abs(Math.Sqrt(Math.Pow(shiftx2, 2) + Math.Pow(shifty2, 2) + Math.Pow(shiftz2, 2)));
// magnitude of Vector 2
double temp = product / (mag1 * mag2);
double angle = (180 / Math.PI) * Math.Acos(temp);
return angle;
}
void sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
//declare an array of Skeletons
Skeleton[] skeletons = new Skeleton[6];
//Opens a SkeletonFrame object, which contains one frame of skeleton data.
using (SkeletonFrame skeletonframe = e.OpenSkeletonFrame())
{
//Check if the Frame is Indeed open
if (skeletonframe != null)
{
skeletons = new Skeleton[skeletonframe.SkeletonArrayLength];
// Copies skeleton data to an array of Skeletons, where each Skeleton contains a collection of the joints.
skeletonframe.CopySkeletonDataTo(skeletons);
Skeleton skeleton = GetPrimarySkeleton(skeletons);
if (skeleton != null)
{
// Get 3 joints - rightelbow, rightwrist and rightshoulder
// You can take any 3 joints of your choice from the 20 available.
Joint rightelbow = skeleton.Joints[JointType.ElbowRight];
Joint rightwrist = skeleton.Joints[JointType.WristRight];
Joint rightshoulder = skeleton.Joints[JointType.ShoulderRight];
// Get the angle between the joints.
double angle = GetAngle(rightwrist, rightelbow, rightshoulder);
label4.Content = " The angle between Right - < wrist, elbow, shoulder > is";
label2.Content = angle;
label3.Content = "Degrees";
// Copies skeleton data to an array of Skeletons, where each Skeleton contains a collection of the joints.
skeletonframe.CopySkeletonDataTo(skeletons);
//draw the Skeleton based on the Default Mode(Standing), "Seated"
if (sensor.SkeletonStream.TrackingMode == SkeletonTrackingMode.Default)
{
//Draw standing Skeleton
DrawStandingSkeletons(skeletons);
}
else if (sensor.SkeletonStream.TrackingMode == SkeletonTrackingMode.Seated)
{
//Draw a Seated Skeleton with 10 joints
DrawSeatedSkeletons(skeletons);
}
}
}
}
}
Thank you