/// <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());
}
void drawUndistortedCornerFrame(CvMat displayMat, CvPoint2D32f[] corners, CvSize boardSize)
{
CvMat cornerMat = new CvMat(1, corners.Length, MatrixType.F32C2);
CvEx.FillCvMat(cornerMat, corners.Select(x => new CvScalar(x.X, x.Y)).ToList());
CvMat undistMat = CvEx.InitCvMat(cornerMat);
Cv.UndistortPoints(cornerMat, undistMat, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true), null, this.UndistortionData.CameraStruct.CreateCvMat());
CvEx.DrawChessboardCornerFrame(displayMat, boardSize, undistMat.Select(x => new CvPoint2D32f(x.Val0, x.Val1)).ToArray(), new CvScalar(216, 216, 216));
}
/*
* public static Microsoft.Xna.Framework.Color ToXnaColor(this CvColor color)
* {
* return new Microsoft.Xna.Framework.Color(color.R, color.G, color.B);
* }
*/
/// <summary>
/// 最小二乗法を解く．doubleのリストをCvMatに変換してCv.Solveを呼び出し，結果をdoubleの配列で返します．
/// </summary>
/// <param name="left">方程式左辺の線形結合の係数の配列の列挙．配列長が一定でなければ例外を投げます</param>
/// <param name="right">方程式の右辺の列挙．左辺の列挙数と異なる場合は例外を投げます</param>
/// <param name="method">逆行列を解くときの手段</param>
/// <exception cref="ArgumentException">入力サイズに不一致がある</exception>
/// <returns></returns>
public static double[] Solve(IEnumerable <double[]> left, IEnumerable <double> right, InvertMethod method)
{
double[][] mat = left.ToArray();
if (mat.Length == 0)
{
return(null);
}
int len = mat[0].Length;
CvMat lMat = new CvMat(mat.Length, len, MatrixType.F64C1);
int i = 0;
foreach (double[] arr in mat)
{
if (len != arr.Length)
{
throw new ArgumentException("left must be matrix");
}
for (int k = 0; k < len; k++)
{
lMat[i, k] = arr[k];
}
i++;
}
int j = 0;
CvMat rMat = new CvMat(mat.Length, 1, MatrixType.F64C1);
foreach (double r in right)
{
if (j >= mat.Length)
{
throw new ArgumentException("right length mismatches");
}
rMat[j, 0] = r;
j++;
}
if (j != mat.Length)
{
throw new ArgumentException("right length mismatches");
}
CvMat ans = new CvMat(len, 1, MatrixType.F64C1);
if (!Cv.Solve(lMat, rMat, ans, method))
{
return(null);
}
return(ans.Select(x => x.Val0).ToArray());
}
/*
public static Microsoft.Xna.Framework.Color ToXnaColor(this CvColor color)
{
return new Microsoft.Xna.Framework.Color(color.R, color.G, color.B);
}
*/
/// <summary>
/// 最小二乗法を解く．doubleのリストをCvMatに変換してCv.Solveを呼び出し，結果をdoubleの配列で返します．
/// </summary>
/// <param name="left">方程式左辺の線形結合の係数の配列の列挙．配列長が一定でなければ例外を投げます</param>
/// <param name="right">方程式の右辺の列挙．左辺の列挙数と異なる場合は例外を投げます</param>
/// <param name="method">逆行列を解くときの手段</param>
/// <exception cref="ArgumentException">入力サイズに不一致がある</exception>
/// <returns></returns>
public static double[] Solve(IEnumerable<double[]> left, IEnumerable<double> right, InvertMethod method)
{
double[][] mat = left.ToArray();
if (mat.Length == 0)
return null;
int len = mat[0].Length;
CvMat lMat = new CvMat(mat.Length, len, MatrixType.F64C1);
int i = 0;
foreach (double[] arr in mat)
{
if (len != arr.Length)
{
throw new ArgumentException("left must be matrix");
}
for (int k = 0; k < len; k++)
{
lMat[i, k] = arr[k];
}
i++;
}
int j = 0;
CvMat rMat = new CvMat(mat.Length, 1, MatrixType.F64C1);
foreach (double r in right)
{
if (j >= mat.Length)
{
throw new ArgumentException("right length mismatches");
}
rMat[j, 0] = r;
j++;
}
if (j != mat.Length)
{
throw new ArgumentException("right length mismatches");
}
CvMat ans = new CvMat(len, 1, MatrixType.F64C1);
if (!Cv.Solve(lMat, rMat, ans, method))
return null;
return ans.Select(x => x.Val0).ToArray();
}
