Преобразование кадра предварительного просмотра в растровое изображение

Я знаю, что тема была на доске много раз, но я все равно не могу заставить ее работать ... Я хочу сохранить рамки просмотра из превью в файлы jpeg. Выглядит это более или менее (код упрощен - без дополнительной логики, исключений и т. Д.) Вот так ...

public void onPreviewFrame(byte[] data, Camera camera) {
  int width = camera.getParameters().getPreviewSize().width;
  int height = camera.getParameters().getPreviewSize().height;


  final int[] rgb = decodeYUV420SP(data, width, height);

  Bitmap bmp = Bitmap.createBitmap(rgb, width, height,Bitmap.Config.ARGB_8888);

  String filename="/sdcard/file" + (index++)+ ".jpg"; 
  FileOutputStream out;
  out = new FileOutputStream(filename);
  bmp.compress(Bitmap.CompressFormat.JPEG, 90, out);
  out.flush();
  out.close();
  out=null;

 }

Вот один из методов, которые я пытался преобразовать (полагаю, с этой платы)

public int[] decodeYUV420SP( byte[] yuv420sp, int width, int height) {   

    final int frameSize = width * height;   

    int rgb[]=new int[width*height];   
    for (int j = 0, yp = 0; j < height; j++) {   
        int uvp = frameSize + (j >> 1) * width, u = 0, v = 0;   
        for (int i = 0; i < width; i++, yp++) {   
            int y = (0xff & ((int) yuv420sp[yp])) - 16;   
            if (y < 0) y = 0;   
            if ((i & 1) == 0) {   
                v = (0xff & yuv420sp[uvp++]) - 128;   
                u = (0xff & yuv420sp[uvp++]) - 128;   
            }   

            int y1192 = 1192 * y;   
            int r = (y1192 + 1634 * v);   
            int g = (y1192 - 833 * v - 400 * u);   
            int b = (y1192 + 2066 * u);   

            if (r < 0) r = 0; else if (r > 262143) r = 262143;   
            if (g < 0) g = 0; else if (g > 262143) g = 262143;   
            if (b < 0) b = 0; else if (b > 262143) b = 262143;   

            rgb[yp] = 0xff000000 | ((r << 6) & 0xff0000) | ((g >> 2) &    
    0xff00) | ((b >> 10) & 0xff);   


        }   
    }   
    return rgb;   
    } 

Проблема в что картинка всегда похожа на три «странные зеленые картинки» ... Я новый пользователь, поэтому я не могу опубликовать его: (

Я не знаю, связано ли это с размером или чем-то еще, но я застрял ... Можете ли вы поддержать меня?