Add basic pinhole cam implementation. A really good idea is to put some kind of tests for this part of the code