public static float CalcError(Mat3 A, Vector3 x, Vector3 b)
{
Vector3 vtmp = new Vector3();
vtmp = MatUtils.Vmul(A, x);
//vtmp = b - vtmp;
vtmp = VecUtils.Sub(b, vtmp);
return(Vector3.Dot(vtmp, vtmp));
}
public static float solveLeastSquares(Mat3 a, Vector3 b, Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 at = new Mat3();
SMat3 ata = new SMat3();
Vector3 atb = new Vector3();
MatUtils.Transpose(at, a);
MatUtils.Mmul_ata(ata, a);
atb = MatUtils.Vmul(at, b);
return(Svd.SolveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol));
}
public static float CalcError(SMat3 origA, Vector3 x, Vector3 b)
{
Mat3 A = new Mat3();
Vector3 vtmp = new Vector3();
A.SetSymmetric(origA);
vtmp = MatUtils.Vmul(A, x);
vtmp = VecUtils.Sub(b, vtmp);
//vtmp = b - vtmp;
return(Vector3.Dot(vtmp, vtmp));
}
public static float SolveSymmetric(SMat3 A, Vector3 b, Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 pinv = new Mat3();
Mat3 V = new Mat3();
SMat3 VTAV = new SMat3();
GetSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps);
Pseudoinverse(pinv, VTAV, V, pinv_tol);
x = MatUtils.Vmul(pinv, b);
return(CalcError(A, x, b));
}
public static void GetSymmetricSvd(SMat3 a, SMat3 vtav, Mat3 v, float tol, int max_sweeps)
{
vtav.SetSymmetric(a);
v.Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
float delta = tol * MatUtils.Fnorm(vtav);
for (int i = 0; i < max_sweeps && MatUtils.Off(vtav) > delta; ++i)
{
Rotate01(vtav, v);
Rotate02(vtav, v);
Rotate12(vtav, v);
}
}