computeNormalAndWeight uses intrinparam wrongly

[Masterxilo]

The method uses the focal length fx, fy in a wrong way to unproject points:

x_yp1.x = x_yp1.z * (x - intrinparam.z) * intrinparam.x;
x_yp1.y = x_yp1.z * ((y + 1.0f) - intrinparam.w) * intrinparam.y;

This is only valid when intrinparam.x and intrinparam.y have been inverted (so that they are 1/fx, 1/fy). But this does not happen anywhere in the calls to that function. As a result, the x_y.xy values are extremely large.

The fix is to compute

Vector4f projParams = projectionParamsSimple.all;
Vector4f invProjParams = projParams;
invProjParams.x = 1.0f / invProjParams.x;
invProjParams.y = 1.0f / invProjParams.y;

like AllocateSceneFromDepth does for example and then pass those.

Or we could correct the formula to:

x_yp1.x = x_yp1.z * (x - intrinparam.z) / intrinparam.x;
x_yp1.y = x_yp1.z * ((y + 1.0f) - intrinparam.w) / intrinparam.y;

Interestingly, the TRACKER_WICP still seems to work fine.

[Masterxilo]

Screenshot with the fix:

(not sure if it should look this way, but the tracker also works with the fix)

Without the fix:

[carlren]

@victorprad @olafkaehler blame is on me, please fix :)

WICP can still work since we have a min weight threshold.