Add missing semicolon
Move “constants” from h to cpp
Add check for empty mat
Add scaling the results back up
Add debug option to code
Change coordinate back to camera system
TODO comment