public static double CalcError(Mat3 A, Vector3 x, Vector3 b)
{
Vector3 vtmp = MatUtils.VMul(A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static double SolveLeastSquares(Mat3 a, Vector3 b, out Vector3 x, double svd_tol, int svd_sweeps, double pinv_tol)
{
var at = MatUtils.Transpose(a);
var ata = MatUtils.MmulAta(a);
var atb = MatUtils.VMul(at, b);
return(SolveSymmetric(ata, atb, out x, svd_tol, svd_sweeps, pinv_tol));
}
public static double CalcError(SMat3 origA, Vector3 x, Vector3 b)
{
var A = new Mat3();
A.SetSymmetric(origA);
var vtmp = MatUtils.VMul(A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static double SolveSymmetric(SMat3 A, Vector3 b, out Vector3 x, double svd_tol, int svd_sweeps, double pinv_tol)
{
var V = new Mat3();
var VTAV = new SMat3();
GetSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps);
var pinv = PseudoInverse(VTAV, V, pinv_tol);
x = MatUtils.VMul(pinv, b);
return(CalcError(A, x, b));
}