P_GRAY doesn't match intensity_img

[wiedemannc]

It looks like P_GRAY is not fitting to intensity_img. For the books dataset, I get the following:

>> P_GRAY

P_GRAY =

   1.0e+03 *

    1.1099         0    0.7471         0
         0    1.1099    0.6665         0
         0         0    0.0010         0


>> size(intensity_img)

ans =

   610   810

This would mean that the main point (747, 666) is outside the image! With a modified P_GRAY I can reproduce the tof_depth_mapped image:

P_GRAY_test =

  554.9590         0  373.5359         0
         0  554.9590  333.2608         0
         0         0    1.0000         0