#include "opencv2/opencv.hpp"
#include <string>
#include <iostream>

using namespace cv;
using namespace std;

int main(){
    Mat frame;
    string imgname;
    int f = 1;

    VideoCapture inputVideo(0);
    if (!inputVideo.isOpened()){
        cout << "Could not open the input video " << endl;
        return -1;
    }
    else{
        cout << "video is opened!" << endl;
    }

    while (1){
        inputVideo >> frame;              
        if (frame.empty()) continue;         
        imshow("Camera", frame);
        char key = waitKey(1);
        if (key == 27) break;
        if (key == 'q' || key == 'Q'){
            imgname = to_string(f++) + ".jpg";
            imwrite(imgname, frame);
        }
    }
    cout << "Finished writing" << endl;
    return 0;
}

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

Mat image, img_gray;
int BOARDSIZE[2]{ 6,9 };//棋盘格每行每列角点个数
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/titan/Calibration/image/pictures/*.jpg";//待处理图路径    
    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]);
        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(200);

    }

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

    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;

    ///*
    //畸变图像校准
    //*/
    Mat src, dst;
    src = imread("/home/titan/Calibration/image/pictures/02.jpg");  //读取校正前图像
    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/titan/Calibration/image/pictures/002.jpg", dst);  //校正后图像

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

#include<iostream>
#include <ctime> 
#include<opencv2/opencv.hpp>

using namespace cv;
using namespace std;

int main()
{
    VideoCapture inputVideo(0);
    if(!inputVideo.isOpened()){
        std::cout << "video is not opened\n\n"<<endl;
    }
    else{
        std::cout << "video is opened \n\n"<<endl;
    }
//  Matlab 标定的相机参数
    Mat frame, frameCalibration;
    inputVideo >> frame;
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0,0) = 1982.56844306278;
    cameraMatrix.at<double>(0,1) = 1.79099355543064;
    cameraMatrix.at<double>(0,2) = 1042.90384922068;
    cameraMatrix.at<double>(1,1) = 1983.84445594899;
    cameraMatrix.at<double>(1,2) = 480.442502729538;

    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0,0) = -0.515906663211726;
    distCoeffs.at<double>(1,0) =  0.201811855093355;
    distCoeffs.at<double>(2,0) =  0.00228453839673728;
    distCoeffs.at<double>(3,0) = -0.00134697993045861;
    distCoeffs.at<double>(4,0) = -0.0572379026696125;

/*  C++程序标定的相机参数
    Mat frame, frameCalibration;
    inputVideo >> frame;
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0,0) = 1978.304376178962;
    cameraMatrix.at<double>(0,1) =                   0;
    cameraMatrix.at<double>(0,2) = 1044.639043480329;
    cameraMatrix.at<double>(1,1) = 1979.71454820083;
    cameraMatrix.at<double>(1,2) = 482.6287237060178;
    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0,0) = -0.5277684150872038;
    distCoeffs.at<double>(1,0) =  0.2663992436241138;
    distCoeffs.at<double>(2,0) = -0.001857829391420174;
    distCoeffs.at<double>(3,0) = -0.002175774665050042;
    distCoeffs.at<double>(4,0) = -0.1007311729522544;
*/

    Mat view, rview, map1, map2;
    Size image_Size;
    image_Size = frame.size();

    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), cameraMatrix, image_Size, CV_16SC2, map1, map2);
    // initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, image_Size, 0.5, image_Size, 0),image_Size, CV_16SC2, map1, map2);

    while(1){
        inputVideo >> frame;
        if(frame.empty()) break;
        remap(frame, frameCalibration, map1, map2, INTER_LINEAR);
        imshow("Original_image",frame);
        imshow("Calibrated_image", frameCalibration);
        char key =waitKey(1);
        if(key == 27 || key == 'q' || key == 'Q') break;
    }


    return 0;
}