public static Vec3 DepthPostoJointPos(Vec2 depthPos, DepthData depth)
{
if (depth.Description.EnableCoordinate == false)
{
return(new Vec3(0, 0, 0));
}
if (depth == null || depth.FrameData == null || depth.FrameData.Length <= 0)
{
return(new Vec3(0, 0, 0));
}
if (depthPos.x >= depth.Description.Width)
{
depthPos.x = depth.Description.Width - 1;
}
else if (depthPos.x < 0)
{
depthPos.x = 0;
}
if (depthPos.y >= depth.Description.Height)
{
depthPos.y = depth.Description.Height - 1;
}
else if (depthPos.y < 0)
{
depthPos.y = 0;
}
if (((int)depthPos.x + (int)depthPos.y * depth.Description.Width) > depth.FrameData.Length)
{
return(new Vec3(0, 0, 0));
}
double depthValue = depth.FrameData[(int)depthPos.x + (int)depthPos.y * depth.Description.Width];
double depthWidth = (double)depth.Description.Width;
double depthHeight = (double)depth.Description.Height;
Vec3 vec = new Vec3(0, 0, 0);
double jZ = (double)depthValue * depth.Description.DepthToJointZMult;
vec.x = ((depthPos.x * (2 / depthWidth)) - 1) / depth.Description.JointDepthXMult + depth.Description.JointDepthXFix;
vec.y = (-(depthPos.y - depthHeight) * 2 / depthWidth - 1) / depth.Description.JointDepthYMult + depth.Description.JointDepthYFix;
vec.x *= jZ;
vec.y *= jZ;
vec.z = jZ;
return(vec);
}
public DepthData GetDepthFrame()
{
if (!DepthFrameAuthority)
{
throw new AuthorityException(AuthorityException.AuthorityTypes.Depth, this);
}
if (!depthFrameReady)
{
return(null);
}
ushort[] bits = null;
if (depthChannel.Read(out bits) == false) // 쉐어드메모리 읽기 실패
{
return(lastDepthFrame);
}
//[수정요망] Min,Max depthValue를 센서에서 받아온 데이터로 바꿔야함.
lastDepthFrame = new DepthData(bits, DepthInfo);
return(lastDepthFrame);
}
