/// <summary>
/// Gets 3D points [units = meters] and calculated pf vector
/// </summary>
/// <param name="state">State.</param>
private void PreparePointCloud(HoloProxies.Engine.trackerState state)
{
boundingbox = findBoundingBoxFromCurrentState(state);
for (int i = 0; i < Height; i++)
{
for (int j = 0; j < Width; j++)
{
int idx = i * Width + j;
// if point is not in the bounding box
if (j < boundingbox.x || j >= boundingbox.z || i < boundingbox.y || i >= boundingbox.w)
{
// mark as not useful
Camera3DPoints[idx].X = 0;
Camera3DPoints[idx].Y = 0;
Camera3DPoints[idx].Z = 0;
PfVec[idx] = defines.OUTSIDE_BB;
}
else // if it's inside the box
{
Color pixel = ColorTexture.GetPixel(j, i);
PfVec[idx] = GetPf(pixel);
// Draw the bounding box region
if (drawBox && OnBoxEdge(j, i))
{
//ColorTextureVisual.SetPixel( j, i, Color.blue );
ColorTextureVisual.SetPixel(j, i, Color.Lerp(pixel, Color.blue, 0.3f));
}
}
}
} //end for
}
/// <summary>
/// Use the current bounding box to assign foreground pixels
/// Everything else assign as background
/// This updates the vector Mask
/// </summary>
private void LabelMaskFromCurrentStateBoundingBox(HoloProxies.Engine.trackerState state)
{
// first get bounding box for foreground pixels
boundingbox = findBoundingBoxFromCurrentState(state);
// (x,y) ----- +
// | |
// | |
// + ----- (z,w)
// set the near background bounding box
UnityEngine.Vector4 boundingboxBG =
new UnityEngine.Vector4(boundingbox.x - defines.BB_MARGIN, //top, left
boundingbox.y - defines.BB_MARGIN,
boundingbox.z + defines.BB_MARGIN, //bottom, right
boundingbox.w + defines.BB_MARGIN);
for (int i = 0; i < Height; i++)
{
for (int j = 0; j < Width; j++)
{
int idx = i * Width + j;
// if it's not a zero depth pixel i.e. useless
//if (Mask[idx] != defines.HIST_USELESS_PIXEL)
//{
// if point is in the foreground bounding box
if (j > boundingbox.x && j < boundingbox.z && i > boundingbox.y && i < boundingbox.w)
{
Mask[idx] = defines.HIST_FG_PIXEL;
}
// else if it is in the near background
else if (j > boundingboxBG.x && j < boundingboxBG.z && i > boundingboxBG.y && i < boundingboxBG.w)
{
Mask[idx] = defines.HIST_BG_PIXEL;
}
else // else it is in the far background
{
Mask[idx] = defines.HIST_USELESS_PIXEL;
}
//}
}
} //end for
}
/// <summary>
/// Finds the state of the bounding box from current.
/// Project current pose points and create a bounding box that we will use
/// for the histogram
/// </summary>
/// <param name="state">State.</param>
private UnityEngine.Vector4 findBoundingBoxFromCurrentState(HoloProxies.Engine.trackerState state)
{
Vector3[] corners = new Vector3[8];
Vector3[] ipts = new Vector3[state.numPoses() * 8];
UnityEngine.Vector4 bb = new UnityEngine.Vector4(Width, Height, 0, 0);
for (int i = -1, idx = 0; i <= 1; i += 2)
{
for (int j = -1; j <= 1; j += 2)
{
for (int k = -1; k <= 1; k += 2, idx++)
{
corners[idx] = new Vector3(i * 0.075f, j * 0.075f, k * 0.075f);
}
}
}
for (int i = 0, idx = 0; i < state.numPoses(); i++)
{
for (int j = 0; j < 8; j++, idx++)
{
Matrix4x4 H = state.getPose(i).getH();
Vector3 temp = H.MultiplyPoint(corners[j]);
ipts[idx] = K * (H.MultiplyPoint(corners[j]));
ipts[idx].x /= ipts[idx].z; ipts[idx].y /= ipts[idx].z;
bb.x = ipts[idx].x < bb.x ? ipts[idx].x : bb.x;
bb.y = ipts[idx].y < bb.y ? ipts[idx].y : bb.y;
bb.z = ipts[idx].x > bb.z ? ipts[idx].x : bb.z;
bb.w = ipts[idx].y > bb.w ? ipts[idx].y : bb.w;
}
}
bb.x = bb.x < 0 ? 0 : bb.x;
bb.y = bb.y < 0 ? 0 : bb.y;
bb.z = bb.z > Width ? Width : bb.z;
bb.w = bb.w > Height ? Height : bb.w;
return(bb);
}
/// <summary>
/// Re-initializes the histogram based on current state.
/// Based on the pose, projects a foreground bounding box (everything
/// else is background) and stores it in Mask. Only the foreground pixels
/// are used to create the histogram.
/// </summary>
/// <param name="state">State.</param>
public void ReinitHistogramFromRendering(HoloProxies.Engine.trackerState state)
{
LabelMaskFromCurrentStateBoundingBox(state);
histogram.BuildHistogram(ColorTexture, Mask);
}
/// <summary>
/// Updates the frame by grabbing the rgb and depth from the Kinnect.
/// </summary>
/// <returns><c>true</c>, if frame was successfully updated, <c>false</c> otherwise.</returns>
/// <param name="state">State.</param>
public bool UpdateFrame(HoloProxies.Engine.trackerState state)
{
bool success = false;
if (_Reader != null)
{
var frame = _Reader.AcquireLatestFrame();
if (frame != null)
{
//Debug.Log( "in here1" );
var colorFrame = frame.ColorFrameReference.AcquireFrame();
if (colorFrame != null)
{
//Debug.Log( "in here2" );
var depthFrame = frame.DepthFrameReference.AcquireFrame();
if (depthFrame != null)
{
//Debug.Log( "in here3" );
// get color data + texture
colorFrame.CopyConvertedFrameDataToArray(ColorData_full, ColorImageFormat.Rgba);
// get depth data
depthFrame.CopyFrameDataToArray(DepthData_full);
// Create a depth texture
//int index = 0;
//foreach (var ir in DepthData_full)
//{
// byte intensity = (byte)(ir >> 8);
// DepthRaw[index++] = intensity;
// DepthRaw[index++] = intensity;
// DepthRaw[index++] = intensity;
// DepthRaw[index++] = 255; // Alpha
//}
//DepthTexture_full.LoadRawTextureData( DepthRaw );
//DepthTexture_full.Apply(); TODO fix this
// dispose frame
depthFrame.Dispose();
depthFrame = null;
// Map depth to RGBD
AlignRGBD();
// Unproject to 3D points in camera space (based on bounding box)
UnprojectFrom2Dto3D();
// Downsample all data frames if necessary
DownsampleAndFilterRGBDImage();
// Apply downsampled texture
ColorTexture.LoadRawTextureData(ColorData);
ColorTextureVisual.LoadRawTextureData(ColorData);
// Unproject points to 3D space
PreparePointCloud(state);
if (plotHistogram)
{
for (int i = 0; i < HistogramTexture.height; i++)
{
for (int j = 0; j < HistogramTexture.width; j++)
{
int idx = i * HistogramTexture.width + j;
HistogramTexture.SetPixel(j, i, new Color(histogram.posterior[idx * 256], 0, 0));
}
}
HistogramTexture.Apply();
}
success = true;
}
colorFrame.Dispose();
colorFrame = null;
}
frame = null;
}
}
return(success);
}
