Add full test coverage for corner_subpix and fix bug