public void CaptureDepthAndColor(string directory)
{
// foreach camera:
// average a bunch of frames to find a good depth image
// get calibration
// TODO: parallelize
foreach (var camera in cameras)
{
string cameraDirectory = directory + "/camera" + camera.name;
if (!Directory.Exists(cameraDirectory))
Directory.CreateDirectory(cameraDirectory);
// compute mean and variance of depth image
var sum = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
sum.Zero();
var sumSquared = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
sumSquared.Zero();
var count = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
count.Zero();
var depth = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
for (int i = 0; i < 100; i++)
{
var depthBytes = camera.Client.LatestDepthImage();
Marshal.Copy(depthBytes, 0, depth.DataIntPtr, Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight * 2);
Console.WriteLine("acquired depth image " + i);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
if (depth[x, y] != 0)
{
ushort d = depth[x, y];
count[x, y]++;
sum[x, y] += d;
sumSquared[x, y] += d * d;
}
}
var meanImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
meanImage.Zero(); // not all pixels will be assigned
var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
varianceImage.Zero(); // not all pixels will be assigned
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
if (count[x, y] > 50)
{
float mean = sum[x, y] / count[x, y];
meanImage[x, y] = mean;
float variance = sumSquared[x, y] / count[x, y] - mean * mean;
varianceImage[x, y] = variance;
}
}
// WIC doesn't support encoding float tiff images, so for now we write to a binary file
meanImage.SaveToFile(cameraDirectory + "/mean.bin");
varianceImage.SaveToFile(cameraDirectory + "/variance.bin");
// create a short version that we can write, used only for debugging
var meanDepthShortImage = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
meanDepthShortImage[x, y] = (ushort)meanImage[x, y];
SaveToTiff(imagingFactory, meanDepthShortImage, cameraDirectory + "/mean.tiff");
// convert to world coordinates and save to ply file
camera.calibration = camera.Client.GetCalibration();
var depthFrameToCameraSpaceTable = camera.calibration.ComputeDepthFrameToCameraSpaceTable();
var world = new Float3Image(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); // TODO: move out/reuse
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
var pointF = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x];
float meanDepthMeters = meanImage[x, y] / 1000.0f;
Float3 worldPoint;
worldPoint.x = pointF.X * meanDepthMeters;
worldPoint.y = pointF.Y * meanDepthMeters;
worldPoint.z = meanDepthMeters;
world[x, y] = worldPoint;
}
SaveToPly(cameraDirectory + "/mean.ply", world);
// TODO: consider writing OBJ instead
}
//// connect to projectors
//foreach (var projector in projectors)
//{
// projector.Client.OpenDisplay(projector.displayIndex);
//}
// collect color images; this is not necessary for calibration, but is nice to have for visualization
//foreach (var projector in projectors)
// projector.Client.SetColor(projector.displayIndex, 0f, 0f, 0f);
//System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/color.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, Kinect2Calibration.colorImageWidth * Kinect2Calibration.colorImageHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/color.tiff");
image.Dispose();
}
//// close all displays
//foreach (var projector in projectors)
//{
// projector.Client.CloseDisplay(projector.displayIndex);
//}
}
public static void SaveToPly(string filename, Float3Image pts3D)
{
// force decimal point so standard programs like MeshLab can read this
string CultureName = System.Threading.Thread.CurrentThread.CurrentCulture.Name;
var cultureInfo = new System.Globalization.CultureInfo(CultureName);
if (cultureInfo.NumberFormat.NumberDecimalSeparator != ".")
{
cultureInfo.NumberFormat.NumberDecimalSeparator = ".";
System.Threading.Thread.CurrentThread.CurrentCulture = cultureInfo;
}
using (System.IO.StreamWriter file = new System.IO.StreamWriter(filename, false, Encoding.ASCII))
{
// Write Header
file.WriteLine("ply");
file.WriteLine("format ascii 1.0");
file.WriteLine("comment VCGLIB generated");
// Write Elements
file.WriteLine("element vertex " + pts3D.Width * pts3D.Height);
file.WriteLine("property float x\nproperty float y\nproperty float z");
//file.WriteLine("element face 0");
//file.WriteLine("property list uchar int vertex_indices");
file.WriteLine("end_header");
for (int r = 0; r < pts3D.Height; r++)
{
for (int c = 0; c < pts3D.Width; c++)
{
Float3 xyz = pts3D[c, r];
if (xyz.z == float.NaN)
{
file.WriteLine("0 0 0");
continue;
}
file.WriteLine(xyz.x.ToString("0.000") + " " + xyz.y.ToString("0.000") + " " + xyz.z.ToString("0.000"));
}
}
}
}
public static void SaveToPly(string filename, Float3Image pts3D)
{
using (var file = new CultureInvariantStreamWriter(filename, false, Encoding.ASCII))
{
// Write Header
file.WriteLine("ply");
file.WriteLine("format ascii 1.0");
file.WriteLine("comment VCGLIB generated");
// Write Elements
file.WriteLine("element vertex " + pts3D.Width * pts3D.Height);
file.WriteLine("property float x\nproperty float y\nproperty float z");
//file.WriteLine("element face 0");
//file.WriteLine("property list uchar int vertex_indices");
file.WriteLine("end_header");
for (int r = 0; r < pts3D.Height; r++)
{
for (int c = 0; c < pts3D.Width; c++)
{
Float3 xyz = pts3D[c, r];
if (xyz.z == float.NaN)
{
file.WriteLine("0 0 0");
continue;
}
file.WriteLine(xyz.x.ToString("0.000") + " " + xyz.y.ToString("0.000") + " " + xyz.z.ToString("0.000"));
}
}
}
}
public void CaptureDepthAndColor(string directory)
{
// foreach camera:
// average a bunch of frames to find a good depth image
// get calibration
// TODO: parallelize
foreach (var camera in cameras)
{
string cameraDirectory = directory + "/camera" + camera.name;
if (!Directory.Exists(cameraDirectory))
Directory.CreateDirectory(cameraDirectory);
// compute mean and variance of depth image
var sum = new FloatImage(depthWidth, depthHeight);
sum.Zero();
var sumSquared = new FloatImage(depthWidth, depthHeight);
sumSquared.Zero();
var count = new ShortImage(depthWidth, depthHeight);
count.Zero();
var depth = new ShortImage(depthWidth, depthHeight);
for (int i = 0; i < 100; i++)
{
var depthBytes = camera.Client.LatestDepthImage();
Marshal.Copy(depthBytes, 0, depth.DataIntPtr, depthWidth * depthHeight * 2);
Console.WriteLine("acquired depth image " + i);
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
if (depth[x, y] != 0)
{
ushort d = depth[x, y];
count[x, y]++;
sum[x, y] += d;
sumSquared[x, y] += d * d;
}
}
var meanImage = new FloatImage(depthWidth, depthHeight);
meanImage.Zero(); // not all pixels will be assigned
var varianceImage = new FloatImage(depthWidth, depthHeight);
varianceImage.Zero(); // not all pixels will be assigned
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
{
if (count[x, y] > 50)
{
float mean = sum[x, y] / count[x, y];
meanImage[x, y] = mean;
float variance = sumSquared[x, y] / count[x, y] - mean * mean;
varianceImage[x, y] = variance;
}
}
// WIC doesn't support encoding float tiff images, so for now we write to a binary file
meanImage.SaveToFile(cameraDirectory + "/mean.bin");
varianceImage.SaveToFile(cameraDirectory + "/variance.bin");
// create a short version that we can write, used only for debugging
var meanDepthShortImage = new ShortImage(depthWidth, depthHeight);
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
meanDepthShortImage[x, y] = (ushort)meanImage[x, y];
SaveToTiff(imagingFactory, meanDepthShortImage, cameraDirectory + "/mean.tiff");
// convert to world coordinates and save to ply file
camera.calibration = camera.Client.GetCalibration();
var depthFrameToCameraSpaceTable = camera.calibration.ComputeDepthFrameToCameraSpaceTable();
var world = new Float3Image(depthWidth, depthHeight); // TODO: move out/reuse
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
{
var pointF = depthFrameToCameraSpaceTable[y * depthWidth + x];
Float3 worldPoint;
worldPoint.x = pointF.X * meanImage[x, y];
worldPoint.y = pointF.Y * meanImage[x, y];
worldPoint.z = meanImage[x, y];
world[x, y] = worldPoint;
}
SaveToPly(cameraDirectory + "/mean.ply", world);
// TODO: consider writing OBJ instead
}
// connect to projectors
foreach (var projector in projectors)
{
//var binding = new NetTcpBinding();
//binding.Security.Mode = SecurityMode.None;
//var uri = "net.tcp://" + projector.hostNameOrAddress + ":9001/ProjectorServer/service";
//var address = new EndpointAddress(uri);
//projector.client = new ProjectorServerClient(binding, address);
projector.Client.OpenDisplay(projector.displayIndex);
}
// collect color images when projecting all white and all black
// set projectors to white
foreach (var projector in projectors)
projector.Client.SetColor(projector.displayIndex, 1f, 1f, 1f);
System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/color.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(colorWidth, colorHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, colorWidth * colorHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/color.tiff");
image.Dispose();
}
foreach (var projector in projectors)
projector.Client.SetColor(projector.displayIndex, 0f, 0f, 0f);
System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/colorDark.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(colorWidth, colorHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, colorWidth * colorHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/colorDark.tiff");
image.Dispose();
}
// close all displays
foreach (var projector in projectors)
{
projector.Client.CloseDisplay(projector.displayIndex);
}
}
