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));
}