package com.yeyupiaoling.teststadiometry;

import android.graphics.Bitmap;

import org.opencv.calib3d.Calib3d;
import org.opencv.calib3d.StereoBM;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;

import static org.opencv.android.Utils.bitmapToMat;
import static org.opencv.android.Utils.matToBitmap;

public class StereoBMUtil {
    private static final String TAG = StereoBMUtil.class.getName();
    // 【需要根据摄像头修改参数】
    private final int imageWidth = 1280;                      // 单目图像的宽度
    private final int imageHeight = 720;                      // 单目图像的高度
    private Mat Q = new Mat();

    //映射表
    private Mat mapLx = new Mat();
    private Mat mapLy = new Mat();
    private Mat mapRx = new Mat();
    private Mat mapRy = new Mat();

    private StereoBM bm = StereoBM.create();
    private Mat xyz;

    public StereoBMUtil() {
        Mat cameraMatrixL = new Mat(3, 3, CvType.CV_64F);
        Mat distCoeffL = new Mat(5, 1, CvType.CV_64F);
        Mat cameraMatrixR = new Mat(3, 3, CvType.CV_64F);
        Mat distCoeffR = new Mat(5, 1, CvType.CV_64F);
        Mat T = new Mat(3, 1, CvType.CV_64F);
        Mat rec = new Mat(3, 1, CvType.CV_64F);
        // 【需要根据摄像头修改参数】左目相机标定参数 fc_left_x  0  cc_left_x  0  fc_left_y  cc_left_y  0  0  1
        cameraMatrixL.put(0, 0, 849.38718, 0, 720.28472, 0, 850.60613, 373.88887, 0, 0, 1);
        //【需要根据摄像头修改参数】左目相机标定参数 kc_left_01,  kc_left_02,  kc_left_03,  kc_left_04,   kc_left_05
        distCoeffL.put(0, 0, 0.01053, 0.02881, 0.00144, 0.00192, 0.00000);
        //【需要根据摄像头修改参数】右目相机标定参数 fc_right_x  0  cc_right_x  0  fc_right_y  cc_right_y  0  0  1
        cameraMatrixR.put(0, 0, 847.54814, 0, 664.36648, 0, 847.75828, 368.46946, 0, 0, 1);
        //【需要根据摄像头修改参数】右目相机标定参数 kc_right_01,  kc_right_02,  kc_right_03,  kc_right_04,   kc_right_05
        distCoeffR.put(0, 0, 0.00905, 0.02094, 0.00082, 0.00183, 0.00000);
        //【需要根据摄像头修改参数】T平移向量
        T.put(0, 0, -59.32102, 0.27563, -0.79807);
        // 【需要根据摄像头修改参数】rec旋转向量
        rec.put(0, 0, -0.00927, -0.00228, -0.00070);

        Size imageSize = new Size(imageWidth, imageHeight);
        Mat R = new Mat();
        Mat Rl = new Mat();
        Mat Rr = new Mat();
        Mat Pl = new Mat();
        Mat Pr = new Mat();
        Rect validROIL = new Rect();
        Rect validROIR = new Rect();
        Calib3d.Rodrigues(rec, R);                                   //Rodrigues变换
        //图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
        Calib3d.stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, Calib3d.CALIB_ZERO_DISPARITY,
                0, imageSize, validROIL, validROIR);
        Imgproc.initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CvType.CV_32FC1, mapLx, mapLy);
        Imgproc.initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CvType.CV_32FC1, mapRx, mapRy);

        int blockSize = 18;
        int numDisparities = 11;
        int uniquenessRatio = 5;
        bm.setBlockSize(2 * blockSize + 5);                           //SAD窗口大小
        bm.setROI1(validROIL);                                        //左右视图的有效像素区域
        bm.setROI2(validROIR);
        bm.setPreFilterCap(61);                                       //预处理滤波器
        bm.setMinDisparity(32);                                       //最小视差，默认值为0, 可以是负值，int型
        bm.setNumDisparities(numDisparities * 16);                    //视差窗口，即最大视差值与最小视差值之差,16的整数倍
        bm.setTextureThreshold(10);
        bm.setUniquenessRatio(uniquenessRatio);                       //视差唯一性百分比,uniquenessRatio主要可以防止误匹配
        bm.setSpeckleWindowSize(100);                                 //检查视差连通区域变化度的窗口大小
        bm.setSpeckleRange(32);                                       //32视差变化阈值，当窗口内视差变化大于阈值时，该窗口内的视差清零
        bm.setDisp12MaxDiff(-1);
    }

    public Bitmap compute(Bitmap left, Bitmap right) {
        Mat rgbImageL = new Mat();
        Mat rgbImageR = new Mat();
        Mat grayImageL = new Mat();
        Mat rectifyImageL = new Mat();
        Mat rectifyImageR = new Mat();
        Mat grayImageR = new Mat();
        //用于存放每个像素点距离相机镜头的三维坐标
        xyz = new Mat();
        Mat disp = new Mat();
        bitmapToMat(left, rgbImageL);
        bitmapToMat(right, rgbImageR);
        Imgproc.cvtColor(rgbImageL, grayImageL, Imgproc.COLOR_BGR2GRAY);
        Imgproc.cvtColor(rgbImageR, grayImageR, Imgproc.COLOR_BGR2GRAY);

        Imgproc.remap(grayImageL, rectifyImageL, mapLx, mapLy, Imgproc.INTER_LINEAR);
        Imgproc.remap(grayImageR, rectifyImageR, mapRx, mapRy, Imgproc.INTER_LINEAR);

        bm.compute(rectifyImageL, rectifyImageR, disp);                    //输入图像必须为灰度图
        Calib3d.reprojectImageTo3D(disp, xyz, Q, true);  //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16
        Core.multiply(xyz, new Mat(xyz.size(), CvType.CV_32FC3, new Scalar(16, 16, 16)), xyz);

        // 用于显示处理
        Mat disp8U = new Mat(disp.rows(), disp.cols(), CvType.CV_8UC1);
        disp.convertTo(disp, CvType.CV_32F, 1.0 / 16);               //除以16得到真实视差值
        Core.normalize(disp, disp8U, 0, 255, Core.NORM_MINMAX, CvType.CV_8U);
        Imgproc.medianBlur(disp8U, disp8U, 9);
        Bitmap resultBitmap = Bitmap.createBitmap(disp8U.cols(), disp8U.rows(), Bitmap.Config.ARGB_8888);
        matToBitmap(disp8U, resultBitmap);
        return resultBitmap;
    }

    public double[] getCoordinate(int dstX, int dstY) {
        double x = xyz.get(dstY, dstX)[0];
        double y = xyz.get(dstY, dstX)[1];
        double z = xyz.get(dstY, dstX)[2];
        return new double[]{x, y, z};
    }
}