Changing the code to use some built in OpenCV functions that execute faster than my own implementations.
Did a little house keeping. Everything works at this point, so restore here if the next optimization breaks it all horibly.