public static void RotFromTo2Quat(Matrix x, Matrix y, Matrix q)
{
Matrix axis = new Matrix(3, 1);
axis.Cross(y, x);
axis.Normalize();
double angle = Math.Acos(x.Dot(y));
double s = Math.Sin(angle / 2.0);
q[0] = axis[0] * s;
q[1] = axis[1] * s;
q[2] = axis[2] * s;
q[3] = Math.Cos(angle / 2.0);
}
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];
}
}
// quaternion ops; quat is ((X, Y, Z), W)
public static void QuatMult(Matrix a, Matrix b, Matrix c)
{
Matrix v1 = new Matrix(3, 1);
Matrix v2 = new Matrix(3, 1);
Matrix v3 = new Matrix(3, 1);
v1[0] = a[0];
v1[1] = a[1];
v1[2] = a[2];
double s1 = a[3];
v2[0] = b[0];
v2[1] = b[1];
v2[2] = b[2];
double s2 = b[3];
v3.Cross(v1, v2);
c[0] = s1 * v2[0] + s2 * v1[0] + v3[0];
c[1] = s1 * v2[1] + s2 * v1[1] + v3[1];
c[2] = s1 * v2[2] + s2 * v1[2] + v3[2];
c[3] = s1 * s2 - v1.Dot(v2);
}
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];
}
}
// quaternion ops; quat is ((X, Y, Z), W)
public static void QuatMult(Matrix a, Matrix b, Matrix c)
{
Matrix v1 = new Matrix(3,1);
Matrix v2 = new Matrix(3,1);
Matrix v3 = new Matrix(3,1);
v1[0] = a[0];
v1[1] = a[1];
v1[2] = a[2];
double s1 = a[3];
v2[0] = b[0];
v2[1] = b[1];
v2[2] = b[2];
double s2 = b[3];
v3.Cross(v1, v2);
c[0] = s1*v2[0] + s2*v1[0] + v3[0];
c[1] = s1*v2[1] + s2*v1[1] + v3[1];
c[2] = s1*v2[2] + s2*v1[2] + v3[2];
c[3] = s1*s2 - v1.Dot(v2);
}
public static void RotFromTo2Quat(Matrix x, Matrix y, Matrix q)
{
Matrix axis = new Matrix(3,1);
axis.Cross(y, x);
axis.Normalize();
double angle = Math.Acos(x.Dot(y));
double s = Math.Sin(angle/2.0);
q[0] = axis[0]*s;
q[1] = axis[1]*s;
q[2] = axis[2]*s;
q[3] = Math.Cos(angle/2.0);
}
