Fixed bugs in camera matrix. Also added some unit tests