/// <summary> /// Demonstrates how to use our Kinect calibration to convert a depth image point to color image coordinates. /// </summary> /// <param name="calibration"></param> /// <param name="kinectSensor"></param> public void Run(Kinect2Calibration calibration, KinectSensor kinectSensor) { this.calibration = calibration; this.kinectSensor = kinectSensor; depthImage = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); depthFrameReader = kinectSensor.DepthFrameSource.OpenReader(); depthFrameReader.FrameArrived += depthFrameReader_FrameArrived; }
public void Copy(ShortImage a, int shift) { ushort *pa = a.Data(0, 0); byte * p = data; for (int i = 0; i < width * height; i++) { *p++ = (byte)(*pa++ >> shift); } }
public void Add(ShortImage a) { ushort *pa = a.Data(0, 0); float * p = data; for (int i = 0; i < width * height; i++) { *p = *p + *pa; p++; pa++; } }
public void Add(ShortImage a) { ushort *p_a = a.Data(0, 0); ushort *p = data; for (int i = 0; i < width * height; i++) { *p = (ushort)((*p_a) + (*p)); p++; p_a++; } }
public void ColorImageToDepthImage(double colorX, double colorY, ShortImage depthImage, out Matrix depthPoint, out double depthX, out double depthY) { double xUndistorted, yUndistorted; // convert to color camera space float fx = (float)colorCameraMatrix[0, 0]; float fy = (float)colorCameraMatrix[1, 1]; float cx = (float)colorCameraMatrix[0, 2]; float cy = (float)colorCameraMatrix[1, 2]; float[] kappa = new float[] { (float)colorLensDistortion[0], (float)colorLensDistortion[1] }; // flip y because our calibration expects y up (right handed coordinates at all times) CameraMath.Undistort(fx, fy, cx, cy, kappa, colorX, (colorImageHeight - colorY), out xUndistorted, out yUndistorted); var colorToDepthTransform = new Matrix(4, 4); colorToDepthTransform.Inverse(depthToColorTransform); var colorPoint = new Matrix(4, 1); depthPoint = new Matrix(4, 1); depthX = 0; depthY = 0; // walk along ray in color camera bool found = false; for (int s = 400; (s < 4500) && !found; s++) // TODO: confirm these limits (mm) { // convert to a 3D point along ray, in meters colorPoint[0] = xUndistorted * s / 1000.0; colorPoint[1] = yUndistorted * s / 1000.0; colorPoint[2] = s / 1000.0; colorPoint[3] = 1; // transform to depth camera 3D point and project depthPoint.Mult(colorToDepthTransform, colorPoint); CameraMath.Project(depthCameraMatrix, depthLensDistortion, depthPoint[0], depthPoint[1], depthPoint[2], out depthX, out depthY); int x = (int)depthX; // Y down, since we are indexing into an image int y = depthImageHeight - (int)depthY; if ((x >= 0) && (x < depthImageWidth) && (y >= 0) && (y < depthImageHeight)) { int z = depthImage[x, y]; if ((z != 0) && (z < s)) { found = true; } } } // convert back to Y down depthY = depthImageHeight - depthY; }
public void Copy(ShortImage shortImage) { ushort *p = shortImage.Data(0, 0); ARGB32 *pOut = data; for (int i = 0; i < width * height; i++) { pOut->A = 255; pOut->R = (byte)*p; pOut->G = (byte)*p; pOut++->B = (byte)*p++; } }
public void XMirror(ShortImage a) { ushort *pOut = data; ushort *pIn = a.data; for (int yy = 0; yy < height; yy++) { pIn = a.Data(width - 1, yy); for (int xx = 0; xx < width; xx++) { *pOut++ = *pIn--; } } }
public void YMirror(ShortImage a) { ushort *pOut = data; ushort *pIn = a.data; for (int yy = 0; yy < height; yy++) { pIn = a.Data(0, height - yy); for (int xx = 0; xx < width; xx++) { *pOut++ = *pIn++; } } }
public void ColorImageToDepthImage(int colorX, int colorY, ShortImage depthImage, System.Drawing.PointF[] colorFrameToCameraSpaceTable, out Matrix depthPoint, out double depthX, out double depthY) { double xUndistorted, yUndistorted; // convert to color camera space // use lookup table to perform undistortion; this will be faster when converting lots of points var point = colorFrameToCameraSpaceTable[colorY * Kinect2Calibration.colorImageWidth + colorX]; xUndistorted = point.X; yUndistorted = point.Y; var colorToDepthTransform = new Matrix(4, 4); colorToDepthTransform.Inverse(depthToColorTransform); var colorPoint = new Matrix(4, 1); depthPoint = new Matrix(4, 1); depthX = 0; depthY = 0; // walk along ray in color camera bool found = false; for (int s = 400; (s < 4500) && !found; s++) // TODO: confirm these limits (mm) { // convert to a 3D point along ray, in meters colorPoint[0] = xUndistorted * s / 1000.0; colorPoint[1] = yUndistorted * s / 1000.0; colorPoint[2] = s / 1000.0; colorPoint[3] = 1; // transform to depth camera 3D point and project depthPoint.Mult(colorToDepthTransform, colorPoint); CameraMath.Project(depthCameraMatrix, depthLensDistortion, depthPoint[0], depthPoint[1], depthPoint[2], out depthX, out depthY); int x = (int)depthX; // Y down, since we are indexing into an image int y = depthImageHeight - (int)depthY; if ((x >= 0) && (x < depthImageWidth) && (y >= 0) && (y < depthImageHeight)) { int z = depthImage[x, y]; if ((z != 0) && (z < s)) { found = true; } } } // convert back to Y down depthY = depthImageHeight - depthY; }
//Converts a 16-bit grayscale depth frame into a 32-bit frame public void CopyShortImageForGrayscaleDisplay(ShortImage shortImage, ushort maxVal) { ushort *pIn = shortImage.Data(0, 0); double multiplier = 255.0 / maxVal; ARGB32 *pOut = data; for (int i = 0; i < width * height; i++) { byte intensity = (byte)(multiplier * *pIn++);//(255 * *pIn++ / maxVal); pOut->A = 255; pOut->R = intensity; pOut->G = intensity; pOut++->B = intensity; } }
public unsafe void Decode(ByteImage[] capturedImages, ShortImage decodedX, ShortImage decodedY, ByteImage mask) { mask.Set(255); // cumulative across X and Y Decode(capturedImages, decodedX, mask, numXBits, width); // TODO: this is a little awkward var Yimages = new ByteImage[numYBits * 2]; for (int i = 0; i < numYBits * 2; i++) { Yimages[i] = capturedImages[numXBits * 2 + i]; } Decode(Yimages, decodedY, mask, numYBits, height); }
public void ColorImageToDepthImage(double colorX, double colorY, ShortImage depthImage, out Matrix depthPoint, out double depthX, out double depthY) { double xUndistorted, yUndistorted; // convert to color camera space float fx = (float)colorCameraMatrix[0, 0]; float fy = (float)colorCameraMatrix[1, 1]; float cx = (float)colorCameraMatrix[0, 2]; float cy = (float)colorCameraMatrix[1, 2]; float[] kappa = new float[] { (float)colorLensDistortion[0], (float)colorLensDistortion[1] }; // flip y because our calibration expects y up (right handed coordinates at all times) CameraMath.Undistort(fx, fy, cx, cy, kappa, colorX, (colorImageHeight - colorY), out xUndistorted, out yUndistorted); var colorToDepthTransform = new Matrix(4, 4); colorToDepthTransform.Inverse(depthToColorTransform); var colorPoint = new Matrix(4, 1); depthPoint = new Matrix(4, 1); depthX = 0; depthY = 0; // walk along ray in color camera bool found = false; for (int s = 400; (s < 4500) && !found; s++) // TODO: confirm these limits (mm) { // convert to a 3D point along ray, in meters colorPoint[0] = xUndistorted * s / 1000.0; colorPoint[1] = yUndistorted * s / 1000.0; colorPoint[2] = s / 1000.0; colorPoint[3] = 1; // transform to depth camera 3D point and project depthPoint.Mult(colorToDepthTransform, colorPoint); CameraMath.Project(depthCameraMatrix, depthLensDistortion, depthPoint[0], depthPoint[1], depthPoint[2], out depthX, out depthY); int x = (int)depthX; // Y down, since we are indexing into an image int y = depthImageHeight - (int)depthY; if ((x >= 0) && (x < depthImageWidth) && (y >= 0) && (y < depthImageHeight)) { int z = depthImage[x, y]; if ((z != 0) && (z < s)) found = true; } } // convert back to Y down depthY = depthImageHeight - depthY; }
public void Threshold(ShortImage a, ushort threshold) { ushort *pa = a.Data(0, 0); byte * p = data; for (int i = 0; i < width * height; i++) { if (*pa++ > threshold) { *p++ = 255; } else { *p++ = 0; } } }
public void ColorImageToDepthImage(int colorX, int colorY, ShortImage depthImage, System.Drawing.PointF[] colorFrameToCameraSpaceTable, out Matrix depthPoint, out double depthX, out double depthY) { double xUndistorted, yUndistorted; // convert to color camera space // use lookup table to perform undistortion; this will be faster when converting lots of points var point = colorFrameToCameraSpaceTable[colorY * Kinect2Calibration.colorImageWidth + colorX]; xUndistorted = point.X; yUndistorted = point.Y; var colorToDepthTransform = new Matrix(4, 4); colorToDepthTransform.Inverse(depthToColorTransform); var colorPoint = new Matrix(4, 1); depthPoint = new Matrix(4, 1); depthX = 0; depthY = 0; // walk along ray in color camera bool found = false; for (int s = 400; (s < 4500) && !found; s++) // TODO: confirm these limits (mm) { // convert to a 3D point along ray, in meters colorPoint[0] = xUndistorted * s / 1000.0; colorPoint[1] = yUndistorted * s / 1000.0; colorPoint[2] = s / 1000.0; colorPoint[3] = 1; // transform to depth camera 3D point and project depthPoint.Mult(colorToDepthTransform, colorPoint); CameraMath.Project(depthCameraMatrix, depthLensDistortion, depthPoint[0], depthPoint[1], depthPoint[2], out depthX, out depthY); int x = (int)depthX; // Y down, since we are indexing into an image int y = depthImageHeight - (int)depthY; if ((x >= 0) && (x < depthImageWidth) && (y >= 0) && (y < depthImageHeight)) { int z = depthImage[x, y]; if ((z != 0) && (z < s)) found = true; } } // convert back to Y down depthY = depthImageHeight - depthY; }
//this is a special function which respects the YUYV ordering when mirroring public void XMirror_YUYSpecial(ShortImage a) { //Y1 U Y2 V ---> Y2 U Y1 V byte *pOut = (byte *)data; byte *pIn = (byte *)a.data; for (int yy = 0; yy < height; yy++) { pIn = (byte *)a.Data(width - 2, yy); for (int xx = 0; xx < width; xx += 2) { *pOut++ = *(pIn + 2); *pOut++ = *(pIn + 1); *pOut++ = *pIn; *pOut++ = *(pIn + 3); pIn -= 4; } } }
/// <summary> /// n should be odd otherwise this function behaves as n = n+1 /// </summary> /// <param name="a"></param> /// <param name="n"></param> public void BlurNxNNonZero(ShortImage a, int n) { if (n % 2 == 0) { n++; } ushort *input; ushort *output; ushort *[] pbs = new ushort *[n]; int[] s = new int[n]; int[] c = new int[n]; int h, hc; int sumS, sumC; int lastElement = n - 1; for (int y = 0; y < (height - (n - 1)); y++) { input = a.Data((int)(n / 2), y + 1); output = this.Data((int)(n / 2), y + 1); for (int cnt = 0; cnt < n; cnt++) { pbs[cnt] = a.Data((n - 1), y + cnt); s[cnt] = 0; c[cnt] = 0; } h = 0; hc = 0; for (int x = 0; x < (width - (n - 1)); x++) { h -= s[0]; hc -= c[0]; int i = 0; for (i = 0; i < (n - 1); i++) { s[i] = s[i + 1]; c[i] = c[i + 1]; } sumS = 0; sumC = 0; for (i = 0; i < n; i++) { ushort bsi = *pbs[i]; sumC += ((bsi > 0) ? 1 : 0); sumS += bsi; pbs[i]++; } c[lastElement] = sumC; s[lastElement] = sumS; h += sumS; hc += sumC; int g = 0; if (hc > 0) { g = h / hc; } //if (g > ushort.MaxValue) // g = ushort.MaxValue; if (*input++ != (ushort)0) { *output++ = (ushort)g; } else { *output++ = (ushort)0; } } } }
public void Blur3x3NonZero(ShortImage a) { ushort *input; ushort *output; ushort *pb02; ushort *pb12; ushort *pb22; int s0, s1, s2; //pixel values int c0, c1, c2; //valid pixel counts (where value > 0) int h, hc; for (int y = 0; y < height - 2; y++) { input = a.Data(1, y + 1); output = this.Data(1, y + 1); pb02 = a.Data(2, y); pb12 = a.Data(2, y + 1); pb22 = a.Data(2, y + 2); h = 0; hc = 0; s0 = 0; s1 = 0; s2 = 0; c0 = 0; c1 = 0; c2 = 0; for (int x = 0; x < width - 2; x++) { h -= s0; hc -= c0; s0 = s1; s1 = s2; c0 = c1; c1 = c2; c2 = (((*pb02) > 0) ? 1 : 0) + (((*pb12) > 0) ? 1 : 0) + (((*pb22) > 0) ? 1 : 0); s2 = *pb02++ + *pb12++ + *pb22++; h += s2; hc += c2; int g = 0; if (hc > 0) { g = h / hc; } if (g > ushort.MaxValue) { g = ushort.MaxValue; } if (*input++ != (ushort)0) { *output++ = (ushort)g; //comment this check if you want to fill holes and smooth edges } else { *output++ = (ushort)0; } } } }
public static void LoadFromTiff(SharpDX.WIC.ImagingFactory imagingFactory, ShortImage image, string filename) { LoadFromTiff(imagingFactory, image, filename, 2); }
// BEWARE: threshold on absdiff, and mask level settings* public unsafe void Decode(ByteImage[] capturedImages, ShortImage decoded, ByteImage mask, int nBits, int max) { decoded.Zero(); int capturedWidth = decoded.Width; int capturedHeight = decoded.Height; // stores decoded bit from previous level var bits = new ByteImage(capturedWidth, capturedHeight); for (int i = 0; i < nBits; i++) { var capturedImage = capturedImages[2 * i]; var invertedCapturedImage = capturedImages[2 * i + 1]; int bitValue = (int)Math.Pow(2.0, nBits - i - 1); ushort *decodedp = decoded.Data(0, 0); byte * capturedp = capturedImage.Data(0, 0); byte * invertedp = invertedCapturedImage.Data(0, 0); byte * maskp = mask.Data(0, 0); byte * bitsp = bits.Data(0, 0); for (int y = 0; y < capturedHeight; y++) { for (int x = 0; x < capturedWidth; x++) { // a bit is considered valid if the diff is greater than some threshold; this value is tricky to set given AGC byte valid = (Math.Abs(*capturedp - *invertedp) > 10) ? (byte)255 : (byte)0; byte bit = (*capturedp >= *invertedp) ? (byte)255 : (byte)0; // Gray code bit *bitsp = (byte)(bit ^ *bitsp); if (*bitsp == 255) { *decodedp = (ushort)(*decodedp + bitValue); } // stop updating the mask for the least significant levels (but continue decoding) // *FIX: this is pretty fragile, perhaps better to record how many bits of rows and column have been recorded and walk back from that if (i < nBits - 4) { *maskp = (byte)(valid & (*maskp)); } decodedp++; capturedp++; invertedp++; maskp++; bitsp++; } } } bits.Dispose(); // check that decoded values are within range for (int y = 0; y < capturedHeight; y++) { for (int x = 0; x < capturedWidth; x++) { int d = decoded[x, y]; // can this be negative? if ((d >= max) || (d < 0)) { mask[x, y] = 0; } } } }
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 void DecodeGrayCodeImages(string directory) { stopWatch.Start(); // decode Gray code captures foreach (var projector in projectors) { string projectorDirectory = directory + "/projector" + projector.name; var grayCode = new GrayCode(projector.width, projector.height); // allocate space for captured images int nCapturedImages = 2 * (grayCode.numXBits + grayCode.numYBits); // varies by projector var capturedImages = new ByteImage[nCapturedImages]; for (int i = 0; i < nCapturedImages; i++) // varies by projector capturedImages[i] = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); foreach (var camera in cameras) { Console.WriteLine("decoding Gray code images for projector " + projector.name + ", camera " + camera.name); string cameraDirectory = projectorDirectory + "/camera" + camera.name; // load and decode Gray code images for (int i = 0; i < nCapturedImages; i++) LoadFromTiff(imagingFactory, capturedImages[i], cameraDirectory + "/grayCode" + i + ".tiff"); var decodedColumns = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var decodedRows = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var mask = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); // TODO: there are a couple of interesting thresholds in Decode; they should be surfaced here grayCode.Decode(capturedImages, decodedColumns, decodedRows, mask); //Console.WriteLine("saving camera " + camera.displayName); SaveToTiff(imagingFactory, decodedColumns, cameraDirectory + "/decodedColumns.tiff"); SaveToTiff(imagingFactory, decodedRows, cameraDirectory + "/decodedRows.tiff"); SaveToTiff(imagingFactory, mask, cameraDirectory + "/mask.tiff"); var decodedColumnsMasked = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var decodedRowsMasked = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); for (int y = 0; y < Kinect2Calibration.colorImageHeight; y++) for (int x = 0; x < Kinect2Calibration.colorImageWidth; x++) { if (mask[x, y] > 0) { decodedColumnsMasked[x, y] = decodedColumns[x, y]; decodedRowsMasked[x, y] = decodedRows[x, y]; } else { decodedColumnsMasked[x, y] = 0; decodedRowsMasked[x, y] = 0; } } SaveToTiff(imagingFactory, decodedColumnsMasked, cameraDirectory + "/decodedColumnsMasked.tiff"); SaveToTiff(imagingFactory, decodedRowsMasked, cameraDirectory + "/decodedRowsMasked.tiff"); } } Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); }
public static void SaveToTiff(SharpDX.WIC.ImagingFactory imagingFactory, ShortImage image, string filename) { SaveToTiff(imagingFactory, image, filename, SharpDX.WIC.PixelFormat.Format16bppGray, 2); }
public void CalibrateProjectorGroups(string directory) { // for all cameras, take depth image points to color image points var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); var validMask = new ByteImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); foreach (var camera in cameras) { Console.WriteLine("projecting depth points to color camera " + camera.name); // load depth image string cameraDirectory = directory + "/camera" + camera.name; depthImage.LoadFromFile(cameraDirectory + "/mean.bin"); varianceImage.LoadFromFile(cameraDirectory + "/variance.bin"); validMask.Zero(); var calibration = camera.calibration; var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable(); // TODO: consider using just one 4x4 in calibration class var colorCamera = new Matrix(4, 1); camera.colorImagePoints = new List<Matrix>(); camera.depthCameraPoints = new List<Matrix>(); var depthCamera4 = new Matrix(4, 1); // for each valid point in depth image int numRejected = 0; for (int y = 0; y < Kinect2Calibration.depthImageHeight; y += 1) for (int x = 0; x < Kinect2Calibration.depthImageWidth; x += 1) { float depth = depthImage[x, y] / 1000f; // m float variance = varianceImage[x, y]; if (depth == 0) continue; if (variance > 6 * 6) { numRejected++; continue; } validMask[x, y] = (byte)255; // convert to depth camera space var point = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x]; depthCamera4[0] = point.X * depth; depthCamera4[1] = point.Y * depth; depthCamera4[2] = depth; depthCamera4[3] = 1; // convert to color camera space colorCamera.Mult(calibration.depthToColorTransform, depthCamera4); //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); if ((colorU >= 0) && (colorU < (Kinect2Calibration.colorImageWidth - 1)) && (colorV >= 0) && (colorV < (Kinect2Calibration.colorImageHeight - 1))) // BEWARE: later do we round or truncate?? { var colorImagePoint = new Matrix(2, 1); colorImagePoint[0] = colorU; colorImagePoint[1] = colorV; camera.colorImagePoints.Add(colorImagePoint); // expect a 3-vector? var depthCamera = new Matrix(3, 1); depthCamera[0] = depthCamera4[0]; depthCamera[1] = depthCamera4[1]; depthCamera[2] = depthCamera4[2]; camera.depthCameraPoints.Add(depthCamera); //Console.WriteLine(depthCamera[0] + "\t" + depthCamera[1] + "\t -> " + colorImagePoint[0] + "\t" + colorImagePoint[1]); } } SaveToTiff(imagingFactory, validMask, cameraDirectory + "/validMask.tiff"); Console.WriteLine("rejected " + 100 * (float)numRejected / (float)(Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight) + "% pixels for high variance"); } // we never save colorImagePoints, depthCameraPoints, so we must remember to run previous Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); // use decoded Gray code images to create calibration point sets foreach (var projector in projectors) { string projectorDirectory = directory + "/projector" + projector.name; projector.calibrationPointSets = new Dictionary<Camera, CalibrationPointSet>(); foreach (var camera in cameras) { string cameraDirectory = projectorDirectory + "/camera" + camera.name; var decodedColumns = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var decodedRows = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var mask = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); LoadFromTiff(imagingFactory, decodedColumns, cameraDirectory + "/decodedColumns.tiff"); LoadFromTiff(imagingFactory, decodedRows, cameraDirectory + "/decodedRows.tiff"); LoadFromTiff(imagingFactory, mask, cameraDirectory + "/mask.tiff"); // we have a bunch of color camera / depth camera point corrspondences // use the Gray code to find the position of the color camera point in the projector frame // find 2D projector coordinates from decoded Gray code images var imagePoints = new List<System.Drawing.PointF>(); var worldPoints = new List<Matrix>(); for (int i = 0; i < camera.colorImagePoints.Count; i++) { var colorImagePoint = camera.colorImagePoints[i]; // We would like to relate projected color points to color images stored in memory. // The Kinect SDK and our camera calibration assumes X left, Y up (from the POV of the camera). // We index images in memory with X right and Y down. // Our Gray code images are flipped in the horizontal direction. // Therefore to map an image space coordinate to a memory location we flip Y (and not X): int x = (int)(colorImagePoint[0] + 0.5f); int y = Kinect2Calibration.colorImageHeight - (int)(colorImagePoint[1] + 0.5f); if ((x < 0) || (x >= Kinect2Calibration.colorImageWidth) || (y < 0) || (y >= Kinect2Calibration.colorImageHeight)) { //Console.WriteLine("out of bounds"); continue; } if (mask[x, y] > 0) // Gray code is valid { // We would like to relate decoded row/column values to projector coordinates. // To match the camera, we want projector's coordinate system X left, Y up (from the POV of the projector). // We assume that the projector is configured in front projection mode (i.e., projected text looks correct in the real world). // In that case decoded columns run X right (in the real world), decoded rows run Y down (in the real world). // So we need to flip both X and Y decoded values. var projectorImagePoint = new System.Drawing.PointF(projector.width - decodedColumns[x, y], projector.height - decodedRows[x, y]); var depthCameraPoint = camera.depthCameraPoints[i]; imagePoints.Add(projectorImagePoint); worldPoints.Add(depthCameraPoint); //Console.WriteLine(depthCameraPoint[0] + "\t" + depthCameraPoint[1] + "\t" + depthCameraPoint[2] + "-> \t" + x + "\t" + y + "-> \t" + projectorImagePoint.X + "\t" + projectorImagePoint.Y); } } if (worldPoints.Count > 1000) { var pointSet = new CalibrationPointSet(); pointSet.worldPoints = worldPoints; pointSet.imagePoints = imagePoints; projector.calibrationPointSets[camera] = pointSet; Console.WriteLine("projector " + projector.name + " is seen by camera " + camera.name + " (" + worldPoints.Count + " points)"); } } } Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); // calibration foreach (var projector in projectors) { Console.WriteLine("calibrating projector " + projector.name); string projectorDirectory = directory + "/projector" + projector.name; // RANSAC double minError = Double.PositiveInfinity; var random = new Random(0); // provide seed to ease debugging int numCompletedFits = 0; for (int i = 0; (numCompletedFits < 4) && (i < 10); i++) { Console.WriteLine("RANSAC iteration " + i); // randomly select small number of points from each calibration set var worldPointSubsets = new List<List<Matrix>>(); var imagePointSubsets = new List<List<System.Drawing.PointF>>(); bool foundNonplanarSubset = false; foreach (var pointSet in projector.calibrationPointSets.Values) { var worldPointSubset = new List<Matrix>(); var imagePointSubset = new List<System.Drawing.PointF>(); // try to find a nonplanar subset bool planar = true; int nTries = 0; while (planar && (nTries++ < 1000)) { worldPointSubset.Clear(); imagePointSubset.Clear(); for (int j = 0; j < 100; j++) { int k = random.Next(pointSet.worldPoints.Count); worldPointSubset.Add(pointSet.worldPoints[k]); imagePointSubset.Add(pointSet.imagePoints[k]); } // planar? Matrix Rplane, tplane, d; CameraMath.PlaneFit(worldPointSubset, out Rplane, out tplane, out d); //Console.WriteLine("planar : " + d[2] / d[1]); planar = (d[2] / d[1]) < 0.001f; } worldPointSubsets.Add(worldPointSubset); imagePointSubsets.Add(imagePointSubset); // we can't initialize extrinsics yet, because we don't know which intrinsics we'll be using if (!planar) foundNonplanarSubset = true; } // we do not optimize intrinsics if all the point sets are planar, or if the projector intrinsics are marked as locked bool fixIntrinsics = (!foundNonplanarSubset) || (projector.lockIntrinsics); // TODO: add option to lock intrinsics var rotations = new List<Matrix>(); var translations = new List<Matrix>(); var cameraMatrix = new Matrix(3, 3); var distCoeffs = new Matrix(2, 1); if (fixIntrinsics) { cameraMatrix.Copy(projector.cameraMatrix); distCoeffs.Copy(projector.lensDistortion); } else // nonplanar, so we can optimize intrinsics { cameraMatrix[0, 0] = 1000; //fx TODO: can we instead init this from FOV? cameraMatrix[1, 1] = 1000; //fy cameraMatrix[0, 2] = projector.width / 2; //cx cameraMatrix[1, 2] = 0; // projector lens shift; note this assumes desktop projection mode cameraMatrix[2, 2] = 1; } // init extrinsics for (int ii = 0; ii < worldPointSubsets.Count; ii++) { Matrix R, t; CameraMath.ExtrinsicsInit(cameraMatrix, distCoeffs, worldPointSubsets[ii], imagePointSubsets[ii], out R, out t); rotations.Add(CameraMath.RotationVectorFromRotationMatrix(R)); translations.Add(t); } // initial RANSAC fit on subset of points double error; if (fixIntrinsics) error = CameraMath.CalibrateCameraExtrinsicsOnly(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations); else error = CameraMath.CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations); Console.WriteLine("error on subset = " + error); // RANSAC: find inliers from overall dataset var worldPointInlierSets = new List<List<Matrix>>(); var imagePointInlierSets = new List<List<System.Drawing.PointF>>(); int setIndex = 0; bool enoughInliers = true; double sumError = 0; int pointsInSum = 0; int totalInliers = 0; int totalPoints = 0; foreach (var pointSet in projector.calibrationPointSets.Values) { var worldPointInlierSet = new List<Matrix>(); var imagePointInlierSet = new List<System.Drawing.PointF>(); var R = CameraMath.RotationMatrixFromRotationVector(rotations[setIndex]); var t = translations[setIndex]; var p = new Matrix(3, 1); for (int k = 0; k < pointSet.worldPoints.Count; k++) { p.Mult(R, pointSet.worldPoints[k]); p.Add(t); double u, v; CameraMath.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v); double dx = pointSet.imagePoints[k].X - u; double dy = pointSet.imagePoints[k].Y - v; double thisError = Math.Sqrt((dx * dx) + (dy * dy)); if (thisError < 2.0f) // TODO: how to set this? { worldPointInlierSet.Add(pointSet.worldPoints[k]); imagePointInlierSet.Add(pointSet.imagePoints[k]); } sumError += thisError * thisError; pointsInSum++; } setIndex++; // require that each view has a minimum number of inliers enoughInliers = enoughInliers && (worldPointInlierSet.Count > 500); // should be related to min number of points in set (above) totalPoints += pointSet.worldPoints.Count; totalInliers += worldPointInlierSet.Count; worldPointInlierSets.Add(worldPointInlierSet); imagePointInlierSets.Add(imagePointInlierSet); } Console.WriteLine("{0}/{1} inliers", totalInliers, totalPoints); // if number of inliers > some threshold (should be for each subset) if (enoughInliers) // should this threshold be a function of the number of cameras, a percentage? { double error2; if (fixIntrinsics) error2 = CameraMath.CalibrateCameraExtrinsicsOnly(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations); else error2 = CameraMath.CalibrateCamera(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations); Console.WriteLine("error with inliers = " + error2); Console.Write("camera matrix = \n" + cameraMatrix); numCompletedFits++; // if reduced error save model (save rotation and translation to calibrationPointSets, cameraMatrix and distortion coeffs to projector) if (error2 < minError) { minError = error2; projector.cameraMatrix = cameraMatrix; projector.lensDistortion = distCoeffs; setIndex = 0; foreach (var pointSet in projector.calibrationPointSets.Values) { // convert to 4x4 transform var R = CameraMath.RotationMatrixFromRotationVector(rotations[setIndex]); var t = translations[setIndex]; var T = new Matrix(4, 4); T.Identity(); for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) T[ii, jj] = R[ii, jj]; T[ii, 3] = t[ii]; } pointSet.pose = T; pointSet.worldPointInliers = worldPointInlierSets[setIndex]; pointSet.imagePointInliers = imagePointInlierSets[setIndex]; setIndex++; } } } } if (numCompletedFits == 0) throw new CalibrationFailedException("Unable to successfully calibrate projector: " + projector.name); Console.WriteLine("final calibration:"); Console.Write("camera matrix = \n" + projector.cameraMatrix); Console.Write("distortion = \n" + projector.lensDistortion); Console.WriteLine("error = " + minError); foreach (var camera in projector.calibrationPointSets.Keys) { Console.WriteLine("camera " + camera.name + " pose:"); Console.Write(projector.calibrationPointSets[camera].pose); } } Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); //Console.WriteLine("x = ["); //for (int ii = 0; ii < imagePointSubsets[0].Count; ii++) // Console.WriteLine("{0} {1}", imagePointSubsets[0][ii].X, imagePointSubsets[0][ii].Y); //Console.WriteLine("]';"); //Console.WriteLine("X = ["); //for (int ii = 0; ii < worldPointSubsets[0].Count; ii++) // Console.WriteLine("{0} {1} {2}", worldPointSubsets[0][ii][0], worldPointSubsets[0][ii][1], worldPointSubsets[0][ii][2]); //Console.WriteLine("]';"); //Console.WriteLine("fc = [{0} {1}];", projector.cameraMatrix[0, 0], projector.cameraMatrix[1, 1]); //Console.WriteLine("cc = [{0} {1}];", projector.cameraMatrix[0, 2], projector.cameraMatrix[1, 2]); //Matrix thisR, thist; //{ // Matrix Rplane, tplane; // CameraMath.PlaneFit(worldPointSubsets[0], out Rplane, out tplane); // CameraMath.PlanarDLT(projector.cameraMatrix, projector.lensDistortion, worldPointSubsets[0], imagePointSubsets[0], Rplane, tplane, out thisR, out thist); // //Console.WriteLine("DLT---------"); // //Console.WriteLine(thisR); // //Console.WriteLine(thist); //} //// if pattern is not planar, we can recover projector intrinsics //List<RoomAliveToolkit.Matrix> rotations = null; //List<RoomAliveToolkit.Matrix> translations = null; //var error = CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations); //Console.WriteLine("error = " + error); // we check whether each view is planar, so that we can use the correct version of DLT // the overall set may not be planar however, so we have to check the union of points // if overall set is planar, leave intrinsics alone // }
public void Copy(ShortImage shortImage) { ushort* p = shortImage.Data(0, 0); ARGB32* pOut = data; for (int i = 0; i < width * height; i++) { pOut->A = 255; pOut->R = (byte)*p; pOut->G = (byte)*p; pOut++->B = (byte)*p++; } }
public unsafe void Decode(ByteImage[] capturedImages, ShortImage decodedX, ShortImage decodedY, ByteImage mask) { mask.Set(255); // cumulative across X and Y Decode(capturedImages, decodedX, mask, numXBits, width); // TODO: this is a little awkward var Yimages = new ByteImage[numYBits*2]; for (int i = 0; i < numYBits*2; i++) Yimages[i] = capturedImages[numXBits * 2 + i]; Decode(Yimages, decodedY, mask, numYBits, height); }
public void CalibrateProjectorGroups(string directory) { // for all cameras, take depth image points to color image points var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); var validMask = new ByteImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); foreach (var camera in cameras) { Console.WriteLine("projecting depth points to color camera " + camera.name); // load depth image string cameraDirectory = directory + "/camera" + camera.name; depthImage.LoadFromFile(cameraDirectory + "/mean.bin"); varianceImage.LoadFromFile(cameraDirectory + "/variance.bin"); validMask.Zero(); var calibration = camera.calibration; var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable(); // TODO: consider using just one 4x4 in calibration class var colorCamera = new Matrix(4, 1); camera.colorImagePoints = new List<Matrix>(); camera.depthCameraPoints = new List<Matrix>(); var depthCamera4 = new Matrix(4, 1); // for each valid point in depth image int numRejected = 0; for (int y = 0; y < Kinect2Calibration.depthImageHeight; y += 1) for (int x = 0; x < Kinect2Calibration.depthImageWidth; x += 1) { float depth = depthImage[x, y] / 1000f; // m float variance = varianceImage[x, y]; if (depth == 0) continue; if (variance > 6 * 6) { numRejected++; continue; } validMask[x, y] = (byte)255; // convert to depth camera space var point = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x]; depthCamera4[0] = point.X * depth; depthCamera4[1] = point.Y * depth; depthCamera4[2] = depth; depthCamera4[3] = 1; // convert to color camera space colorCamera.Mult(calibration.depthToColorTransform, depthCamera4); //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); if ((colorU >= 0) && (colorU < (Kinect2Calibration.colorImageWidth - 1)) && (colorV >= 0) && (colorV < (Kinect2Calibration.colorImageHeight - 1))) // BEWARE: later do we round or truncate?? { var colorImagePoint = new Matrix(2, 1); colorImagePoint[0] = colorU; colorImagePoint[1] = colorV; camera.colorImagePoints.Add(colorImagePoint); // expect a 3-vector? var depthCamera = new Matrix(3, 1); depthCamera[0] = depthCamera4[0]; depthCamera[1] = depthCamera4[1]; depthCamera[2] = depthCamera4[2]; camera.depthCameraPoints.Add(depthCamera); //Console.WriteLine(depthCamera[0] + "\t" + depthCamera[1] + "\t -> " + colorImagePoint[0] + "\t" + colorImagePoint[1]); } } SaveToTiff(imagingFactory, validMask, cameraDirectory + "/validMask.tiff"); Console.WriteLine("rejected " + 100 * (float)numRejected / (float)(Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight) + "% pixels for high variance"); } // we never save colorImagePoints, depthCameraPoints, so we must remember to run previous Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); // use decoded Gray code images to create calibration point sets foreach (var projector in projectors) { string projectorDirectory = directory + "/projector" + projector.name; projector.calibrationPointSets = new Dictionary<Camera, CalibrationPointSet>(); foreach (var camera in cameras) { string cameraDirectory = projectorDirectory + "/camera" + camera.name; var decodedColumns = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var decodedRows = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); var mask = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); LoadFromTiff(imagingFactory, decodedColumns, cameraDirectory + "/decodedColumns.tiff"); LoadFromTiff(imagingFactory, decodedRows, cameraDirectory + "/decodedRows.tiff"); LoadFromTiff(imagingFactory, mask, cameraDirectory + "/mask.tiff"); // we have a bunch of color camera / depth camera point corrspondences // use the Gray code to find the position of the color camera point in the projector frame // find 2D projector coordinates from decoded Gray code images var imagePoints = new List<System.Drawing.PointF>(); var worldPoints = new List<Matrix>(); for (int i = 0; i < camera.colorImagePoints.Count; i++) { var colorImagePoint = camera.colorImagePoints[i]; // We would like to relate projected color points to color images stored in memory. // The Kinect SDK and our camera calibration assumes X left, Y up (from the POV of the camera). // We index images in memory with X right and Y down. // Our Gray code images are flipped in the horizontal direction. // Therefore to map an image space coordinate to a memory location we flip Y (and not X): int x = (int)(colorImagePoint[0] + 0.5f); int y = Kinect2Calibration.colorImageHeight - (int)(colorImagePoint[1] + 0.5f); if ((x < 0) || (x >= Kinect2Calibration.colorImageWidth) || (y < 0) || (y >= Kinect2Calibration.colorImageHeight)) { //Console.WriteLine("out of bounds"); continue; } if (mask[x, y] > 0) // Gray code is valid { // We would like to relate decoded row/column values to projector coordinates. // To match the camera, we want projector's coordinate system X left, Y up (from the POV of the projector). // We assume that the projector is configured in front projection mode (i.e., projected text looks correct in the real world). // In that case decoded columns run X right (in the real world), decoded rows run Y down (in the real world). // So we need to flip both X and Y decoded values. var projectorImagePoint = new System.Drawing.PointF(projector.width - decodedColumns[x, y], projector.height - decodedRows[x, y]); var depthCameraPoint = camera.depthCameraPoints[i]; imagePoints.Add(projectorImagePoint); worldPoints.Add(depthCameraPoint); //Console.WriteLine(depthCameraPoint[0] + "\t" + depthCameraPoint[1] + "\t" + depthCameraPoint[2] + "-> \t" + x + "\t" + y + "-> \t" + projectorImagePoint.X + "\t" + projectorImagePoint.Y); } } if (worldPoints.Count > 1000) { var pointSet = new CalibrationPointSet(); pointSet.worldPoints = worldPoints; pointSet.imagePoints = imagePoints; projector.calibrationPointSets[camera] = pointSet; Console.WriteLine("projector " + projector.name + " is seen by camera " + camera.name + " (" + worldPoints.Count + " points)"); } } } Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); // calibration foreach (var projector in projectors) { Console.WriteLine("calibrating projector " + projector.name); string projectorDirectory = directory + "/projector" + projector.name; // RANSAC double minError = Double.PositiveInfinity; var random = new Random(0); // provide seed to ease debugging int numCompletedFits = 0; for (int i = 0; (numCompletedFits < 4) && (i < 10); i++) { Console.WriteLine("RANSAC iteration " + i); // randomly select small number of points from each calibration set var worldPointSubsets = new List<List<Matrix>>(); var imagePointSubsets = new List<List<System.Drawing.PointF>>(); foreach (var pointSet in projector.calibrationPointSets.Values) { var worldPointSubset = new List<Matrix>(); var imagePointSubset = new List<System.Drawing.PointF>(); bool nonCoplanar = false; int nTries = 0; while (!nonCoplanar) { for (int j = 0; j < 100; j++) { int k = random.Next(pointSet.worldPoints.Count); worldPointSubset.Add(pointSet.worldPoints[k]); imagePointSubset.Add(pointSet.imagePoints[k]); } // check that points are not coplanar Matrix X; double D; double ssdToPlane = PlaneFit(worldPointSubset, out X, out D); int numOutliers = 0; foreach (var point in worldPointSubset) { double distanceFromPlane = X.Dot(point) + D; if (Math.Abs(distanceFromPlane) > 0.1f) numOutliers++; } nonCoplanar = (numOutliers > worldPointSubset.Count * 0.10f); if (!nonCoplanar) { Console.WriteLine("points are coplanar (try #{0})", nTries); worldPointSubset.Clear(); imagePointSubset.Clear(); } if (nTries++ > 1000) { throw new CalibrationFailedException("Unable to find noncoplanar points."); // consider moving this check up with variance check (when calibration point sets are formed) } } worldPointSubsets.Add(worldPointSubset); imagePointSubsets.Add(imagePointSubset); } var cameraMatrix = new Matrix(3, 3); cameraMatrix[0, 0] = 1000; //fx TODO: can we instead init this from FOV? cameraMatrix[1, 1] = 1000; //fy cameraMatrix[0, 2] = projector.width / 2; //cx cameraMatrix[1, 2] = 0; // projector lens shift; note this assumes desktop projection mode cameraMatrix[2, 2] = 1; var distCoeffs = new RoomAliveToolkit.Matrix(2, 1); List<RoomAliveToolkit.Matrix> rotations = null; List<RoomAliveToolkit.Matrix> translations = null; var error = CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations); Console.WriteLine("error = " + error); //Console.WriteLine("intrinsics = \n" + cameraMatrix); //// we differ from opencv's 'error' in that we do not distinguish between x and y. //// i.e. opencv uses the method below; this number would match if we used pointsInSum2*2 in the divisor. //// double check opencv's error //{ // double sumError2 = 0; // int pointsInSum2 = 0; // for (int ii = 0; ii < worldPointSubsets.Count; ii++) // { // var R = Orientation.Rodrigues(rotations[ii]); // var t = translations[ii]; // var p = new Matrix(3, 1); // var worldPointSet = worldPointSubsets[ii]; // var imagePointSet = imagePointSubsets[ii]; // for (int k = 0; k < worldPointSet.Count; k++) // { // p.Mult(R, worldPointSet[k]); // p.Add(t); // double u, v; // Kinect2.Kinect2Calibration.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v); // double dx = imagePointSet[k].X - u; // double dy = imagePointSet[k].Y - v; // double thisError = dx * dx + dy * dy; // sumError2 += thisError; // pointsInSum2++; // } // } // // opencv's error is rms but over both x and y combined // Console.WriteLine("average projection error = " + Math.Sqrt(sumError2 / (float)(pointsInSum2))); //} // find inliers from overall dataset var worldPointInlierSets = new List<List<Matrix>>(); var imagePointInlierSets = new List<List<System.Drawing.PointF>>(); int setIndex = 0; bool enoughInliers = true; double sumError = 0; int pointsInSum = 0; foreach (var pointSet in projector.calibrationPointSets.Values) { var worldPointInlierSet = new List<Matrix>(); var imagePointInlierSet = new List<System.Drawing.PointF>(); //var R = Vision.Orientation.Rodrigues(rotations[setIndex]); var R = RotationMatrixFromRotationVector(rotations[setIndex]); var t = translations[setIndex]; var p = new Matrix(3, 1); for (int k = 0; k < pointSet.worldPoints.Count; k++) { p.Mult(R, pointSet.worldPoints[k]); p.Add(t); double u, v; CameraMath.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v); double dx = pointSet.imagePoints[k].X - u; double dy = pointSet.imagePoints[k].Y - v; double thisError = Math.Sqrt((dx * dx) + (dy * dy)); if (thisError < 1.0f) { worldPointInlierSet.Add(pointSet.worldPoints[k]); imagePointInlierSet.Add(pointSet.imagePoints[k]); } sumError += thisError * thisError; pointsInSum++; } setIndex++; // require that each view has a minimum number of inliers enoughInliers = enoughInliers && (worldPointInlierSet.Count > 1000); worldPointInlierSets.Add(worldPointInlierSet); imagePointInlierSets.Add(imagePointInlierSet); } // if number of inliers > some threshold (should be for each subset) if (enoughInliers) // should this threshold be a function of the number of cameras, a percentage? { var error2 = CalibrateCamera(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations); Console.WriteLine("error with inliers = " + error2); Console.Write("camera matrix = \n" + cameraMatrix); numCompletedFits++; // if err < besterr save model (save rotation and translation to calibrationPointSets, cameraMatrix and distortion coeffs to projector) if (error < minError) { minError = error; projector.cameraMatrix = cameraMatrix; projector.lensDistortion = distCoeffs; setIndex = 0; foreach (var pointSet in projector.calibrationPointSets.Values) { // convert to 4x4 transform var R = RotationMatrixFromRotationVector(rotations[setIndex]); var t = translations[setIndex]; var T = new Matrix(4, 4); T.Identity(); for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) T[ii, jj] = R[ii, jj]; T[ii, 3] = t[ii]; } pointSet.pose = T; pointSet.worldPointInliers = worldPointInlierSets[setIndex]; pointSet.imagePointInliers = imagePointInlierSets[setIndex]; setIndex++; } } } } if (numCompletedFits == 0) throw new CalibrationFailedException("Unable to successfully calibrate projector: " + projector.name); Console.WriteLine("final calibration:"); Console.Write("camera matrix = \n" + projector.cameraMatrix); Console.Write("distortion = \n" + projector.lensDistortion); Console.WriteLine("error = " + minError); foreach (var camera in projector.calibrationPointSets.Keys) { Console.WriteLine("camera " + camera.name + " pose:"); Console.Write(projector.calibrationPointSets[camera].pose); } } Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds); }
public void Threshold(ShortImage a, ushort threshold) { ushort* pa = a.Data(0, 0); byte* p = data; for (int i = 0; i < width * height; i++) { if (*pa++ > threshold) *p++ = 255; else *p++ = 0; } }
public void Copy(ShortImage a, int shift) { ushort* pa = a.Data(0, 0); byte* p = data; for (int i = 0; i < width * height; i++) *p++ = (byte)(*pa++ >> shift); }
public void Blur5x5NonZero(ShortImage a) { ushort *input; ushort *output; ushort *pb04; ushort *pb14; ushort *pb24; ushort *pb34; ushort *pb44; int s0, s1, s2, s3, s4; //pixel values int c0, c1, c2, c3, c4; //valid pixel counts (where value > 0) int h, hc; for (int y = 0; y < height - 4; y++) { input = a.Data(2, y + 1); output = this.Data(2, y + 1); pb04 = a.Data(4, y); pb14 = a.Data(4, y + 1); pb24 = a.Data(4, y + 2); pb34 = a.Data(4, y + 3); pb44 = a.Data(4, y + 4); h = 0; hc = 0; s0 = 0; s1 = 0; s2 = 0; s3 = 0; s4 = 0; c0 = 0; c1 = 0; c2 = 0; c3 = 0; c4 = 0; for (int x = 0; x < width - 4; x++) { h -= s0; hc -= c0; s0 = s1; s1 = s2; s2 = s3; s3 = s4; c0 = c1; c1 = c2; c2 = c3; c3 = c4; c4 = (((*pb04) > 0) ? 1 : 0) + (((*pb14) > 0) ? 1 : 0) + (((*pb24) > 0) ? 1 : 0) + (((*pb34) > 0) ? 1 : 0) + (((*pb44) > 0) ? 1 : 0); s4 = *pb04++ + *pb14++ + *pb24++ + *pb34++ + *pb44++; h += s4; hc += c4; int g = 0; if (hc > 0) { g = h / hc; } //if (g > ushort.MaxValue) // g = ushort.MaxValue; if (*input++ != (ushort)0) { *output++ = (ushort)g; } else { *output++ = (ushort)0; } } } }
// BEWARE: threshold on absdiff, and mask level settings* public unsafe void Decode(ByteImage[] capturedImages, ShortImage decoded, ByteImage mask, int nBits, int max) { decoded.Zero(); int capturedWidth = decoded.Width; int capturedHeight = decoded.Height; // stores decoded bit from previous level var bits = new ByteImage(capturedWidth, capturedHeight); for (int i = 0; i < nBits; i++) { var capturedImage = capturedImages[2 * i]; var invertedCapturedImage = capturedImages[2 * i + 1]; int bitValue = (int)Math.Pow(2.0, nBits - i - 1); ushort* decodedp = decoded.Data(0, 0); byte* capturedp = capturedImage.Data(0, 0); byte* invertedp = invertedCapturedImage.Data(0, 0); byte* maskp = mask.Data(0, 0); byte* bitsp = bits.Data(0, 0); for (int y = 0; y < capturedHeight; y++) for (int x = 0; x < capturedWidth; x++) { // a bit is considered valid if the diff is greater than some threshold; this value is tricky to set given AGC byte valid = (Math.Abs(*capturedp - *invertedp) > 10) ? (byte)255 : (byte)0; byte bit = (*capturedp >= *invertedp) ? (byte)255 : (byte)0; // Gray code bit *bitsp = (byte)(bit ^ *bitsp); if (*bitsp == 255) *decodedp = (ushort)(*decodedp + bitValue); // stop updating the mask for the least significant levels (but continue decoding) // *FIX: this is pretty fragile, perhaps better to record how many bits of rows and column have been recorded and walk back from that if (i < nBits - 4) *maskp = (byte)(valid & (*maskp)); decodedp++; capturedp++; invertedp++; maskp++; bitsp++; } } bits.Dispose(); // check that decoded values are within range for (int y = 0; y < capturedHeight; y++) for (int x = 0; x < capturedWidth; x++) { int d = decoded[x, y]; // can this be negative? if ((d >= max) || (d < 0)) mask[x, y] = 0; } }
// encapsulates d3d resources for a camera public CameraDeviceResource(SharpDX.Direct3D11.Device device, ProjectorCameraEnsemble.Camera camera, Object renderLock, string directory) { this.device = device; this.camera = camera; this.renderLock = renderLock; // Kinect depth image var depthImageTextureDesc = new Texture2DDescription() { Width = 512, Height = 424, MipLevels = 1, ArraySize = 1, Format = SharpDX.DXGI.Format.R16_UInt, SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0), Usage = ResourceUsage.Dynamic, BindFlags = BindFlags.ShaderResource, CpuAccessFlags = CpuAccessFlags.Write, }; depthImageTexture = new Texture2D(device, depthImageTextureDesc); depthImageTextureRV = new ShaderResourceView(device, depthImageTexture); var floatDepthImageTextureDesc = new Texture2DDescription() { Width = 512, Height = 424, MipLevels = 1, ArraySize = 1, Format = SharpDX.DXGI.Format.R32_Float, SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0), Usage = ResourceUsage.Default, BindFlags = BindFlags.RenderTarget | BindFlags.ShaderResource, CpuAccessFlags = CpuAccessFlags.None, }; floatDepthImageTexture = new Texture2D(device, floatDepthImageTextureDesc); floatDepthImageRV = new ShaderResourceView(device, floatDepthImageTexture); floatDepthImageRenderTargetView = new RenderTargetView(device, floatDepthImageTexture); floatDepthImageTexture2 = new Texture2D(device, floatDepthImageTextureDesc); floatDepthImageRV2 = new ShaderResourceView(device, floatDepthImageTexture2); floatDepthImageRenderTargetView2 = new RenderTargetView(device, floatDepthImageTexture2); // Kinect color image var colorImageStagingTextureDesc = new Texture2DDescription() { Width = colorImageWidth, Height = colorImageHeight, MipLevels = 1, ArraySize = 1, Format = SharpDX.DXGI.Format.B8G8R8A8_UNorm, SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0), Usage = ResourceUsage.Dynamic, BindFlags = BindFlags.ShaderResource, CpuAccessFlags = CpuAccessFlags.Write }; colorImageStagingTexture = new Texture2D(device, colorImageStagingTextureDesc); var colorImageTextureDesc = new Texture2DDescription() { Width = colorImageWidth, Height = colorImageHeight, MipLevels = 0, ArraySize = 1, Format = SharpDX.DXGI.Format.B8G8R8A8_UNorm, SampleDescription = new SharpDX.DXGI.SampleDescription(1, 0), Usage = ResourceUsage.Default, BindFlags = BindFlags.ShaderResource | BindFlags.RenderTarget, CpuAccessFlags = CpuAccessFlags.None, OptionFlags = ResourceOptionFlags.GenerateMipMaps }; colorImageTexture = new Texture2D(device, colorImageTextureDesc); colorImageTextureRV = new ShaderResourceView(device, colorImageTexture); // vertex buffer var table = camera.calibration.ComputeDepthFrameToCameraSpaceTable(); int numVertices = 6 * (depthImageWidth - 1) * (depthImageHeight - 1); var vertices = new VertexPosition[numVertices]; Int3[] quadOffsets = new Int3[] { new Int3(0, 0, 0), new Int3(1, 0, 0), new Int3(0, 1, 0), new Int3(1, 0, 0), new Int3(1, 1, 0), new Int3(0, 1, 0), }; int vertexIndex = 0; for (int y = 0; y < depthImageHeight - 1; y++) { for (int x = 0; x < depthImageWidth - 1; x++) { for (int i = 0; i < 6; i++) { int vertexX = x + quadOffsets[i].X; int vertexY = y + quadOffsets[i].Y; var point = table[depthImageWidth * vertexY + vertexX]; var vertex = new VertexPosition(); vertex.position = new SharpDX.Vector4(point.X, point.Y, vertexX, vertexY); vertices[vertexIndex++] = vertex; } } } var stream = new DataStream(numVertices * VertexPosition.SizeInBytes, true, true); stream.WriteRange(vertices); stream.Position = 0; var vertexBufferDesc = new BufferDescription() { BindFlags = BindFlags.VertexBuffer, CpuAccessFlags = CpuAccessFlags.None, Usage = ResourceUsage.Default, SizeInBytes = numVertices * VertexPosition.SizeInBytes, }; vertexBuffer = new SharpDX.Direct3D11.Buffer(device, stream, vertexBufferDesc); vertexBufferBinding = new VertexBufferBinding(vertexBuffer, VertexPosition.SizeInBytes, 0); stream.Dispose(); var colorImage = new RoomAliveToolkit.ARGBImage(colorImageWidth, colorImageHeight); ProjectorCameraEnsemble.LoadFromTiff(imagingFactory, colorImage, directory + "/camera" + camera.name + "/colorDark.tiff"); var depthImage = new RoomAliveToolkit.ShortImage(depthImageWidth, depthImageHeight); ProjectorCameraEnsemble.LoadFromTiff(imagingFactory, depthImage, directory + "/camera" + camera.name + "/mean.tiff"); lock (renderLock) // necessary? { UpdateColorImage(device.ImmediateContext, colorImage.DataIntPtr); UpdateDepthImage(device.ImmediateContext, depthImage.DataIntPtr); } colorImage.Dispose(); depthImage.Dispose(); }
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); } }