Changes done to Transformation into camera coorinate system. Due to Johans comments.