public static Matrix Zero(int m, int n) { var A = new Matrix(m, n); A.Zero(); return(A); }
public void Diag(Matrix A, Matrix d) { A.Zero(); for (int i = 0; i < A.m; i++) { A[i, i] = d[i]; } }
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); }
static double CalibrateDepthCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs) { 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]; 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; } 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 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? }
public static double CalibrateCameraExtrinsicsOnly(List <List <Matrix> > worldPointSets, List <List <System.Drawing.PointF> > imagePointSets, Matrix cameraMatrix, ref List <Matrix> rotations, ref List <Matrix> translations) { int nSets = worldPointSets.Count; int nPoints = 0; for (int i = 0; i < nSets; i++) { nPoints += worldPointSets[i].Count; // for later } var distCoeffs = Matrix.Zero(2, 1); //// if necessary run DLT on each point set to get initial rotation and translations //if (rotations == null) //{ // rotations = new List<Matrix>(); // translations = new List<Matrix>(); // for (int i = 0; i < nSets; i++) // { // Matrix R, t; // CameraMath.DLT(cameraMatrix, distCoeffs, worldPointSets[i], imagePointSets[i], out R, out t); // var r = CameraMath.RotationVectorFromRotationMatrix(R); // rotations.Add(r); // translations.Add(t); // } //} // Levenberg-Marquardt for camera matrix (ignore lens distortion for now) // pack parameters into vector // parameters: camera has f, cx, cy; each point set has rotation + translation (6) //int nParameters = 3 + 6 * nSets; int nParameters = 6 * nSets; var parameters = new Matrix(nParameters, 1); { int pi = 0; //parameters[pi++] = cameraMatrix[0, 0]; // f //parameters[pi++] = cameraMatrix[0, 2]; // cx //parameters[pi++] = cameraMatrix[1, 2]; // cy for (int i = 0; i < nSets; i++) { parameters[pi++] = rotations[i][0]; parameters[pi++] = rotations[i][1]; parameters[pi++] = rotations[i][2]; parameters[pi++] = translations[i][0]; parameters[pi++] = translations[i][1]; parameters[pi++] = translations[i][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 f = p[pi++]; //double cx = p[pi++]; //double cy = p[pi++]; var K = Matrix.Identity(3, 3); //K[0, 0] = f; //K[1, 1] = f; //K[0, 2] = cx; //K[1, 2] = cy; K[0, 0] = cameraMatrix[0, 0]; K[1, 1] = cameraMatrix[1, 1]; K[0, 2] = cameraMatrix[0, 2]; K[1, 2] = cameraMatrix[1, 2]; var d = Matrix.Zero(2, 1); int fveci = 0; for (int i = 0; i < nSets; i++) { var rotation = new Matrix(3, 1); rotation[0] = p[pi++]; rotation[1] = p[pi++]; rotation[2] = p[pi++]; var R = RotationMatrixFromRotationVector(rotation); var t = new Matrix(3, 1); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; var worldPoints = worldPointSets[i]; var imagePoints = imagePointSets[i]; var x = new Matrix(3, 1); for (int j = 0; j < worldPoints.Count; j++) { // transform world point to local camera coordinates x.Mult(R, worldPoints[j]); 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[j]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); calibrate.minimumReduction = 1.0e-4; calibrate.Minimize(parameters); //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 f = parameters[pi++]; //double cx = parameters[pi++]; //double cy = parameters[pi++]; //cameraMatrix[0, 0] = f; //cameraMatrix[1, 1] = f; //cameraMatrix[0, 2] = cx; //cameraMatrix[1, 2] = cy; for (int i = 0; i < nSets; i++) { rotations[i][0] = parameters[pi++]; rotations[i][1] = parameters[pi++]; rotations[i][2] = parameters[pi++]; translations[i][0] = parameters[pi++]; translations[i][1] = parameters[pi++]; translations[i][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 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 static void TestPlanarDLT() { var cameraMatrix = Matrix.Identity(3, 3); cameraMatrix[0, 0] = 300; cameraMatrix[1, 1] = 300; cameraMatrix[0, 2] = 250; cameraMatrix[1, 2] = 220; var distCoeffs = new Matrix(5, 1); distCoeffs[0] = 0.05; distCoeffs[1] = -0.1; // generate a bunch of points in a plane // project under some other camera (view) var R = new Matrix(3, 3); R.RotEuler2Matrix(0.3, -0.2, 0.6); var t = new Matrix(3, 1); t[0] = 0.2; t[1] = 0.3; t[2] = 2; var modelR = new Matrix(3, 3); modelR.RotEuler2Matrix(-0.6, 0.2, 0.3); var modelT = new Matrix(3, 1); modelT[0] = -0.1; modelT[1] = 1.0; modelT[2] = 1.5; var worldPoints = new List <Matrix>(); var worldTransformedPoints = new List <Matrix>(); var imagePoints = new List <System.Drawing.PointF>(); var zero3 = Matrix.Zero(3, 1); for (float y = -1f; y <= 1.0f; y += 0.2f) { for (float x = -1f; x <= 1.0f; x += 0.2f) { var model = new Matrix(3, 1); model[0] = x; model[1] = y; model[2] = 0; var noise = Matrix.GaussianSample(zero3, 0.1 * 0.1); var world = new Matrix(3, 1); world.Mult(modelR, model); world.Add(modelT); world.Add(noise); worldPoints.Add(world); // under some camera: var worldTransformed = new Matrix(3, 1); worldTransformed.Mult(R, world); worldTransformed.Add(t); worldTransformedPoints.Add(worldTransformed); double u, v; Project(cameraMatrix, distCoeffs, worldTransformed[0], worldTransformed[1], worldTransformed[2], out u, out v); var image = new System.Drawing.PointF(); image.X = (float)u; image.Y = (float)v; imagePoints.Add(image); } } Console.WriteLine("R\n" + R); Console.WriteLine("t\n" + t); var Rplane = new Matrix(3, 1); var Tplane = new Matrix(3, 1); PlaneFit(worldPoints, out Rplane, out Tplane); var Rest = new Matrix(3, 3); var test = new Matrix(3, 1); PlanarDLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, Rplane, Tplane, out Rest, out test); Console.WriteLine("Rest\n" + Rest); Console.WriteLine("test\n" + test); }
public static void TestDLT() { var cameraMatrix = Matrix.Identity(3, 3); cameraMatrix[0, 0] = 700; cameraMatrix[1, 1] = 700; cameraMatrix[0, 2] = 250; cameraMatrix[1, 2] = 220; var distCoeffs = new Matrix(5, 1); distCoeffs[0] = 0.05; distCoeffs[1] = -0.1; // generate a bunch of points in a volume // project under some other camera (view) var R = new Matrix(3, 3); R.RotEuler2Matrix(0.2, 0.3, 0.3); var t = new Matrix(3, 1); t[0] = 2; t[1] = 0; t[2] = -4; var modelPoints = new List <Matrix>(); var imagePoints = new List <System.Drawing.PointF>(); var zero3 = Matrix.Zero(3, 1); for (float z = 1f; z <= 3.0f; z += 0.4f) { for (float y = -1f; y <= 1.0f; y += 0.4f) { for (float x = -1f; x <= 1.0f; x += 0.4f) { var model = new Matrix(3, 1); model[0] = x; model[1] = y; model[2] = z; modelPoints.Add(model); // under our camera: var transformedPoint = new Matrix(3, 1); transformedPoint.Mult(R, model); transformedPoint.Add(t); var noise = Matrix.GaussianSample(zero3, 0.1 * 0.1); transformedPoint.Add(noise); double u, v; Project(cameraMatrix, distCoeffs, transformedPoint[0], transformedPoint[1], transformedPoint[2], out u, out v); var image = new System.Drawing.PointF(); image.X = (float)u; image.Y = (float)v; imagePoints.Add(image); } } } Console.WriteLine("x = ["); for (int i = 0; i < imagePoints.Count; i++) { Console.WriteLine("{0} {1}", imagePoints[i].X, imagePoints[i].Y); } Console.WriteLine("]';"); Console.WriteLine("X = ["); for (int i = 0; i < modelPoints.Count; i++) { Console.WriteLine("{0} {1} {2}", modelPoints[i][0], modelPoints[i][1], modelPoints[i][2]); } Console.WriteLine("]';"); Console.WriteLine("fc = [{0} {1}];", cameraMatrix[0, 0], cameraMatrix[1, 1]); Console.WriteLine("cc = [{0} {1}];", cameraMatrix[0, 2], cameraMatrix[1, 2]); Console.WriteLine("kc = [{0} {1} 0 0 0];", distCoeffs[0], distCoeffs[1]); Console.WriteLine(); Console.WriteLine("R\n" + R); Console.WriteLine("t\n" + t); var Rest = new Matrix(3, 3); var test = new Matrix(3, 1); DLT(cameraMatrix, distCoeffs, modelPoints, imagePoints, out Rest, out test); Console.WriteLine("Rest\n" + Rest); Console.WriteLine("test\n" + test); }
public static Matrix Zero(int m, int n) { var A = new Matrix(m, n); A.Zero(); return A; }
public void Diag(Matrix A, Matrix d) { A.Zero(); for (int i = 0; i < A.m; i++) A[i, i] = d[i]; }