//
// Created by zx on 22-11-1.
//

#include <opencv2/imgproc/types_c.h>
#include<opencv2/opencv.hpp>
#include<iostream>
using namespace cv;
using namespace std;

cv::Mat undistored_image(cv::Mat image,Mat cameraMatrix, Mat distCoeffs)
{
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    double cx=cameraMatrix.at<double>(0,2);
    double cy=cameraMatrix.at<double>(1,2);
    double fx=cameraMatrix.at<double>(0,0);
    double fy=cameraMatrix.at<double>(1,1);

    double k1=distCoeffs.at<double>(0,0);
    double k2=distCoeffs.at<double>(0,1);
    double p1=distCoeffs.at<double>(0,2);
    double p2=distCoeffs.at<double>(0,3);
    double k3=distCoeffs.at<double>(0,4);

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++) {
        for (int u = 0; u < cols; u++) {
            // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
            double x = (u - cx) / fx, y = (v - cy) / fy;
            double r = sqrt(x * x + y * y);
            double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r+k3 *r*r* r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
            double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r+k3 *r*r* r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
            double u_distorted = fx * x_distorted + cx;
            double v_distorted = fy * y_distorted + cy;

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }
    }
    return image_undistort;

}

Mat image, img_gray;
int BOARDSIZE[2]{ 8,11 };//棋盘格每行每列角点个数
int main()
{
    vector<vector<Point3f>> objpoints_img;//保存棋盘格上角点的三维坐标
    vector<Point3f> obj_world_pts;//三维世界坐标
    vector<vector<Point2f>> images_points;//保存所有角点
    vector<Point2f> img_corner_points;//保存每张图检测到的角点
    vector<String> images_path;//创建容器存放读取图像路径

    string image_path = "/home/zx/doc/private_hub/triangle/images/标定数据/上方/all/*.bmp";//待处理图路径
    glob(image_path, images_path);//读取指定文件夹下图像

    //转世界坐标系
    for (int i = 0; i < BOARDSIZE[1]; i++)
    {
        for (int j = 0; j < BOARDSIZE[0]; j++)
        {
            obj_world_pts.push_back(Point3f(j, i, 0));
        }
    }

    for (int i = 0; i < images_path.size(); i++)
    {
        image = imread(images_path[i],0);
        img_gray=image;
        //cvtColor(image, img_gray, COLOR_BGR2GRAY);
        //检测角点
        bool found_success = findChessboardCorners(img_gray, Size(BOARDSIZE[0], BOARDSIZE[1]),
                                                   img_corner_points,
                                                   CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

        //显示角点
        if (found_success)
        {
            //迭代终止条件
            TermCriteria criteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001);

            //进一步提取亚像素角点
            cornerSubPix(img_gray, img_corner_points, Size(11, 11),
                         Size(-1, -1), criteria);

            //绘制角点
            drawChessboardCorners(image, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points,
                                  found_success);

            objpoints_img.push_back(obj_world_pts);//从世界坐标系到相机坐标系
            images_points.push_back(img_corner_points);
        }
        //char *output = "image";
        char text[] = "image";
        char *output = text;
        //imshow(output, image);
       // waitKey(0);

    }

    /*
    计算内参和畸变系数等
    */

    Mat cameraMatrix, distCoeffs, R, T;//内参矩阵，畸变系数，旋转量，偏移量
    calibrateCamera(objpoints_img, images_points, img_gray.size(),
                    cameraMatrix, distCoeffs, R, T);

    cout << "cameraMatrix:" << endl;
    cout << cameraMatrix << endl;

    cout << "*****************************" << endl;
    cout << "distCoeffs:" << endl;
    cout << distCoeffs << endl;
    cout << "*****************************" << endl;

    cout << "Rotation vector:" << endl;
    cout << R << endl;

    cout << "*****************************" << endl;
    cout << "Translation vector:" << endl;
    cout << T << endl;

  FileStorage fwrite("../cfg/cameraMatrix.yaml", FileStorage::WRITE);
  //存入矩阵Mat类型的数据
   fwrite.write("cameraMatrix", cameraMatrix);
  fwrite.write("distCoeffs", distCoeffs);

    ///*
    //畸变图像校准
    //*/
    Mat src, dst;
    src = imread("/home/zx/doc/private_hub/triangle/images/标定数据/上方/all/down_1.bmp",0);  //读取校正前图像
    undistort(src, dst, cameraMatrix, distCoeffs);

    char texts[] = "image_dst";
    char *dst_output = texts;
    //char *dst_output = "image_dst";
    imshow(dst_output, dst);
    waitKey(100);
    imwrite("/home/zx/doc/private_hub/triangle/images/undistored.bmp", dst);  //校正后图像

    cv::Mat undist=undistored_image(src,cameraMatrix,distCoeffs);
    imwrite("/home/zx/doc/private_hub/triangle/images/undistored_1.bmp", undist);  //校正后图像

    cv::Mat diff=undist-dst;
    imwrite("/home/zx/doc/private_hub/triangle/images/diff.bmp", diff);  //

    destroyAllWindows();//销毁显示窗口
    return 0;
}