fixed indexing (reshape in openCV is c,r) and added some testing code