public static float CalcError(Mat3 a, Vector3 x, Vector3 b)
{
Vector3 vtmp = a.Vmul(x);
vtmp = b - vtmp;
return(vtmp.x * vtmp.x + vtmp.y * vtmp.y + vtmp.z * vtmp.z);
}
public static float CalcError(SMat3 origA, Vector3 x, Vector3 b)
{
Mat3 A = new Mat3();
A.SetSymmetric(origA);
Vector3 vtmp = A.Vmul(x);
vtmp = b - vtmp;
return(vtmp.x * vtmp.x + vtmp.y * vtmp.y + vtmp.z * vtmp.z);
}
public static float SolveSymmetric(SMat3 A, Vector3 b, ref Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 mtmp = new Mat3(), pinv = new Mat3(), V = new Mat3();
SMat3 VTAV = new SMat3();
GetSymmetricSvd(ref A, ref VTAV, ref V, svd_tol, svd_sweeps);
pinv = PseudoInverse(VTAV, V, pinv_tol);
x = pinv.Vmul(b);
return(CalcError(A, x, b));
}
float SolveLeastSquares(Mat3 a, Vector3 b, ref Vector3 x, float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 at = new Mat3();
SMat3 ata = new SMat3();
Vector3 atb = new Vector3();
at = a.Transpose();
ata = a.MulATA();
atb = at.Vmul(b);
return(SolveSymmetric(ata, atb, ref x, svd_tol, svd_sweeps, pinv_tol));
}