// 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 public 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; Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y); int ii = 2 * j; A[ii, 4] = -X[0]; A[ii, 5] = -X[1]; A[ii, 6] = -X[2]; A[ii, 7] = -1; 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] = X[0]; A[ii, 1] = X[1]; A[ii, 2] = X[2]; A[ii, 3] = 1; A[ii, 8] = -x * X[0]; A[ii, 9] = -x * X[1]; A[ii, 10] = -x * X[2]; A[ii, 11] = -x; } // Pcolumn is the eigenvector of ATA with the smallest eignvalue var Pcolumn = new Matrix(12, 1); { var ATA = new Matrix(12, 12); ATA.MultATA(A, A); var V = new Matrix(12, 12); var ww = new Matrix(12, 1); ATA.Eig(V, ww); Pcolumn.CopyCol(V, 0); } // reshape into 3x4 projection matrix var P = new Matrix(3, 4); P.Reshape(Pcolumn); R = new Matrix(3, 3); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) R[i, j] = P[i, j]; if (R.Det3x3() < 0) { R.Scale(-1); P.Scale(-1); } // orthogonalize R { var U = new Matrix(3, 3); var V = new Matrix(3, 3); var ww = new Matrix(3, 1); R.SVD(U, ww, V); R.MultAAT(U, V); } // determine scale factor var RP = new Matrix(3, 3); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) RP[i, j] = P[i, j]; double s = RP.Norm() / R.Norm(); t = new Matrix(3, 1); for (int i = 0; i < 3; i++) t[i] = P[i, 3]; t.Scale(1.0 / s); }
static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t, out Matrix d2) { int n = X.Count; var mu = new Matrix(3, 1); for (int i = 0; i < n; i++) mu.Add(X[i]); mu.Scale(1f / (float)n); var A = new Matrix(3, 3); var xc = new Matrix(3, 1); var M = new Matrix(3, 3); for (int i = 0; i < X.Count; i++) { var x = X[i]; xc.Sub(x, mu); M.Outer(xc, xc); A.Add(M); } var V = new Matrix(3, 3); var d = new Matrix(3, 1); A.Eig(V, d); // eigenvalues in ascending order // arrange in descending order so that z = 0 var V2 = new Matrix(3, 3); for (int i = 0; i < 3; i++) { V2[i, 2] = V[i, 0]; V2[i, 1] = V[i, 1]; V2[i, 0] = V[i, 2]; } d2 = new Matrix(3, 1); d2[2] = d[0]; d2[1] = d[1]; d2[0] = d[2]; R = new Matrix(3, 3); R.Transpose(V2); if (R.Det3x3() < 0) R.Scale(-1); t = new Matrix(3, 1); t.Mult(R, mu); t.Scale(-1); // eigenvalues are the sum of squared distances in each direction // i.e., min eigenvalue is the sum of squared distances to the plane = d2[2] // compute the distance to the plane by transforming to the plane and take z-coordinate: // xPlane = R*x + t; distance = xPlane[2] }
// 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 public 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; Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y); int ii = 2 * j; A[ii, 4] = -X[0]; A[ii, 5] = -X[1]; A[ii, 6] = -X[2]; A[ii, 7] = -1; 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] = X[0]; A[ii, 1] = X[1]; A[ii, 2] = X[2]; A[ii, 3] = 1; A[ii, 8] = -x * X[0]; A[ii, 9] = -x * X[1]; A[ii, 10] = -x * X[2]; A[ii, 11] = -x; } // Pcolumn is the eigenvector of ATA with the smallest eignvalue var Pcolumn = new Matrix(12, 1); { var ATA = new Matrix(12, 12); ATA.MultATA(A, A); var V = new Matrix(12, 12); var ww = new Matrix(12, 1); ATA.Eig(V, ww); Pcolumn.CopyCol(V, 0); } // reshape into 3x4 projection matrix var P = new Matrix(3, 4); P.Reshape(Pcolumn); R = new Matrix(3, 3); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R[i, j] = P[i, j]; } } if (R.Det3x3() < 0) { R.Scale(-1); P.Scale(-1); } // orthogonalize R { var U = new Matrix(3, 3); var V = new Matrix(3, 3); var ww = new Matrix(3, 1); R.SVD(U, ww, V); R.MultAAT(U, V); } // determine scale factor var RP = new Matrix(3, 3); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { RP[i, j] = P[i, j]; } } double s = RP.Norm() / R.Norm(); t = new Matrix(3, 1); for (int i = 0; i < 3; i++) { t[i] = P[i, 3]; } t.Scale(1.0 / s); }
// 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 public 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; 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, 2 * n); // full SVD, alas, supports small number of points 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.Det3x3() < 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); 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 public void PlaneFit(IList <Matrix> X, out Matrix R, out Matrix t, out Matrix d2) { int n = X.Count; var mu = new Matrix(3, 1); for (int i = 0; i < n; i++) { mu.Add(X[i]); } mu.Scale(1f / (float)n); var A = new Matrix(3, 3); var xc = new Matrix(3, 1); var M = new Matrix(3, 3); for (int i = 0; i < X.Count; i++) { var x = X[i]; xc.Sub(x, mu); M.Outer(xc, xc); A.Add(M); } var V = new Matrix(3, 3); var d = new Matrix(3, 1); A.Eig(V, d); // eigenvalues in ascending order // arrange in descending order so that z = 0 var V2 = new Matrix(3, 3); for (int i = 0; i < 3; i++) { V2[i, 2] = V[i, 0]; V2[i, 1] = V[i, 1]; V2[i, 0] = V[i, 2]; } d2 = new Matrix(3, 1); d2[2] = d[0]; d2[1] = d[1]; d2[0] = d[2]; R = new Matrix(3, 3); R.Transpose(V2); if (R.Det3x3() < 0) { R.Scale(-1); } t = new Matrix(3, 1); t.Mult(R, mu); t.Scale(-1); // eigenvalues are the sum of squared distances in each direction // i.e., min eigenvalue is the sum of squared distances to the plane = d2[2] // compute the distance to the plane by transforming to the plane and take z-coordinate: // xPlane = R*x + t; distance = xPlane[2] }