public static float Solve(
Mat3 ata, Vec3 atb, Vec4 pointaccum, out Vec3 x)
{
Vec3 masspoint =
new Vec3(pointaccum.x, pointaccum.y, pointaccum.z) /
pointaccum.w;
atb -= Svd.VmulSym(ata, masspoint);
Svd.SolveAtaAtb(ata, atb, out x);
float result = CalcError(ata, x, atb);
x += masspoint;
return(result);
}
public static float CalcError(Mat3 a, Vec3 x, Vec3 b)
{
Vec3 vtmp = b - Svd.VmulSym(a, x);
return(Vec3.Dot(vtmp, vtmp));
}
