public static void PlanarDLT(Matrix cameraMatrix, Matrix distCoeffs, List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t) { int n = worldPoints.Count; var undistortedImagePoints = new List <System.Drawing.PointF>(); for (int i = 0; i < n; i++) { var imagePoint = imagePoints[i]; double x, y; Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y); var undistorted = new System.Drawing.PointF(); undistorted.X = (float)x; undistorted.Y = (float)y; undistortedImagePoints.Add(undistorted); } var H = Homography(worldPoints, undistortedImagePoints); H.Scale(1.0 / H[2, 2]); //Console.WriteLine(H); var r1 = new Matrix(3, 1); r1.CopyCol(H, 0); var r2 = new Matrix(3, 1); r2.CopyCol(H, 1); t = new Matrix(3, 1); t.CopyCol(H, 2); t.Scale(1 / ((r1.Norm() + r2.Norm()) / 2.0)); r1.Scale(1 / r1.Norm()); r2.Scale(1 / r2.Norm()); var r3 = new Matrix(3, 1); r3.Cross(r1, r2); R = new Matrix(3, 3); for (int i = 0; i < 3; i++) { R[i, 0] = r1[i]; R[i, 1] = r2[i]; R[i, 2] = r3[i]; } }
// 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 PlanarDLT(Matrix cameraMatrix, Matrix distCoeffs, List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t) { int n = worldPoints.Count; var undistortedImagePoints = new List<System.Drawing.PointF>(); for (int i = 0; i < n; i++) { var imagePoint = imagePoints[i]; double x, y; Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y); var undistorted = new System.Drawing.PointF(); undistorted.X = (float)x; undistorted.Y = (float)y; undistortedImagePoints.Add(undistorted); } var H = Homography(worldPoints, undistortedImagePoints); H.Scale(1.0 / H[2, 2]); //Console.WriteLine(H); var r1 = new Matrix(3, 1); r1.CopyCol(H, 0); var r2 = new Matrix(3, 1); r2.CopyCol(H, 1); t = new Matrix(3, 1); t.CopyCol(H, 2); t.Scale(1 / ((r1.Norm() + r2.Norm()) / 2.0)); r1.Scale(1 / r1.Norm()); r2.Scale(1 / r2.Norm()); var r3 = new Matrix(3, 1); r3.Cross(r1, r2); R = new Matrix(3, 3); for (int i = 0; i < 3; i++) { R[i, 0] = r1[i]; R[i, 1] = r2[i]; R[i, 2] = r3[i]; } }
public static double PlaneFit(IList<Matrix> points, out Matrix X, out double D) { X = new Matrix(3, 1); var mu = new RoomAliveToolkit.Matrix(3, 1); for (int i = 0; i < points.Count; i++) mu.Add(points[i]); mu.Scale(1f / (float)points.Count); var A = new RoomAliveToolkit.Matrix(3, 3); var pc = new RoomAliveToolkit.Matrix(3, 1); var M = new RoomAliveToolkit.Matrix(3, 3); for (int i = 0; i < points.Count; i++) { var p = points[i]; pc.Sub(p, mu); M.Outer(pc, pc); A.Add(M); } var V = new RoomAliveToolkit.Matrix(3, 3); var d = new RoomAliveToolkit.Matrix(3, 1); A.Eig(V, d); // TODO: replace with 3x3 version? //Console.WriteLine("------"); //Console.WriteLine(A); //Console.WriteLine(V); //Console.WriteLine(d); double minEigenvalue = Double.MaxValue; int minEigenvaluei = 0; for (int i = 0; i < 3; i++) if (d[i] < minEigenvalue) { minEigenvalue = d[i]; minEigenvaluei = i; } X.CopyCol(V, minEigenvaluei); D = -X.Dot(mu); // min eigenvalue is the sum of squared distances to the plane // signed distance is: double distance = X.Dot(point) + D; return minEigenvalue; }
// 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? }
// 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); }