/// <summary>
/// 深度データからリアルワールドの色を見つける
/// </summary>
/// <param name="depthData"></param>
/// <param name="colorFrameData"></param>
/// <param name="colorSize"></param>
/// <returns></returns>
public List <Tuple <CvPoint3D64f, CvColor> > DepthColorMatToRealPoints(CvMat depthMat, CvMat colorMat)
{
List <Tuple <CvPoint3D64f, CvColor> > res = new List <Tuple <CvPoint3D64f, CvColor> >();
int bytesPerPixel = colorMat.ElemChannels;
CvSize colorSize = colorMat.GetSize();
unsafe
{
short *depthArr = depthMat.DataInt16;
byte * colorArr = colorMat.DataByte;
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
ushort depthVal = (ushort)depthArr[depthIndex];
ColorSpacePoint colorPoint = this.MapDepthPointToColorSpace(x, y, depthVal, colorSize.Width, colorSize.Height);
CameraSpacePoint cameraPoint = this.MapDepthPointToCameraSpace(x, y, depthVal);
// make sure the depth pixel maps to a valid point in color space
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorSize.Width) && (colorY >= 0) && (colorY < colorSize.Height))
{
// calculate index into color array
int colorIndex = ((colorY * colorSize.Width) + colorX) * bytesPerPixel;
CvColor color = new CvColor(colorArr[colorIndex + 2], colorArr[colorIndex + 1], colorArr[colorIndex + 0]);
res.Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
}
}
}
return(res);
}
/// <summary>
/// ユーザのボクセルを返す
/// </summary>
/// <param name="depthData"></param>
/// <param name="colorFrameData"></param>
/// <param name="bodyIndexFrameData"></param>
/// <param name="colorSize"></param>
/// <returns></returns>
public Dictionary <int, List <Tuple <CvPoint3D64f, CvColor> > > GetEachUserColorPoints(CvMat depthMat, CvMat colorMat, CvMat userMat)
{
Dictionary <int, List <Tuple <CvPoint3D64f, CvColor> > > res = new Dictionary <int, List <Tuple <CvPoint3D64f, CvColor> > >();
List <Tuple <CvPoint3D64f, CvColor> > lis;
int bytesPerPixel = colorMat.ElemChannels;
CvSize colorSize = colorMat.GetSize();
unsafe
{
short *depthArr = depthMat.DataInt16;
byte * colorArr = colorMat.DataByte;
byte * userArr = userMat.DataByte;
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = userArr[depthIndex];
if (player != 0xff)
{
ushort depthVal = (ushort)depthArr[depthIndex];
ColorSpacePoint colorPoint = this.MapDepthPointToColorSpace(x, y, depthVal, colorSize.Width, colorSize.Height);
CameraSpacePoint cameraPoint = this.MapDepthPointToCameraSpace(x, y, depthVal);
// make sure the depth pixel maps to a valid point in color space
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorSize.Width) && (colorY >= 0) && (colorY < colorSize.Height))
{
// calculate index into color array
int colorIndex = ((colorY * colorSize.Width) + colorX) * bytesPerPixel;
CvColor color = new CvColor(colorArr[colorIndex + 2], colorArr[colorIndex + 1], colorArr[colorIndex + 0]);
if (res.TryGetValue((int)player, out lis))
{
res[(int)player].Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
else
{
res[(int)player] = new List <Tuple <CvPoint3D64f, CvColor> >();
res[(int)player].Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
}
}
}
}
}
return(res);
}
/// <summary>
/// 深度データからリアルワールドの色を見つける
/// </summary>
/// <param name="depthData"></param>
/// <param name="colorFrameData"></param>
/// <param name="colorSize"></param>
/// <returns></returns>
public List<Tuple<CvPoint3D64f, CvColor>> DepthColorMatToRealPoints(CvMat depthMat, CvMat colorMat)
{
List<Tuple<CvPoint3D64f, CvColor>> res = new List<Tuple<CvPoint3D64f, CvColor>>();
int bytesPerPixel = colorMat.ElemChannels;
CvSize colorSize = colorMat.GetSize();
unsafe
{
short* depthArr = depthMat.DataInt16;
byte* colorArr = colorMat.DataByte;
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
ushort depthVal = (ushort)depthArr[depthIndex];
ColorSpacePoint colorPoint = this.MapDepthPointToColorSpace(x, y, depthVal, colorSize.Width, colorSize.Height);
CameraSpacePoint cameraPoint = this.MapDepthPointToCameraSpace(x, y, depthVal);
// make sure the depth pixel maps to a valid point in color space
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorSize.Width) && (colorY >= 0) && (colorY < colorSize.Height))
{
// calculate index into color array
int colorIndex = ((colorY * colorSize.Width) + colorX) * bytesPerPixel;
CvColor color = new CvColor(colorArr[colorIndex + 2], colorArr[colorIndex + 1], colorArr[colorIndex + 0]);
res.Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
}
}
}
return res;
}
/// <summary>
/// ユーザのボクセルを返す
/// </summary>
/// <param name="depthData"></param>
/// <param name="colorFrameData"></param>
/// <param name="bodyIndexFrameData"></param>
/// <param name="colorSize"></param>
/// <returns></returns>
public Dictionary<int, List<Tuple<CvPoint3D64f, CvColor>>> GetEachUserColorPoints(CvMat depthMat, CvMat colorMat, CvMat userMat)
{
Dictionary<int, List<Tuple<CvPoint3D64f, CvColor>>> res = new Dictionary<int, List<Tuple<CvPoint3D64f, CvColor>>>();
List<Tuple<CvPoint3D64f, CvColor>> lis;
int bytesPerPixel = colorMat.ElemChannels;
CvSize colorSize = colorMat.GetSize();
unsafe
{
short* depthArr = depthMat.DataInt16;
byte* colorArr = colorMat.DataByte;
byte* userArr = userMat.DataByte;
for (int y = 0; y < depthHeight; ++y)
{
for (int x = 0; x < depthWidth; ++x)
{
int depthIndex = (y * depthWidth) + x;
byte player = userArr[depthIndex];
if (player != 0xff)
{
ushort depthVal = (ushort)depthArr[depthIndex];
ColorSpacePoint colorPoint = this.MapDepthPointToColorSpace(x, y, depthVal, colorSize.Width, colorSize.Height);
CameraSpacePoint cameraPoint = this.MapDepthPointToCameraSpace(x, y, depthVal);
// make sure the depth pixel maps to a valid point in color space
int colorX = (int)Math.Floor(colorPoint.X + 0.5);
int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
if ((colorX >= 0) && (colorX < colorSize.Width) && (colorY >= 0) && (colorY < colorSize.Height))
{
// calculate index into color array
int colorIndex = ((colorY * colorSize.Width) + colorX) * bytesPerPixel;
CvColor color = new CvColor(colorArr[colorIndex + 2], colorArr[colorIndex + 1], colorArr[colorIndex + 0]);
if (res.TryGetValue((int)player, out lis))
{
res[(int)player].Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
else
{
res[(int)player] = new List<Tuple<CvPoint3D64f, CvColor>>();
res[(int)player].Add(Tuple.Create((CvPoint3D64f)cameraPoint.ToCvPoint3D(), color));
}
}
}
}
}
}
return res;
}
