public bool Update(ShortImage depth)
{
// compute the raw valid pixels for this frame
ByteImage maskCur = Mask1(depth);
// initialize the acc image if we need to
if (mFrameIter == 0)
{
mAcc32.Add(depth);
mCount16.Add(maskCur);
mMask.Copy(maskCur);
mFrameIter++;
return(mFrameIter >= mNumFrames);
}
// compute the pixels that don't differ from previous values
ByteImage diffMask = DiffMask(depth, mAcc32, mFrameIter, mValidDelta * 1000.0f);
// update the rolling depthMask
mMask.And(maskCur);
mMask.And(diffMask);
mCount16.Add(maskCur);
mAcc32.Add(depth);
mFrameIter++;
maskCur.Dispose();
diffMask.Dispose();
return(mFrameIter >= mNumFrames);
}
public PreProcessDepthMap_Variance(int width, int height)
{
sum = new FloatImage(width, height);
sum.SetTo(0);
sumSquare = new FloatImage(width, height);
sumSquare.SetTo(0);
n = new ShortImage(width, height);
n.SetTo(0);
}
public void Update(ShortImage depth)
{
for (int r = 0; r < depth.Height; r++)
{
for (int c = 0; c < depth.Width; c++)
{
float d = (float)depth[c, r] / 1000f; // meters
if (d != 0)
{
sum[c, r] += d;
sumSquare[c, r] += d * d;
n[c, r]++;
}
}
}
}
public PreProcessDepthMap_Conservative(int width, int height, int numFrames, float validDelta)
{
mAcc32 = new FloatImage(width, height);
mMask = new ByteImage(width, height);
mMask.SetTo(1);
mCount16 = new ShortImage(width, height);
mCount16.SetTo(0);
mValidDelta = validDelta;
if (numFrames < 1 || numFrames >= 655536)
{
throw new System.ArgumentException("PreProcessDepthMap numFrames must be >= 1 && < 655536");
}
mNumFrames = numFrames;
mFrameIter = 0;
}
/// <summary>
/// Compute a mask representing pixels that are similiar to the current average pixels computed
/// with acc32/count
/// </summary>
/// <param name="depth"></param>
/// <param name="acc32"></param>
/// <param name="count32"></param>
/// <returns></returns>
public static ByteImage DiffMask(ShortImage depthIm, FloatImage acc32, int count, float delta)
{
ByteImage depthMask = new ByteImage(depthIm.Width, depthIm.Height);
// all values zero by default
for (int r = 0; r < depthIm.Height; r++)
{
for (int c = 0; c < depthIm.Width; c++)
{
ushort depthVal = depthIm[c, r];
float depthAvgVal = acc32[c, r] / count;
float diff = Math.Abs(depthVal - depthAvgVal);
if (diff < delta)
{
depthMask[c, r] = 1;
}
}
}
return(depthMask);
}
/// <summary>
/// Return 1 for valid 0 for invalid pts
/// NOTE: NOT 0/255
/// </summary>
/// <param name="depth"></param>
/// <returns></returns>
public static ByteImage Mask1(ShortImage depth)
{
// now erode the results (to correct for any errors in Kinect Depth -> RGB calibration
// create a mask to erode
ByteImage depthMask = new ByteImage(depth.Width, depth.Height);
// intialize the depth mask to be the valid Kinect data
for (int r = 0; r < depth.Height; r++)
{
for (int c = 0; c < depth.Width; c++)
{
if (depth[c, r] > 0.0f)
{
depthMask[c, r] = 1;
}
else
{
depthMask[c, r] = 0;
}
}
}
return(depthMask);
}
/// <summary>
///
/// </summary>
/// <param name="directory"></param>
/// <param name="objPath"></param>
/// <param name="depthImage"></param>
/// <param name="rgbImage"></param>
/// <param name="pose">Optional transformation pose (4x4 Matrix). Supply Identity by default</param>
public static void Save(string objPath, Kinect2Calibration calibration, ShortImage depthImage, string colorImageFileName, Matrix pose)
{
var objFilename = Path.GetFileNameWithoutExtension(objPath);
var objDirectory = Path.GetDirectoryName(objPath);
if (!Directory.Exists(objDirectory))
{
Directory.CreateDirectory(objDirectory);
}
//copy the background color image to file
//SaveColorToJPEG(objDirectory + "/" + objFilename + ".jpg", rgbImage);
if (File.Exists(colorImageFileName))
{
File.Copy(colorImageFileName, objDirectory + "/" + objFilename + ".jpg", true);
}
else
{
Console.WriteLine("Saving to OBJ Error! File " + colorImageFileName + " doesn't exists!");
}
// Because we need to form triangles, we go back to the depth image
var quadOffsets = new System.Drawing.Point[]
{
new System.Drawing.Point(0, 0),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(0, 1),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(1, 1),
new System.Drawing.Point(0, 1),
};
var streamWriter = new StreamWriter(objDirectory + "/" + objFilename + ".obj");
var mtlFileWriter = new StreamWriter(objDirectory + "/" + objFilename + ".mtl");
streamWriter.WriteLine("mtllib " + objFilename + ".mtl");
uint nextVertexIndex = 1;
//var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
mtlFileWriter.WriteLine("newmtl camera0");
mtlFileWriter.WriteLine("Ka 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Kd 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Ks 0.000000 0.000000 0.000000");
mtlFileWriter.WriteLine("Tr 1.000000");
mtlFileWriter.WriteLine("illum 1");
mtlFileWriter.WriteLine("Ns 0.000000");
mtlFileWriter.WriteLine("map_Kd " + objFilename + ".jpg");
streamWriter.WriteLine("usemtl camera0");
// load depth image
//string cameraDirectory = directory + "/camera" + camera.name;
//depthImage.LoadFromFile(cameraDirectory + "/mean.bin");
//var calibration = camera.calibration;
var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable();
var vertices = new Vertex[Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight];
var colorCamera = new Matrix(4, 1);
var depthCamera = new Matrix(4, 1);
var world = new Matrix(4, 1);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
{
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
// depth camera coords
var depth = depthImage[x, y] / 1000f; // m
// convert to depth camera space
var point = depthFrameToCameraSpaceTable[Kinect2Calibration.depthImageWidth * y + x];
depthCamera[0] = point.X * depth;
depthCamera[1] = point.Y * depth;
depthCamera[2] = depth;
depthCamera[3] = 1;
// world coordinates
world.Mult(pose, depthCamera);
//world.Scale(1.0 / world[3]); not necessary for this transform
// convert to color camera space
colorCamera.Mult(calibration.depthToColorTransform, depthCamera);
colorCamera.Scale(1.0 / colorCamera[3]);
// project to color image
double colorU, colorV;
CameraMath.Project(calibration.colorCameraMatrix, calibration.colorLensDistortion, colorCamera[0], colorCamera[1], colorCamera[2], out colorU, out colorV);
colorU /= (double)Kinect2Calibration.colorImageWidth;
colorV /= (double)Kinect2Calibration.colorImageHeight;
var vertex = new Vertex();
vertex.x = (float)world[0];
vertex.y = (float)world[1];
vertex.z = (float)world[2];
vertex.u = (float)colorU;
vertex.v = (float)colorV;
vertices[Kinect2Calibration.depthImageWidth * y + x] = vertex;
}
}
streamWriter.WriteLine("g camera0");
streamWriter.WriteLine("usemtl camera0");
// examine each triangle
for (int y = 0; y < Kinect2Calibration.depthImageHeight - 1; y++)
{
for (int x = 0; x < Kinect2Calibration.depthImageWidth - 1; x++)
{
int offseti = 0;
for (int tri = 0; tri < 2; tri++)
{
// the indexes of the vertices of this triangle
var i0 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti].Y) + (x + quadOffsets[offseti].X);
var i1 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 1].Y) + (x + quadOffsets[offseti + 1].X);
var i2 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 2].Y) + (x + quadOffsets[offseti + 2].X);
// is triangle valid?
bool nonZero = (vertices[i0].z != 0) && (vertices[i1].z != 0) && (vertices[i2].z != 0);
bool jump01 = Vertex.DistanceSquared(vertices[i0], vertices[i1]) < 0.2 * 0.2;
bool jump02 = Vertex.DistanceSquared(vertices[i0], vertices[i2]) < 0.2 * 0.2;
bool jump12 = Vertex.DistanceSquared(vertices[i1], vertices[i2]) < 0.2 * 0.2;
bool valid = nonZero && jump01 && jump02 && jump12;
if (valid)
{
// only add the vertex if we haven't already
if (vertices[i0].index == 0)
{
streamWriter.WriteLine(vertices[i0]);
vertices[i0].index = nextVertexIndex++;
}
if (vertices[i1].index == 0)
{
streamWriter.WriteLine(vertices[i1]);
vertices[i1].index = nextVertexIndex++;
}
if (vertices[i2].index == 0)
{
streamWriter.WriteLine(vertices[i2]);
vertices[i2].index = nextVertexIndex++;
}
streamWriter.WriteLine("f {0}/{0} {1}/{1} {2}/{2}", vertices[i0].index, vertices[i1].index, vertices[i2].index);
}
offseti += 3;
}
}
}
streamWriter.Close();
mtlFileWriter.Close();
}
