Fixed RGBD -> World corordinates + Visualization