/// <summary> /// /// </summary> /// <param name="directory"></param> /// <param name="objPath"></param> /// <param name="depthImage"></param> /// <param name="rgbImage"></param> /// <param name="pose">Optional transformation pose (4x4 Matrix). Supply Identity by default</param> public static void Save(string objPath, Kinect2Calibration calibration, ShortImage depthImage, string colorImageFileName, Matrix pose) { var objFilename = Path.GetFileNameWithoutExtension(objPath); var objDirectory = Path.GetDirectoryName(objPath); if (!Directory.Exists(objDirectory)) { Directory.CreateDirectory(objDirectory); } //copy the background color image to file //SaveColorToJPEG(objDirectory + "/" + objFilename + ".jpg", rgbImage); if (File.Exists(colorImageFileName)) { File.Copy(colorImageFileName, objDirectory + "/" + objFilename + ".jpg", true); } else { Console.WriteLine("Saving to OBJ Error! File " + colorImageFileName + " doesn't exists!"); } // Because we need to form triangles, we go back to the depth image var quadOffsets = new System.Drawing.Point[] { new System.Drawing.Point(0, 0), new System.Drawing.Point(1, 0), new System.Drawing.Point(0, 1), new System.Drawing.Point(1, 0), new System.Drawing.Point(1, 1), new System.Drawing.Point(0, 1), }; var streamWriter = new StreamWriter(objDirectory + "/" + objFilename + ".obj"); var mtlFileWriter = new StreamWriter(objDirectory + "/" + objFilename + ".mtl"); streamWriter.WriteLine("mtllib " + objFilename + ".mtl"); uint nextVertexIndex = 1; //var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); mtlFileWriter.WriteLine("newmtl camera0"); mtlFileWriter.WriteLine("Ka 1.000000 1.000000 1.000000"); mtlFileWriter.WriteLine("Kd 1.000000 1.000000 1.000000"); mtlFileWriter.WriteLine("Ks 0.000000 0.000000 0.000000"); mtlFileWriter.WriteLine("Tr 1.000000"); mtlFileWriter.WriteLine("illum 1"); mtlFileWriter.WriteLine("Ns 0.000000"); mtlFileWriter.WriteLine("map_Kd " + objFilename + ".jpg"); streamWriter.WriteLine("usemtl camera0"); // load depth image //string cameraDirectory = directory + "/camera" + camera.name; //depthImage.LoadFromFile(cameraDirectory + "/mean.bin"); //var calibration = camera.calibration; var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable(); var vertices = new Vertex[Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight]; var colorCamera = new Matrix(4, 1); var depthCamera = new Matrix(4, 1); var world = new Matrix(4, 1); for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++) { for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++) { // depth camera coords var depth = depthImage[x, y] / 1000f; // m // convert to depth camera space var point = depthFrameToCameraSpaceTable[Kinect2Calibration.depthImageWidth * y + x]; depthCamera[0] = point.X * depth; depthCamera[1] = point.Y * depth; depthCamera[2] = depth; depthCamera[3] = 1; // world coordinates world.Mult(pose, depthCamera); //world.Scale(1.0 / world[3]); not necessary for this transform // convert to color camera space colorCamera.Mult(calibration.depthToColorTransform, depthCamera); colorCamera.Scale(1.0 / colorCamera[3]); // project to color image double colorU, colorV; CameraMath.Project(calibration.colorCameraMatrix, calibration.colorLensDistortion, colorCamera[0], colorCamera[1], colorCamera[2], out colorU, out colorV); colorU /= (double)Kinect2Calibration.colorImageWidth; colorV /= (double)Kinect2Calibration.colorImageHeight; var vertex = new Vertex(); vertex.x = (float)world[0]; vertex.y = (float)world[1]; vertex.z = (float)world[2]; vertex.u = (float)colorU; vertex.v = (float)colorV; vertices[Kinect2Calibration.depthImageWidth * y + x] = vertex; } } streamWriter.WriteLine("g camera0"); streamWriter.WriteLine("usemtl camera0"); // examine each triangle for (int y = 0; y < Kinect2Calibration.depthImageHeight - 1; y++) { for (int x = 0; x < Kinect2Calibration.depthImageWidth - 1; x++) { int offseti = 0; for (int tri = 0; tri < 2; tri++) { // the indexes of the vertices of this triangle var i0 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti].Y) + (x + quadOffsets[offseti].X); var i1 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 1].Y) + (x + quadOffsets[offseti + 1].X); var i2 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 2].Y) + (x + quadOffsets[offseti + 2].X); // is triangle valid? bool nonZero = (vertices[i0].z != 0) && (vertices[i1].z != 0) && (vertices[i2].z != 0); bool jump01 = Vertex.DistanceSquared(vertices[i0], vertices[i1]) < 0.2 * 0.2; bool jump02 = Vertex.DistanceSquared(vertices[i0], vertices[i2]) < 0.2 * 0.2; bool jump12 = Vertex.DistanceSquared(vertices[i1], vertices[i2]) < 0.2 * 0.2; bool valid = nonZero && jump01 && jump02 && jump12; if (valid) { // only add the vertex if we haven't already if (vertices[i0].index == 0) { streamWriter.WriteLine(vertices[i0]); vertices[i0].index = nextVertexIndex++; } if (vertices[i1].index == 0) { streamWriter.WriteLine(vertices[i1]); vertices[i1].index = nextVertexIndex++; } if (vertices[i2].index == 0) { streamWriter.WriteLine(vertices[i2]); vertices[i2].index = nextVertexIndex++; } streamWriter.WriteLine("f {0}/{0} {1}/{1} {2}/{2}", vertices[i0].index, vertices[i1].index, vertices[i2].index); } offseti += 3; } } } streamWriter.Close(); mtlFileWriter.Close(); }
static double CalibrateColorCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, Matrix rotation, Matrix translation, bool silent) { int nPoints = worldPoints.Count; { Matrix R, t; DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t); var r = Orientation.RotationVector(R); rotation.Copy(r); translation.Copy(t); } // pack parameters into vector // parameters: fx, fy, cx, cy, k1, k2, + 3 for rotation, 3 translation = 12 int nParameters = 12; var parameters = new Matrix(nParameters, 1); { int pi = 0; parameters[pi++] = cameraMatrix[0, 0]; // fx parameters[pi++] = cameraMatrix[1, 1]; // fy parameters[pi++] = cameraMatrix[0, 2]; // cx parameters[pi++] = cameraMatrix[1, 2]; // cy parameters[pi++] = distCoeffs[0]; // k1 parameters[pi++] = distCoeffs[1]; // k2 parameters[pi++] = rotation[0]; parameters[pi++] = rotation[1]; parameters[pi++] = rotation[2]; parameters[pi++] = translation[0]; parameters[pi++] = translation[1]; parameters[pi++] = translation[2]; } // size of our error vector int nValues = nPoints * 2; // each component (x,y) is a separate entry LevenbergMarquardt.Function function = delegate(Matrix p) { var fvec = new Matrix(nValues, 1); // unpack parameters int pi = 0; double fx = p[pi++]; double fy = p[pi++]; double cx = p[pi++]; double cy = p[pi++]; double k1 = p[pi++]; double k2 = p[pi++]; var K = Matrix.Identity(3, 3); K[0, 0] = fx; K[1, 1] = fy; K[0, 2] = cx; K[1, 2] = cy; var d = Matrix.Zero(5, 1); d[0] = k1; d[1] = k2; var r = new Matrix(3, 1); r[0] = p[pi++]; r[1] = p[pi++]; r[2] = p[pi++]; var t = new Matrix(3, 1); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; var R = Orientation.Rodrigues(r); var x = new Matrix(3, 1); int fveci = 0; for (int i = 0; i < worldPoints.Count; i++) { // transform world point to local camera coordinates x.Mult(R, worldPoints[i]); x.Add(t); // fvec_i = y_i - f(x_i) double u, v; Kinect2Calibration.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[i]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); while (calibrate.State == LevenbergMarquardt.States.Running) { var rmsError = calibrate.MinimizeOneStep(parameters); if (!silent) { Console.WriteLine("rms error = " + rmsError); } } if (!silent) { for (int i = 0; i < nParameters; i++) { Console.WriteLine(parameters[i] + "\t"); } Console.WriteLine(); } // unpack parameters { int pi = 0; double fx = parameters[pi++]; double fy = parameters[pi++]; double cx = parameters[pi++]; double cy = parameters[pi++]; double k1 = parameters[pi++]; double k2 = parameters[pi++]; cameraMatrix[0, 0] = fx; cameraMatrix[1, 1] = fy; cameraMatrix[0, 2] = cx; cameraMatrix[1, 2] = cy; distCoeffs[0] = k1; distCoeffs[1] = k2; rotation[0] = parameters[pi++]; rotation[1] = parameters[pi++]; rotation[2] = parameters[pi++]; translation[0] = parameters[pi++]; translation[1] = parameters[pi++]; translation[2] = parameters[pi++]; } return(calibrate.RMSError); }
// Use DLT to obtain estimate of calibration rig pose; in our case this is the pose of the Kinect camera. // This pose estimate will provide a good initial estimate for subsequent projector calibration. // Note for a full PnP solution we should probably refine with Levenberg-Marquardt. // DLT is described in Hartley and Zisserman p. 178 static void DLT(Matrix cameraMatrix, Matrix distCoeffs, List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t) { int n = worldPoints.Count; var A = Matrix.Zero(2 * n, 12); for (int j = 0; j < n; j++) { var X = worldPoints[j]; var imagePoint = imagePoints[j]; double x, y; Kinect2Calibration.Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y); double w = 1; int ii = 2 * j; A[ii, 4] = -w * X[0]; A[ii, 5] = -w * X[1]; A[ii, 6] = -w * X[2]; A[ii, 7] = -w; A[ii, 8] = y * X[0]; A[ii, 9] = y * X[1]; A[ii, 10] = y * X[2]; A[ii, 11] = y; ii++; // next row A[ii, 0] = w * X[0]; A[ii, 1] = w * X[1]; A[ii, 2] = w * X[2]; A[ii, 3] = w; A[ii, 8] = -x * X[0]; A[ii, 9] = -x * X[1]; A[ii, 10] = -x * X[2]; A[ii, 11] = -x; } var Pcolumn = new Matrix(12, 1); { var U = new Matrix(2 * n, 12); var V = new Matrix(12, 12); var ww = new Matrix(12, 1); A.SVD(U, ww, V); // find smallest singular value int min = 0; ww.Minimum(ref min); // Pcolumn is last column of V Pcolumn.CopyCol(V, min); } // reshape into 3x4 projection matrix var P = new Matrix(3, 4); P.Reshape(Pcolumn); // x = P * X // P = K [ R | t ] // inv(K) P = [ R | t ] //var Kinv = new Matrix(3, 3); //Kinv.Inverse(cameraMatrix); //var Rt = new Matrix(3, 4); //Rt.Mult(Kinv, P); var Rt = new Matrix(3, 4); Rt.Copy(P); // P does not contain camera matrix (by earlier undistort) R = new Matrix(3, 3); t = new Matrix(3, 1); for (int ii = 0; ii < 3; ii++) { t[ii] = Rt[ii, 3]; for (int jj = 0; jj < 3; jj++) { R[ii, jj] = Rt[ii, jj]; } } //R.Copy(0, 0, Rt); //t.CopyCol(Rt, 3); if (R.Det() < 0) { R.Scale(-1); t.Scale(-1); } // orthogonalize R { var U = new Matrix(3, 3); var Vt = new Matrix(3, 3); var V = new Matrix(3, 3); var ww = new Matrix(3, 1); //OpenCV.SVD.Compute(R, out ww, out U, out Vt); R.SVD(U, ww, V); Vt.Transpose(V); R.Mult(U, Vt); double s = ww.Sum() / 3.0; t.Scale(1.0 / s); } // compute error? }
static double CalibrateDepthCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, bool silent) { int nPoints = worldPoints.Count; // pack parameters into vector // parameters: fx, fy, cx, cy, k1, k2 = 6 parameters int nParameters = 6; var parameters = new Matrix(nParameters, 1); { int pi = 0; parameters[pi++] = cameraMatrix[0, 0]; // fx parameters[pi++] = cameraMatrix[1, 1]; // fy parameters[pi++] = cameraMatrix[0, 2]; // cx parameters[pi++] = cameraMatrix[1, 2]; // cy parameters[pi++] = distCoeffs[0]; // k1 parameters[pi++] = distCoeffs[1]; // k2 } // size of our error vector int nValues = nPoints * 2; // each component (x,y) is a separate entry LevenbergMarquardt.Function function = delegate(Matrix p) { var fvec = new Matrix(nValues, 1); // unpack parameters int pi = 0; double fx = p[pi++]; double fy = p[pi++]; double cx = p[pi++]; double cy = p[pi++]; double k1 = p[pi++]; double k2 = p[pi++]; var K = Matrix.Identity(3, 3); K[0, 0] = fx; K[1, 1] = fy; K[0, 2] = cx; K[1, 2] = cy; var d = Matrix.Zero(5, 1); d[0] = k1; d[1] = k2; int fveci = 0; for (int i = 0; i < worldPoints.Count; i++) { // fvec_i = y_i - f(x_i) double u, v; var x = worldPoints[i]; Kinect2Calibration.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[i]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); while (calibrate.State == LevenbergMarquardt.States.Running) { var rmsError = calibrate.MinimizeOneStep(parameters); if (!silent) { Console.WriteLine("rms error = " + rmsError); } } if (!silent) { for (int i = 0; i < nParameters; i++) { Console.WriteLine(parameters[i] + "\t"); } Console.WriteLine(); } // unpack parameters { int pi = 0; double fx = parameters[pi++]; double fy = parameters[pi++]; double cx = parameters[pi++]; double cy = parameters[pi++]; double k1 = parameters[pi++]; double k2 = parameters[pi++]; cameraMatrix[0, 0] = fx; cameraMatrix[1, 1] = fy; cameraMatrix[0, 2] = cx; cameraMatrix[1, 2] = cy; distCoeffs[0] = k1; distCoeffs[1] = k2; } return(calibrate.RMSError); }