public static Quaternion OptimalRotationWeighted(Vector[] p, Vector[] x, Vector up, Vector ux, double[] w)
{
MatrixByArr cov = CovarianceWeighted(p, x, up, ux, w);
double tr = cov[0, 0] + cov[1, 1] + cov[2, 2];
double A23 = cov[1, 2] - cov[2, 1];
double A31 = cov[2, 0] - cov[0, 2];
double A12 = cov[0, 1] - cov[1, 0];
MatrixByArr mat = new double[4, 4];
mat[0, 0] = tr;
mat[1, 0] = mat[0, 1] = A23;
mat[2, 0] = mat[0, 2] = A31;
mat[3, 0] = mat[0, 3] = A12;
cov = cov + cov.Tr();
cov = cov - LinAlg.Eye(3, tr);
mat[1, 1] = cov[0, 0];
mat[1, 2] = cov[1, 0];
mat[1, 3] = cov[2, 0];
mat[2, 1] = cov[0, 1];
mat[2, 2] = cov[1, 1];
mat[2, 3] = cov[2, 1];
mat[3, 1] = cov[0, 2];
mat[3, 2] = cov[1, 2];
mat[3, 3] = cov[2, 2];
LinAlg.Eigen eigen = new LinAlg.Eigen(mat);
double[,] eigenD = eigen.getD();
double[,] eigenV = eigen.getV();
int index_max = 0;
if (eigenD[1, 1] > eigenD[index_max, index_max])
{
index_max = 1;
}
if (eigenD[2, 2] > eigenD[index_max, index_max])
{
index_max = 2;
}
if (eigenD[3, 3] > eigenD[index_max, index_max])
{
index_max = 3;
}
Quaternion quater = new Quaternion(
eigenV[0, index_max],
eigenV[1, index_max],
eigenV[2, index_max],
eigenV[3, index_max]
);
return(quater);
}
public static Vector FlattenCol(this MatrixByArr mat) /* <summary> flatten toward column direction </summary>*/
{
return(mat.Tr().Flatten());
}
