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 Matrix RotationMatrixFromRotationVector(Matrix rotationVector)
{
double angle = rotationVector.Norm();
var axis = new SharpDX.Vector3((float)(rotationVector[0] / angle), (float)(rotationVector[1] / angle), (float)(rotationVector[2] / angle));
// Why the negative sign? SharpDX returns a post-multiply matrix. Instead of transposing to get the pre-multiply matrix we just invert the input rotation.
var sR = SharpDX.Matrix.RotationAxis(axis, -(float)angle);
var R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R[i, j] = sR[i, j];
}
}
return(R);
}
public static void Normalize(Matrix A, Matrix B)
{
B.Scale(A, 1.0 / A.Norm());
}
public static Matrix RotationMatrixFromRotationVector(Matrix rotationVector)
{
double angle = rotationVector.Norm();
var axis = new SharpDX.Vector3((float)(rotationVector[0] / angle), (float)(rotationVector[1] / angle), (float)(rotationVector[2] / angle));
// Why the negative sign? SharpDX returns a post-multiply matrix. Instead of transposing to get the pre-multiply matrix we just invert the input rotation.
var sR = SharpDX.Matrix.RotationAxis(axis, -(float)angle);
var R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R[i, j] = sR[i, j];
return R;
}
// 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 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 void Normalize(Matrix A, Matrix B)
{
B.Scale(A, 1.0/A.Norm());
}
public static Matrix RotationMatrixFromRotationVector(Matrix rotationVector)
{
double angle = rotationVector.Norm();
var axis = new SharpDX.Vector3((float)(rotationVector[0] / angle), (float)(rotationVector[1] / angle), (float)(rotationVector[2] / angle));
var sR = SharpDX.Matrix.RotationAxis(axis, -(float)angle); // TODO: why sign?
var R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R[i, j] = sR[i, j];
return R;
}