public void Copy(ByteImage a, float scale) { byte* pa = a.Data(0, 0); float* p = data; for (int i = 0; i < width * height; i++) *p++ = *pa++ * scale; }
public void Copy(ByteImage byteImage) { byte* p = byteImage.Data(0, 0); RGB24* pOut = data; for (int i = 0; i < width * height; i++) { pOut->R = pOut->G = pOut++->B = *p++; } }
public void Copy(ByteImage byteImage) { byte * p = byteImage.Data(0, 0); RGB24 *pOut = data; for (int i = 0; i < width * height; i++) { pOut->R = pOut->G = pOut++->B = *p++; } }
public void Invert(ByteImage a) { byte *p0 = a.data; byte *p1 = data; for (int i = 0; i < width * height; i++) { *p1++ = (byte)(255 - *p0++); } }
public void Copy(ByteImage a, float scale) { byte * pa = a.Data(0, 0); float *p = data; for (int i = 0; i < width * height; i++) { *p++ = *pa++ *scale; } }
public void And(ByteImage a) { byte *p_a = a.Data(0, 0); byte *p = data; for (int i = 0; i < width * height; i++) { *p = (byte)((*p_a) & (*p)); p++; p_a++; } }
public void Add(ByteImage a) { byte * pa = a.Data(0, 0); float *p = data; for (int i = 0; i < width * height; i++) { *p = *p + *pa; p++; pa++; } }
public void Add(ByteImage a) { byte * 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 Decimate(ByteImage a, int factor) { byte* output = data; for (int y = 0; y < height; y++) { byte* pa = a.Data(0, y * factor); for (int x = 0; x < width; x++) { *output++ = *pa; pa += factor; } } }
public void Decimate(ByteImage a, int factor) { byte *output = data; for (int y = 0; y < height; y++) { byte *pa = a.Data(0, y * factor); for (int x = 0; x < width; x++) { *output++ = *pa; pa += factor; } } }
public void XMirror(ByteImage a) { byte *pOut = data; byte *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 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 Threshold(ByteImage a, byte threshold) { byte *pa = a.data; byte *p = data; for (int i = 0; i < a.width * a.height; i++) { if (*pa++ > threshold) { *p++ = 255; } else { *p++ = 0; } } }
public void ThresholdHighPass(ByteImage a, byte threshold) { byte *pa = a.data; byte *p = data; for (int i = 0; i < a.width * a.height; i++) { if (*pa > threshold) { *p++ = *pa; } else { *p++ = 0; } pa++; } }
public void CopyRectangle(ByteImage byteImage, int startX, int startY, int w, int h) { byte * pOrig = byteImage.Data(0, 0); ARGB32 *pOutOrig = data; byte * p; ARGB32 *pOut; for (int j = startY; j < h; j++) { for (int i = startX; i < w; i++) { p = pOrig + j * byteImage.Width + i; pOut = pOutOrig + j * width + i; pOut->A = 255; pOut->R = pOut->G = pOut->B = *p; } } }
public void CopyRectangle(ByteImage byteImage, int startX, int startY, int w, int h) { byte* pOrig = byteImage.Data(0, 0); ARGB32* pOutOrig = data; byte* p; ARGB32* pOut; for (int j = startY; j < h; j++) { for (int i = startX; i < w; i++) { p = pOrig + j * byteImage.Width + i; pOut = pOutOrig + j * width + i; pOut->A = 255; pOut->R = pOut->G = pOut->B = *p; } } }
// 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 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 LoadFromTiff(SharpDX.WIC.ImagingFactory imagingFactory, ByteImage image, string filename) { LoadFromTiff(imagingFactory, image, filename, 1); }
public void CaptureGrayCodes(string directory) { // for each projector // for each gray code // display gray code // for each camera (fork?) // capture color image; save to file // store as projector#/camera#/grayCode# // foreach camera // get calibration // save depth map to file var grayImage = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight); // pick up projector image dimensions form server and save them in configuration // put up projector's name on each foreach (var projector in projectors) { var size = projector.Client.Size(projector.displayIndex); projector.width = size.Width; projector.height = size.Height; projector.Client.OpenDisplay(projector.displayIndex); projector.Client.DisplayName(projector.displayIndex, projector.name); } // let AGC settle System.Threading.Thread.Sleep(2000); CaptureDepthAndColor(directory); //// save an image with projector name displayed, useful for later for visualization of results //foreach (var camera in cameras) //{ // string cameraDirectory = directory + "/camera" + camera.name; // if (!Directory.Exists(cameraDirectory)) // Directory.CreateDirectory(cameraDirectory); // //var jpegBytes = camera.client.LatestJPEGImage(); // //File.WriteAllBytes(cameraDirectory + "/projectorLabels.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 + "/projectorLabels.tiff"); // image.Dispose(); //} //// TODO: consider combining with later aquiring color and depth // set all projectors to black foreach (var projector in projectors) projector.Client.SetColor(projector.displayIndex, 0, 0, 0); foreach (var projector in projectors) { string projectorDirectory = directory + "/projector" + projector.name; if (!Directory.Exists(projectorDirectory)) Directory.CreateDirectory(projectorDirectory); int numberOfGrayCodeImages = projector.Client.NumberOfGrayCodeImages(projector.displayIndex); // set display to gray, to give AGC a chance to settle projector.Client.SetColor(projector.displayIndex, 0.5f, 0.5f, 0.5f); System.Threading.Thread.Sleep(1500); for (int i = 0; i < numberOfGrayCodeImages; i++) { projector.Client.DisplayGrayCode(projector.displayIndex, i); // wait for the image to be displayed and give camera AGC time to settle System.Threading.Thread.Sleep(500); // TODO: parallelize? foreach (var camera in cameras) { string cameraDirectory = projectorDirectory + "/camera" + camera.name; if (!Directory.Exists(cameraDirectory)) Directory.CreateDirectory(cameraDirectory); //// acquire color frames until exposure and gain have settled to a stable value //int numUnchanging = 0; //long lastExposureTime = 0; //float lastGain = 0; //const int numUnchangingThreshold = 5; //byte[] colorImageBytes = null; //while (numUnchanging < numUnchangingThreshold) //{ // colorImageBytes = camera.client.NextColorImage(); // consider providing a way of getting color exposure etc. w/o calling NextColorImage // long exposureTime = camera.client.LastColorExposureTimeTicks(); // float gain = camera.client.LastColorGain(); // if ((gain == lastGain) && (exposureTime == lastExposureTime)) // numUnchanging++; // lastGain = gain; // lastExposureTime = exposureTime; //} var colorImageBytes = camera.Client.LatestYUVImage(); // convert YUY2 to grayscale for (int y = 0; y < Kinect2Calibration.colorImageHeight; y++) for (int x = 0; x < Kinect2Calibration.colorImageWidth; x++) grayImage[x, y] = colorImageBytes[2 * (Kinect2Calibration.colorImageWidth * y + x)]; // save to file SaveToTiff(imagingFactory, grayImage, cameraDirectory + "/grayCode" + i + ".tiff"); } } projector.Client.SetColor(projector.displayIndex, 0, 0, 0); } // close all displays foreach (var projector in projectors) { projector.Client.CloseDisplay(projector.displayIndex); } }
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 static void SaveToTiff(SharpDX.WIC.ImagingFactory imagingFactory, ByteImage image, string filename) { SaveToTiff(imagingFactory, image, filename, SharpDX.WIC.PixelFormat.Format8bppGray, 1); }
public void Threshold(ByteImage a, byte threshold) { byte* pa = a.data; byte* p = data; for (int i = 0; i < a.width * a.height; i++) { if (*pa++ > threshold) *p++ = 255; else *p++ = 0; } }
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 ThresholdHighPass(ByteImage a, byte threshold) { byte* pa = a.data; byte* p = data; for (int i = 0; i < a.width * a.height; i++) { if (*pa > threshold) *p++ = *pa; else *p++ = 0; pa++; } }
public void Invert(ByteImage a) { byte* p0 = a.data; byte* p1 = data; for (int i = 0; i < width * height; i++) { *p1++ = (byte)(255 - *p0++); } }
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); }
// 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; } }