/// <summary>
/// Расчитывает матрицу перехода
/// Взято здесь http://jepsonsblog.blogspot.ru/2012/11/rotation-in-3d-using-opencvs.html
/// </summary>
private void calculateTransformationMatrix(CoordinatesTransformer transformer)
{
// Матрица поворота 3x3
CvMat R3x3 = new CvMat(3, 3, MatrixType.F64C1);
Cv.Rodrigues2(transformer.Rotation, R3x3);
// Произведение матрицы поворота и матрицы переноса
CvMat RxT = new CvMat(4, 4, MatrixType.F64C1, new double[, ]
{
{ R3x3[0, 0], R3x3[0, 1], R3x3[0, 2], transformer.Translation[0, 0] },
{ R3x3[1, 0], R3x3[1, 1], R3x3[1, 2], transformer.Translation[0, 1] },
{ R3x3[2, 0], R3x3[2, 1], R3x3[2, 2], transformer.Translation[0, 2] },
{ 0, 0, 0, 1 },
});
CvMat A1 = new CvMat(4, 3, MatrixType.F64C1, new double[, ]
{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 0 },
{ 0, 0, 1 }
});
CvMat A2 = new CvMat(3, 4, MatrixType.F64C1, new double[, ]
{
{ transformer.Intrinsic[0, 0], 0, transformer.Intrinsic[0, 2], 0 },
{ 0, transformer.Intrinsic[1, 1], transformer.Intrinsic[1, 2], 0 },
{ 0, 0, 1, 0 },
});
CvMat InvTransformationMatrix = (A2 * (RxT * A1));
InvTransformationMatrix.Invert(transformer.TransformationMatrix);
}
/// <summary>
/// 最小二乗法で解を得ます
/// </summary>
/// <returns></returns>
public double[] Solve()
{
CvMat invLeft = CvEx.InitCvMat(_left);
_left.Invert(invLeft, InvertMethod.Cholesky);
CvMat ret = invLeft * _right;
return(ret.Select(r => r.Val0).ToArray());
}
