// 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 double CalibrateColorCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, Matrix rotation, Matrix translation) { int nPoints = worldPoints.Count; { Matrix R, t; CameraMath.DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t); //var r = Orientation.RotationVector(R); var r = RoomAliveToolkit.ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(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 R = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(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; CameraMath.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); Console.WriteLine("rms error = " + rmsError); } 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); }
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); }