public double MinimizeOneStep(Matrix parameters) { // initial value of the function; callee knows the size of the returned vector var errorVector = function(parameters); var error = errorVector.Dot(errorVector); // Jacobian; callee knows the size of the returned matrix var J = jacobianFunction(parameters); // J'*J var JtJ = new Matrix(parameters.Size, parameters.Size); //stopWatch.Restart(); //JtJ.MultATA(J, J); // this is the big calculation that could be parallelized JtJ.MultATAParallel(J, J); //Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds); // J'*error var JtError = new Matrix(parameters.Size, 1); //stopWatch.Restart(); JtError.MultATA(J, errorVector); // error vector must be a column vector //Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds); // allocate some space var JtJaugmented = new Matrix(parameters.Size, parameters.Size); var JtJinv = new Matrix(parameters.Size, parameters.Size); var delta = new Matrix(parameters.Size, 1); var newParameters = new Matrix(parameters.Size, 1); // find a value of lambda that reduces error double lambda = initialLambda; while (true) { // augment J'*J: J'*J += lambda*(diag(J)) JtJaugmented.Copy(JtJ); for (int i = 0; i < parameters.Size; i++) JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i]; //WriteMatrixToFile(errorVector, "errorVector"); //WriteMatrixToFile(J, "J"); //WriteMatrixToFile(JtJaugmented, "JtJaugmented"); //WriteMatrixToFile(JtError, "JtError"); // solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error JtJinv.Inverse(JtJaugmented); delta.Mult(JtJinv, JtError); // new parameters = parameters - delta [why not add?] newParameters.Sub(parameters, delta); // evaluate function, compute error var newErrorVector = function(newParameters); double newError = newErrorVector.Dot(newErrorVector); // if error is reduced, divide lambda by 10 bool improvement; if (newError < error) { lambda /= lambdaIncrement; improvement = true; } else // if not, multiply lambda by 10 { lambda *= lambdaIncrement; improvement = false; } // termination criteria: // reduction in error is too small var diff = new Matrix(errorVector.Size, 1); diff.Sub(errorVector, newErrorVector); double diffSq = diff.Dot(diff); double errorDelta = Math.Sqrt(diffSq / error); if (errorDelta < minimumReduction) state = States.ReductionStepTooSmall; // lambda is too big if (lambda > maximumLambda) state = States.LambdaTooLarge; // change in parameters is too small [not implemented] // if we made an improvement, accept the new parameters if (improvement) { parameters.Copy(newParameters); error = newError; break; } // if we meet termination criteria, break if (state != States.Running) break; } rmsError = Math.Sqrt(error / errorVector.Size); return 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 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); }
public static Matrix Homography(List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints) { int n = worldPoints.Count; // normalize image coordinates var mu = new Matrix(2, 1); for (int i = 0; i < n; i++) { mu[0] += imagePoints[i].X; mu[1] += imagePoints[i].Y; } mu.Scale(1.0 / n); var muAbs = new Matrix(2, 1); for (int i = 0; i < n; i++) { muAbs[0] += Math.Abs(imagePoints[i].X - mu[0]); muAbs[1] += Math.Abs(imagePoints[i].Y - mu[1]); } muAbs.Scale(1.0 / n); var Hnorm = Matrix.Identity(3, 3); Hnorm[0, 0] = 1 / muAbs[0]; Hnorm[1, 1] = 1 / muAbs[1]; Hnorm[0, 2] = -mu[0] / muAbs[0]; Hnorm[1, 2] = -mu[1] / muAbs[1]; var invHnorm = Matrix.Identity(3, 3); invHnorm[0, 0] = muAbs[0]; invHnorm[1, 1] = muAbs[1]; invHnorm[0, 2] = mu[0]; invHnorm[1, 2] = mu[1]; var A = Matrix.Zero(2 * n, 9); for (int i = 0; i < n; i++) { var X = worldPoints[i]; var imagePoint = imagePoints[i]; var x = new Matrix(3, 1); x[0] = imagePoint.X; x[1] = imagePoint.Y; x[2] = 1; var xn = new Matrix(3, 1); xn.Mult(Hnorm, x); // Zhang's formulation; Hartley's is similar int ii = 2 * i; A[ii, 0] = X[0]; A[ii, 1] = X[1]; A[ii, 2] = 1; A[ii, 6] = -xn[0] * X[0]; A[ii, 7] = -xn[0] * X[1]; A[ii, 8] = -xn[0]; ii++; // next row A[ii, 3] = X[0]; A[ii, 4] = X[1]; A[ii, 5] = 1; A[ii, 6] = -xn[1] * X[0]; A[ii, 7] = -xn[1] * X[1]; A[ii, 8] = -xn[1]; } // h is the eigenvector of ATA with the smallest eignvalue var h = new Matrix(9, 1); { var ATA = new Matrix(9, 9); ATA.MultATA(A, A); var V = new Matrix(9, 9); var ww = new Matrix(9, 1); ATA.Eig(V, ww); h.CopyCol(V, 0); } var Hn = new Matrix(3, 3); Hn.Reshape(h); var H = new Matrix(3, 3); H.Mult(invHnorm, Hn); return H; }
// 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); }
public static Matrix Homography(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints) { int n = worldPoints.Count; // normalize image coordinates var mu = new Matrix(2, 1); for (int i = 0; i < n; i++) { mu[0] += imagePoints[i].X; mu[1] += imagePoints[i].Y; } mu.Scale(1.0 / n); var muAbs = new Matrix(2, 1); for (int i = 0; i < n; i++) { muAbs[0] += Math.Abs(imagePoints[i].X - mu[0]); muAbs[1] += Math.Abs(imagePoints[i].Y - mu[1]); } muAbs.Scale(1.0 / n); var Hnorm = Matrix.Identity(3, 3); Hnorm[0, 0] = 1 / muAbs[0]; Hnorm[1, 1] = 1 / muAbs[1]; Hnorm[0, 2] = -mu[0] / muAbs[0]; Hnorm[1, 2] = -mu[1] / muAbs[1]; var invHnorm = Matrix.Identity(3, 3); invHnorm[0, 0] = muAbs[0]; invHnorm[1, 1] = muAbs[1]; invHnorm[0, 2] = mu[0]; invHnorm[1, 2] = mu[1]; var A = Matrix.Zero(2 * n, 9); for (int i = 0; i < n; i++) { var X = worldPoints[i]; var imagePoint = imagePoints[i]; var x = new Matrix(3, 1); x[0] = imagePoint.X; x[1] = imagePoint.Y; x[2] = 1; var xn = new Matrix(3, 1); xn.Mult(Hnorm, x); // Zhang's formulation; Hartley's is similar int ii = 2 * i; A[ii, 0] = X[0]; A[ii, 1] = X[1]; A[ii, 2] = 1; A[ii, 6] = -xn[0] * X[0]; A[ii, 7] = -xn[0] * X[1]; A[ii, 8] = -xn[0]; ii++; // next row A[ii, 3] = X[0]; A[ii, 4] = X[1]; A[ii, 5] = 1; A[ii, 6] = -xn[1] * X[0]; A[ii, 7] = -xn[1] * X[1]; A[ii, 8] = -xn[1]; } // h is the eigenvector of ATA with the smallest eignvalue var h = new Matrix(9, 1); { var ATA = new Matrix(9, 9); ATA.MultATA(A, A); var V = new Matrix(9, 9); var ww = new Matrix(9, 1); ATA.Eig(V, ww); h.CopyCol(V, 0); } var Hn = new Matrix(3, 3); Hn.Reshape(h); var H = new Matrix(3, 3); H.Mult(invHnorm, Hn); return(H); }
public double MinimizeOneStep(Matrix parameters) { // initial value of the function; callee knows the size of the returned vector var errorVector = function(parameters); var error = errorVector.Dot(errorVector); // Jacobian; callee knows the size of the returned matrix var J = jacobianFunction(parameters); // J'*J var JtJ = new Matrix(parameters.Size, parameters.Size); //stopWatch.Restart(); //JtJ.MultATA(J, J); // this is the big calculation that could be parallelized JtJ.MultATAParallel(J, J); //Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds); // J'*error var JtError = new Matrix(parameters.Size, 1); //stopWatch.Restart(); JtError.MultATA(J, errorVector); // error vector must be a column vector //Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds); // allocate some space var JtJaugmented = new Matrix(parameters.Size, parameters.Size); var JtJinv = new Matrix(parameters.Size, parameters.Size); var delta = new Matrix(parameters.Size, 1); var newParameters = new Matrix(parameters.Size, 1); // find a value of lambda that reduces error double lambda = initialLambda; while (true) { // augment J'*J: J'*J += lambda*(diag(J)) JtJaugmented.Copy(JtJ); for (int i = 0; i < parameters.Size; i++) { JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i]; } //WriteMatrixToFile(errorVector, "errorVector"); //WriteMatrixToFile(J, "J"); //WriteMatrixToFile(JtJaugmented, "JtJaugmented"); //WriteMatrixToFile(JtError, "JtError"); // solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error JtJinv.Inverse(JtJaugmented); delta.Mult(JtJinv, JtError); // new parameters = parameters - delta [why not add?] newParameters.Sub(parameters, delta); // evaluate function, compute error var newErrorVector = function(newParameters); double newError = newErrorVector.Dot(newErrorVector); // if error is reduced, divide lambda by 10 bool improvement; if (newError < error) { lambda /= lambdaIncrement; improvement = true; } else // if not, multiply lambda by 10 { lambda *= lambdaIncrement; improvement = false; } // termination criteria: // reduction in error is too small var diff = new Matrix(errorVector.Size, 1); diff.Sub(errorVector, newErrorVector); double diffSq = diff.Dot(diff); double errorDelta = Math.Sqrt(diffSq / error); if (errorDelta < minimumReduction) { state = States.ReductionStepTooSmall; } // lambda is too big if (lambda > maximumLambda) { state = States.LambdaTooLarge; } // change in parameters is too small [not implemented] // if we made an improvement, accept the new parameters if (improvement) { parameters.Copy(newParameters); error = newError; break; } // if we meet termination criteria, break if (state != States.Running) { break; } } rmsError = Math.Sqrt(error / errorVector.Size); return(rmsError); }