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