public static double calcError(DMat3 A, Vec3 x, Vec3 b)
{
Vec3 vtmp;
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static double calcError(SMat3 origA, Vec3 x, Vec3 b)
{
var A = new DMat3();
Vec3 vtmp;
A.setSymmetric(origA);
MatUtils.vmul(out vtmp, A, x);
vtmp = b - vtmp;
return(vtmp.Dot(vtmp));
}
public static double solveLeastSquares(DMat3 a, Vec3 b, Vec3 x, double svd_tol, int svd_sweeps,
double pinv_tol)
{
DMat3 at;
SMat3 ata;
Vec3 atb;
MatUtils.transpose(out at, a);
MatUtils.mmul_ata(out ata, a);
MatUtils.vmul(out atb, at, b);
return(solveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol));
}
public static double solveSymmetric(SMat3 A, Vec3 b, Vec3 x, double svd_tol, int svd_sweeps,
double pinv_tol)
{
DMat3 pinv;
var V = new DMat3();
var VTAV = new SMat3();
getSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps);
pseudoinverse(out pinv, VTAV, V, pinv_tol);
MatUtils.vmul(out x, pinv, b);
return(calcError(A, x, b));
}
