public static void LeastSquares(Matrix x, Matrix A, Matrix b) { // use svd // for overdetermined systems A*x = b // x = V * diag(1/wj) * U T * b // NRC p. 66 int m = A.m; int n = A.n; Matrix U = new Matrix(m, n), V = new Matrix(n, n), w = new Matrix(n, 1), W = new Matrix(n, n); A.SVD(U, w, V); w.Reciprocal(); W.Diag(w); Matrix M = new Matrix(n, n); M.Mult(V, W); Matrix N = new Matrix(n, m); N.MultAAT(M, U); x.Mult(N, b); }
// 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); 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); }