// camCalib.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <vector>
#include "sourceCode/MonoLaserCalibrate.h"
#include "sourceCode/FitMapParam.h"

#define _DO_CAMERA_CALIB    1

//version 1.0.0 : base version released
std::string	m_strVersion = "camCalib 1.0.0";
const char* wd_camCalibVersion(void)
{
	return m_strVersion.c_str();
}

void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
{
    std::ofstream sw(fileName);

    for (int i = 0; i < fitMap.rows; i++)
    {
        // 存储拟合结果
        double K[FittingOrder + 1];
        for (int _c = 0; _c <= FittingOrder; _c++)
            K[_c] = fitMap.ptr<float>(i)[_c];
        char dataStr[250];
        //sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g, %g, %g, %g", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sw << dataStr << std::endl;
    }
    sw.close();
    return;
}

void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe)
{
    std::ofstream sw(fileName);

    char dataStr[250];
    for (int i = 0; i < K.rows; i++)
    {
        // 存储旋转矩阵
        double temp[3];
        for (int _c = 0; _c < 3; _c++)
            temp[_c] = K.ptr<double>(i)[_c];

        //sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
        sprintf_s(dataStr, 250, "%g, %g, %g", temp[0], temp[1], temp[2]);
        sw << dataStr << std::endl;
    }
    double temp[5];
    for (int _c = 0; _c < 5; _c++)
        temp[_c] = D.ptr<double>(0)[_c];
    sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]);
    sw << dataStr << std::endl;

    //ax+by+cz+d = 0
    float a = (float)pe[0];
    float b = (float)pe[1];
    float c = (float)pe[2];
    float d = (float)pe[3];
    //将c变成-1，转成z=ax+by+c的形式，使用3个参数
    a = -a / c;
    b = -b / c;
    d = -d / c;
    c = -1;
    sprintf_s(dataStr, 250, "%g, %g, %g", a, b, d);
    sw << dataStr << std::endl;

    sw.close();
    return;
}

void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
{
    K = cv::Mat::zeros(3, 3, CV_64FC1);
    D = cv::Mat::zeros(1, 5, CV_64FC1);
    std::ifstream inputFile(fileName);
    std::string linedata;

    if (inputFile.is_open() == false)
        return;

    int line = 0;
    while (getline(inputFile, linedata))
    {
        if (line < 3)
        {
            double data[3];
            sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &data[0], &data[1], &data[2]);
            for (int _c = 0; _c < 3; _c++)
                K.ptr<double>(line)[_c] = data[_c];
        }
        else if(line == 3)
        {
            double data[5];
            sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf", &data[0], &data[1], &data[2], &data[3], &data[4]);
            for (int _c = 0; _c < 5; _c++)
                D.ptr<double>(0)[_c] = data[_c];
        }
        line++;
    }
    inputFile.close();
    return;
}

void saveSubpixData(char* filename, std::vector<cv::Point2f>& subpixPnt)
{
    if (subpixPnt.size() < 6)
        return;

    std::ofstream TXTFile(filename);
    char TXTData[250];
    int headOffset = 6;
    for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
    {
        cv::Point2f a_subPix = subpixPnt[i];
        snprintf(TXTData, sizeof(TXTData), "%.5f %.5f", a_subPix.x, a_subPix.y);
        TXTFile << TXTData << std::endl;
    }
    TXTFile.close();
}


typedef struct
{
    int nMin;	//< 最小值
    int nMax;	//< 最大值
} SWdNLRange;

#define CALIB_CHESS_BOARD   1
#define CALIB_CIRCLE_GRID   2
#define CALIB_CHARUCO       3
#define CALIB_BALL_POLE     4

void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R,
    const cv::Mat& newK, const cv::Size& size, cv::Mat& mapX, cv::Mat& mapY) {

    std::vector<cv::Point2f> srcPts;
    for (int r = 0; r < size.height; r++) {
        for (int c = 0; c < size.width; c++) {
            srcPts.push_back(cv::Point2f(c, r));
        }
    }

    std::vector<cv::Point2f> dstPts;
    cv::undistortPoints(srcPts, dstPts, K, D, R, newK);

    mapX = cv::Mat::zeros(size.height, size.width, CV_32FC1);
    mapY = cv::Mat::zeros(size.height, size.width, CV_32FC1);
    int idx = 0;
    for (int r = 0; r < size.height; r++) {
        for (int c = 0; c < size.width; c++) {
            mapX.ptr<float>(r)[c] = dstPts[idx].x;
            mapY.ptr<float>(r)[c] = dstPts[idx].y;
            idx++;
        }
    }

    return;
}

void lineFitting(std::vector< cv::Point2f>& inliers, double* _k, double* _b)
{
    //最小二乘拟合直线参数
    double xx_sum = 0;
    double x_sum = 0;
    double y_sum = 0;
    double xy_sum = 0;
    int num = 0;
    for (int i = 0; i < inliers.size(); i++)
    {
        x_sum += inliers[i].x; //x的累加和
        y_sum += inliers[i].y; //y的累加和
        xx_sum += inliers[i].x * inliers[i].x; //x的平方累加和
        xy_sum += inliers[i].x * inliers[i].y; //x，y的累加和
        num++;
    }
    *_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //根据公式求解k
    *_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
}
//拟合成通用直线方程ax+by+c=0，包括垂直
void lineFitting_abc(std::vector< cv::Point2f>& inliers, double* _a, double* _b, double* _c)
{
    //判断是否为垂直
    int dataSize = (int)inliers.size();
    if (dataSize < 2)
        return;

    double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
    double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
    std::vector< cv::Point2f> fittingData;
    if (deltaX < deltaY)
    {
        //x=ky+b 拟合
        for (int i = 0; i < dataSize; i++)
        {
            cv::Point2f a_fitPt;
            a_fitPt.x = inliers[i].y;
            a_fitPt.y = inliers[i].x;
            fittingData.push_back(a_fitPt);
        }
        double k = 0, b = 0;
        lineFitting(fittingData, &k, &b);
        //ax+by+c
        *_a = 1.0;
        *_b = -k;
        *_c = -b;
    }
    else
    {
        //y = kx+b拟合
        double k = 0, b = 0;
        lineFitting(inliers, &k, &b);
        //ax+by+c
        *_a = k;
        *_b = -1;
        *_c = b;
    }
    return;
}


#define MONO_CALIB_TEST_GROUP 5
int MonoCamLaserCalib()
{
    std::cout << "Hello World!\n";

    const char* calibDataPath[MONO_CALIB_TEST_GROUP] = {
        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\",   //1
        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
        "F:/ShangGu/单目相机标定数据/TestData/", //4
    };
    const SWdNLRange fileIdx[MONO_CALIB_TEST_GROUP] = {
        {3,39},{6,33},{1,33},{1,10},{1,26}
    };
    const int boardType[MONO_CALIB_TEST_GROUP] =
    {
        CALIB_CHESS_BOARD,
        CALIB_CHESS_BOARD,
        CALIB_CIRCLE_GRID,
        CALIB_CHARUCO,
        CALIB_CHARUCO,
    };

    for(int grp = 4; grp <= 4; grp ++)
    {
        //grp = 1;
        int calibType = boardType[grp];
        cv::Size cbPattern;
        float cbSquareSize;
        float markSize;
        if (CALIB_CHESS_BOARD == calibType)
        {
            cbPattern = cv::Size(8, 11); // 10);
            cbSquareSize = 40.0f; // 10.f;
        }
        else if (CALIB_CIRCLE_GRID == calibType)
        {
            cbPattern = cv::Size(7, 7); // 10);
            cbSquareSize = 50.0f; // 10.f;
        }
        else if (CALIB_CHARUCO == calibType)
        {
            cbPattern = cv::Size(9, 9); //cv::Size(47, 21); // 10);
            cbSquareSize = 50.0f;
            markSize = 37.0f;
        }
        else
            continue;

        std::vector<std::vector<cv::Point2f>> cbCornersList;
        std::vector<std::vector<int>> cbCornersIdList;
        cv::Size imageSize;

        int startIndex = fileIdx[grp].nMin;
        int endIndex = fileIdx[grp].nMax;
        int index;
#if _DO_CAMERA_CALIB
        for (index = startIndex; index <= endIndex; index++) {
            char filenameInput[256];
            sprintf_s(filenameInput, "%sImage_%d.png", calibDataPath[grp], index);
            cv::Mat img = cv::imread(filenameInput);
            if (img.empty())
                break;

            char filename[256];
#if 0
            cv::Mat img;
            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
            char rotateFilename[256];
            sprintf_s(rotateFilename, "%sresult/calib_%03d_rotate.bmp", calibDataPath[grp], index);
            cv::String rFileName(rotateFilename);
            cv::imwrite(rFileName, img);
#endif
            std::vector<cv::Point2f> corners;
            if (CALIB_CHESS_BOARD == calibType)
            {
                detectCorners(img, cbPattern, corners);
#if ENABLE_DEBUG
                if (corners.size() > 0)
                {
                    cv::Mat color = img.clone();
                    cv::drawChessboardCorners(color, cbPattern, corners, true);
                    //cv::resize(color, color, cv::Size(), 0.5, 0.5);
                    sprintf_s(filename, "%scalib_%03d_corner.bmp", calibDataPath[grp], index);
                    cv::imwrite(filename, color);
                }
#endif
                if (corners.empty())
                    continue;
                cbCornersList.push_back(corners);
            }
            else if (CALIB_CIRCLE_GRID == calibType)
            {
                detectCirclePoints(img, cbPattern, corners);
#if ENABLE_DEBUG
                if (corners.size() > 0)
                {
                    cv::Mat color = img.clone();
                    cv::drawChessboardCorners(color, cbPattern, corners, true);
                    //cv::resize(color, color, cv::Size(), 0.5, 0.5);
                    sprintf_s(filename, "%sresult/calib_%03d_center.bmp", calibDataPath[grp], index);
                    cv::imwrite(filename, color);
                }
#endif
                if (corners.empty())
                    continue;
                cbCornersList.push_back(corners);
            }
            else if (CALIB_CHARUCO == calibType)
            {
                std::vector<int> markerIds;
                std::vector<std::vector<cv::Point2f> > markerCorners;
                std::vector<int> charucoIds;
                detectCharucoCorners(img,
                    cbPattern,
                    cbSquareSize,
                    markSize,
                    markerIds,
                    markerCorners,
                    charucoIds,
                    corners);
                cv::Mat imageCopy = img.clone();
                if (markerIds.size() > 0)
                {
                    cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds);
                }
                if (charucoIds.size() > 0)
                {
                    cv::aruco::drawDetectedCornersCharuco(imageCopy, corners, charucoIds, cv::Scalar(0, 255, 255));
                }
                char markFilename[256];
                sprintf_s(markFilename, "%sresult/calib_%03d_markers.bmp", calibDataPath[grp], index);
                cv::imwrite(markFilename, imageCopy);

                if (corners.empty())
                    continue;
                cbCornersList.push_back(corners);
                cbCornersIdList.push_back(charucoIds);
                printf("%s... cornerSize=%d, charucoIDSize=%d\n", filenameInput, (int)corners.size(), (int)charucoIds.size());
            }
            imageSize = cv::Size(img.cols, img.rows);
        }

        cv::Mat K, D;
        std::vector<double> reprojectionError;
        if (CALIB_CHARUCO == calibType)
            monocularCalibration_charuco(cbCornersIdList, cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        else
            monocularCalibration_chessboard(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        std::cout << "K:" << std::endl;
        std::cout << K << std::endl;
        std::cout << "D:" << std::endl;
        std::cout << D << std::endl;
        for(int i = 0; i < reprojectionError.size(); i ++)
            std::cout << reprojectionError[i] << std::endl;


        // 输出映射类型，通常使用CV_32FC1或CV_16SC2
        cv::Mat backwardMap_x, backwardMap_y;
        cv::Mat forwardMap_x, forwardMap_y;
        cv::Mat newCamMatrix;
        // 生成畸变矫正映射
#if ENABLE_FISH_EYE
        cv::fisheye::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map1, map2);
#else
        double alpha = 0.4; // 0.4;
        newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);
        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, forwardMap_x, forwardMap_y);
#endif

        // 生成系数表
        cv::Mat fitMap_x = GetFitParamMap(forwardMap_x, 1);
        cv::Mat fitMap_y = GetFitParamMap(forwardMap_y, 1);
        //输出系数文件
        char calibParamName[256];
        sprintf_s(calibParamName, "%sresult/calib_param_x.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_x);
        sprintf_s(calibParamName, "%sresult/calib_param_y.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_y);

        //比较误差
        cv::Mat mapGen_x = GetMapFromFitMap(fitMap_x, imageSize, 1);
        cv::Mat mapGen_y = GetMapFromFitMap(fitMap_y, imageSize, 1);
        //搜索最大和平均误差
            // 计算绝对差异
        cv::Mat diff_x, diff_y;
        cv::absdiff(forwardMap_x, mapGen_x, diff_x);
        cv::absdiff(forwardMap_y, mapGen_y, diff_y);
        // 查找最大值和最小值
        double minVal_x, maxVal_x;
        cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x);
        double minVal_y, maxVal_y;
        cv::minMaxLoc(diff_y, &minVal_y, &maxVal_y);
        // 计算平均值
        cv::Scalar meanVal_x = cv::mean(diff_x);
        cv::Scalar meanVal_y = cv::mean(diff_y);

        std::cout << "X Max_difference: " << maxVal_x << std::endl;
        std::cout << "X Mean difference: " << meanVal_x[0] << std::endl;
        std::cout << "Y Max difference: " << maxVal_y << std::endl;
        std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;

#if 0
        //生成矫正图像
        for (index = startIndex; index <= endIndex; index++) {
            char filename[256];
            sprintf_s(filename, "%sImage_%d.png", calibDataPath[grp], index);
            cv::Mat img = cv::imread(filename);
            if (img.empty())
                break;

            //cv::Mat img;
            //cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
            cv::Mat calibImg;
            cv::remap(img,
                calibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            sprintf_s(filename, "%sresult/calib_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, calibImg);
        }
#endif

#else
        char calibKDName[256];
        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
        cv::Mat K, D;
        sg_readCalibKD(calibKDName, K, D);

        //生成opencv校正表
        cv::Mat backwardMap_x, backwardMap_y;
        double alpha = 0.4; // 0.4;
        imageSize = cv::Size(1200, 2048);
        cv::Mat newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);

#endif

#if ENABLE_GEN_IMAGE
        cv::Vec4f laserPE;
        generateLaserLine(10.f, 5.f, laserPE);
        std::cout << "generateLaserLine pe: " << laserPE << std::endl;

        index = 3;
        for (;; index++) {

            char filename[256];
            sprintf_s(filename, "%s%03d.bmp", cbImagePath, index);
            cv::Mat img = cv::imread(filename);
            if (img.empty())
                break;

            std::vector<cv::Point2f> corners;
            detectCorners(img, cbPattern, corners);
            if (corners.empty())
                continue;

            cv::Vec4f pe;
            fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);

            cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize);

#if ENABLE_DEBUG
            cv::Mat color = image.clone();
            cv::resize(color, color, cv::Size(), 0.5, 0.5);
            cv::imshow("image", color);
            cv::waitKey(10);·
#endif

                sprintf_s(filename, "%s%d.bmp", laserImagePath, index);
            cv::imwrite(filename, image);
        }

#endif

        std::vector<cv::Point3f> all_pts3d;
        for (index = startIndex; index <= endIndex; index++) {

            char filename[256];
            sprintf_s(filename, "%sImage_%d.png", calibDataPath[grp], index);
            cv::Mat img = cv::imread(filename);
            if (img.empty())
                break;

            //cv::Mat img;
            //cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
#if 1
            cv::Mat charucoCalibImg;
            cv::remap(img,
                charucoCalibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            cv::Size _size = charucoCalibImg.size();
            _size.width = _size.width * 5;
            //cv::resize(charucoCalibImg, charucoCalibImg, _size, 0, 0, cv::INTER_NEAREST);
            sprintf_s(filename, "%sresult/calib_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, charucoCalibImg);
#endif

            std::vector<cv::Point2f> corners;
            std::vector<int> charucoIds;
            if (CALIB_CHESS_BOARD == calibType)
            {
                detectCorners(img, cbPattern, corners);
            }
            else if (CALIB_CIRCLE_GRID == calibType)
            {
                detectCirclePoints(img, cbPattern, corners);
            }
            else if (CALIB_CHARUCO == calibType)
            {
                std::vector<int> markerIds;
                std::vector<std::vector<cv::Point2f> > markerCorners;
                detectCharucoCorners(img,
                    cbPattern,
                    cbSquareSize,
                    markSize,
                    markerIds,
                    markerCorners,
                    charucoIds,
                    corners);
            }
            if (corners.empty())
                continue;
        
            // 创建棋盘格区域的掩码
            cv::Mat chessMask;
            if (CALIB_CHARUCO == calibType)
            {
                chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                float roi_L = corners[0].x;
                float roi_R = roi_L;
                float roi_T = corners[0].y;
                float roi_B = roi_T;
                for (int i = 1; i < (int)corners.size(); i++)
                {
                    roi_L = roi_L > corners[i].x ? corners[i].x : roi_L;
                    roi_R = roi_R < corners[i].x ? corners[i].x : roi_R;
                    roi_T = roi_T > corners[i].y ? corners[i].y : roi_T;
                    roi_B = roi_B < corners[i].y ? corners[i].y : roi_B;
                }
                int left = (int)roi_L;
                if (left < 0)
                    left = 0;
                int right = (int)roi_R;
                if (right >= chessMask.cols)
                    right = chessMask.cols - 1;
                int top = (int)roi_T - 100;
                if (top < 0)
                    top = 0;
                int btm = (int)roi_B + 100;
                if (btm >= chessMask.rows)
                    btm = chessMask.rows - 1;

                for (int y = top; y <= btm; y++)
                {
                    for (int x = left; x <= right; x++)
                    {
                        chessMask.at<uchar>(y, x) = 0xff;
                    }
                }
            }
            else
            {
                chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                // 使用多边形近似来填充角点之间的区域
                // 棋盘格区域需要比角点区域大一圈
                std::vector<cv::Point2f> contour_line[4];
                for (int i = 0; i < cbPattern.width; i++)
                {
                    cv::Point2f pt_c = corners[i];
                    cv::Point2f pt_2 = corners[cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[0].push_back(pt_1);
                }
                for (int i = 0; i < cbPattern.height; i++)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[1].push_back(pt_1);
                }
                for (int i = cbPattern.width - 1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
                    cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[2].push_back(pt_1);
                }
                for (int i = cbPattern.height - 1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[3].push_back(pt_1);
                }
                std::vector<cv::Point> contours;
                //生成轮廓点
                for (int n = 0; n < 4; n++)
                {
                    int num = contour_line[n].size();
                    for (int i = 0; i < num; i++)
                        contours.push_back(contour_line[n][i]);
                    cv::Point2f pt_c = contour_line[n][num - 1];
                    cv::Point2f pt_2 = contour_line[n][num - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contours.push_back(pt_1);
                }
                // 使用 fillPoly 填充多边形
                cv::Scalar color(255); // 红色
                cv::fillPoly(chessMask, contours, color);
#if 1
                sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
                cv::imwrite(filename, chessMask);
#endif
            }

            cv::Vec4f pe;
            if (CALIB_CHARUCO == calibType)
                fitChessboardPlane_charuco(charucoIds,corners, K, D, cbPattern, cbSquareSize, pe);
            else
                fitChessboardPlane_chessboard(corners, K, D, cbPattern, cbSquareSize, pe);

            sprintf_s(filename, "%sImage_%d_Laser.png", calibDataPath[grp], index);
            cv::Mat laserImg_unMask = cv::imread(filename);
            if (laserImg_unMask.empty())
                break;

            //cv::Mat laserImg_unMask;
            //cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
            //与Mask相与，保证待处理的激光线在标定板上
            cv::Mat laserImg;
            cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
#if 1
            sprintf_s(filename, "%sresult/laser_mask_%d.png", calibDataPath[grp], index);
            cv::imwrite(filename, laserImg);

            cv::Mat laserCalibImg;
            cv::remap(laserImg,
                laserCalibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            sprintf_s(filename, "%sresult/laser_%d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, laserCalibImg);

            cv::Mat enlargeLaserImg = laserCalibImg.clone();
            cv::Size laserImgSize = enlargeLaserImg.size();
            laserImgSize.width = laserImgSize.width * 5;
            cv::resize(enlargeLaserImg, enlargeLaserImg, laserImgSize, 0, 0, cv::INTER_NEAREST);
            sprintf_s(filename, "%sresult/laser_%d_enlarge_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, enlargeLaserImg);
#endif
            bool multiPeakFlag = false; //每行一个Peak
            std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg,  multiPeakFlag);
#if 0
            //显示亚像素点
            cv::Mat enlargeImg;
            if (laserImg.channels() == 1)
                laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
            else
                enlargeImg = laserImg.clone();
            cv::Size objSize = laserImg.size();
            objSize.width = objSize.width * 5;
            cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
#endif

            // 去畸变
            if (pts2d.size() == 0)
                continue;
            std::vector<cv::Point2f> undistortedPoints;
            cv::undistortPoints(pts2d, undistortedPoints,
                K, D, cv::noArray(), newCamMatrix);

#if 1
            //直线拟合
            double _a, _b, _c;
            lineFitting_abc(undistortedPoints, &_a, &_b, &_c);

            std::vector<cv::Point2f> fittingPts2d;
            for (int i = 0; i < (int)undistortedPoints.size(); i++)
            {
                cv::Point2f a_pt;
                a_pt.y = undistortedPoints[i].y;
                a_pt.x = -(a_pt.y * _b + _c) / _a;
                fittingPts2d.push_back(a_pt);
            }
#else
            std::vector<cv::Point2f> fittingPts2d;
            fittingPts2d.insert(fittingPts2d.end(), undistortedPoints.begin(), undistortedPoints.end());
#endif

            if (fittingPts2d.size() > 0)
            {
                sprintf_s(filename, "%sresult/laser_enlarge_%d_subpixData.txt", calibDataPath[grp], index);
                saveSubpixData(filename, fittingPts2d);
            }

            cv::Mat cloneEnlageImg = enlargeLaserImg.clone();
            for (int i = 0, i_max = (int)undistortedPoints.size(); i < i_max; i++)
            {
                cv::Point2f a_subPix = undistortedPoints[i];
                int row = (int)(a_subPix.y + 0.5);
                int col = (int)(a_subPix.x * 5 + 0.5);
                cloneEnlageImg.at<cv::Vec3b>(row, col)[0] = 0;
                cloneEnlageImg.at<cv::Vec3b>(row, col)[1] = 0;
                cloneEnlageImg.at<cv::Vec3b>(row, col)[2] = 255;
            }
            sprintf_s(filename, "%sresult/laser_enlarge_%d_raw_subpix.png", calibDataPath[grp], index);
            cv::imwrite(filename, cloneEnlageImg);

            for (int i = 0, i_max = (int)fittingPts2d.size(); i < i_max; i++)
            {
                cv::Point2f a_subPix = fittingPts2d[i];
                int row = (int)(a_subPix.y + 0.5);
                int col = (int)(a_subPix.x * 5 + 0.5);
                enlargeLaserImg.at<cv::Vec3b>(row, col)[0] = 0;
                enlargeLaserImg.at<cv::Vec3b>(row, col)[1] = 0;
                enlargeLaserImg.at<cv::Vec3b>(row, col)[2] = 255;
            }
            sprintf_s(filename, "%sresult/laser_enlarge_%d_subpix.png", calibDataPath[grp], index);
            cv::imwrite(filename, enlargeLaserImg);

            std::vector<cv::Point3f> pts3d = project2DTo3D(fittingPts2d, pe, K, D);
            all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
            printf("  %sImage_%d_Laser.png  ... done\n", calibDataPath[grp], index);
        }

        cv::Vec4f laser_pe = fitPlaneToPoints(all_pts3d);
        std::cout << "pe: " << laser_pe << std::endl;

        //output K and D
        char calibKDPName[256];
        sprintf_s(calibKDPName, "%sresult/calib_param_K_D.txt", calibDataPath[grp]);
        sg_outputCalibKD(calibKDPName, K, D, laser_pe);
    }
    return 0;
}

#if 0
#define STEREO_CALIB_TEST_GROUP 1
int StereoCamCalib()
{
    std::cout << "Hello World!\n";

    const char* calibDataPath[STEREO_CALIB_TEST_GROUP] = {
        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
    };
    const SWdNLRange fileIdx[STEREO_CALIB_TEST_GROUP] = {
        {3,39},
    };
    const int boardType[STEREO_CALIB_TEST_GROUP] =
    {
        CALIB_BALL_POLE
    };

    for (int grp = 0; grp < 1; grp++)
    {
        grp = 1;
        int calibType = boardType[grp];
        cv::Size cbPattern;
        float cbSquareSize;
        float markSize;
        if (CALIB_CHARUCO == calibType)
        {
            cbPattern = cv::Size(47, 21); // 10);
            cbSquareSize = 50.0f;
            markSize = 37.0f;
        }
        else if (CALIB_CIRCLE_GRID == calibType)
        {
            cbPattern = cv::Size(7, 7); // 10);
            cbSquareSize = 50.0f; // 10.f;
        }
        else if (CALIB_BALL_POLE == calibType)
        {

        }
        else
            continue;

        std::vector<std::vector<cv::Point2f>> cbCornersListL;
        std::vector<std::vector<int>> cbCornersIdListL;
        std::vector<std::vector<cv::Point2f>> cbCornersListR;
        std::vector<std::vector<int>> cbCornersIdListR;
        cv::Size imageSize;

        int startIndex = fileIdx[grp].nMin;
        int endIndex = fileIdx[grp].nMax;
        int index;
#if _DO_CAMERA_CALIB
        for (index = startIndex; index <= endIndex; index++) 
        {
            char imgFileL[256];
            char imgFileR[256];
            sprintf_s(imgFileL, "%scalib_%03d_L.bmp", calibDataPath[grp], index);
            sprintf_s(imgFileR, "%scalib_%03d_R.bmp", calibDataPath[grp], index);
            cv::Mat srcImgL = cv::imread(imgFileL);
            cv::Mat srcImgR = cv::imread(imgFileR);
            if ( (true == srcImgL.empty()) || (true == srcImgR.empty()))
                continue;

            std::vector<cv::Point2f> cornersL;
            std::vector<cv::Point2f> cornersR;
            if (CALIB_CIRCLE_GRID == calibType)
            {
                detectCirclePoints(srcImgL, cbPattern, cornersL);
                detectCirclePoints(srcImgL, cbPattern, cornersR);
                if ( (true == cornersL.empty()) || (true == cornersR.empty()))
                    continue;

#if ENABLE_DEBUG
                {
                    cv::Mat colorL = srcImgL.clone();
                    cv::Mat colorR = srcImgR.clone();
                    cv::drawChessboardCorners(colorL, cbPattern, cornersL, true);
                    cv::drawChessboardCorners(colorR, cbPattern, cornersR, true);
                    sprintf_s(imgFileL, "%sdebug_%03d_gridL.bmp", calibDataPath[grp], index);
                    sprintf_s(imgFileR, "%sdebug_%03d_gridR.bmp", calibDataPath[grp], index);
                    cv::imwrite(imgFileL, colorL);
                    cv::imwrite(imgFileR, colorR);
                }
#endif
                cbCornersListL.push_back(cornersL);
                cbCornersListR.push_back(cornersR);
            }
            else if (CALIB_CHARUCO == calibType)
            {
                std::vector<int> markerIds_L;
                std::vector<int> markerIds_R;
                std::vector<std::vector<cv::Point2f> > markerCorners_L;
                std::vector<std::vector<cv::Point2f> > markerCorners_R;
                std::vector<int> charucoIds_L;
                std::vector<int> charucoIds_R;
                detectCharucoCorners(srcImgL,
                    cbPattern,
                    cbSquareSize,
                    markSize,
                    markerIds_L,
                    markerCorners_L,
                    charucoIds_L,
                    cornersL);
                detectCharucoCorners(srcImgR,
                    cbPattern,
                    cbSquareSize,
                    markSize,
                    markerIds_R,
                    markerCorners_R,
                    charucoIds_R,
                    cornersR);

                if ((true == cornersL.empty()) || (true == cornersR.empty()))
                    continue;
#if ENABLE_DEBUG
                {
                    cv::Mat colorL = srcImgL.clone();
                    cv::Mat colorR = srcImgR.clone();
                    cv::aruco::drawDetectedMarkers(colorL, markerCorners_L, markerIds_L);
                    cv::aruco::drawDetectedCornersCharuco(colorL, cornersL, charucoIds_L, cv::Scalar(0, 255, 255));
                    cv::aruco::drawDetectedMarkers(colorR, markerCorners_R, markerIds_R);
                    cv::aruco::drawDetectedCornersCharuco(colorR, cornersR, charucoIds_R, cv::Scalar(0, 255, 255));
                    sprintf_s(imgFileL, "%sdebug_%03d_charucoL.bmp", calibDataPath[grp], index);
                    sprintf_s(imgFileR, "%sdebug_%03d_charucoR.bmp", calibDataPath[grp], index);
                    cv::imwrite(imgFileL, colorL);
                    cv::imwrite(imgFileR, colorR);
                }
#endif

                cbCornersListL.push_back(cornersL);
                cbCornersListR.push_back(cornersR);
                cbCornersIdListL.push_back(charucoIds_L);
                cbCornersIdListR.push_back(charucoIds_R);
            }
            else if (CALIB_BALL_POLE == calibType)
            {

            }
            imageSize = cv::Size(srcImgL.cols, srcImgL.rows);
        }

        cv::Mat K, D;
        std::vector<double> reprojectionError;
        if (CALIB_CHARUCO == calibType)
            monocularCalibration_charuco(cbCornersIdList, cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        else
            monocularCalibration_chessboard(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        std::cout << "K:" << std::endl;
        std::cout << K << std::endl;
        std::cout << "D:" << std::endl;
        std::cout << D << std::endl;
        for (int i = 0; i < reprojectionError.size(); i++)
            std::cout << reprojectionError[i] << std::endl;


        // 输出映射类型，通常使用CV_32FC1或CV_16SC2
        cv::Mat backwardMap_x, backwardMap_y;
        cv::Mat forwardMap_x, forwardMap_y;
        cv::Mat newCamMatrix;
        // 生成畸变矫正映射
#if ENABLE_FISH_EYE
        cv::fisheye::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map1, map2);
#else
        double alpha = 0.4; // 0.4;
        newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);
        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, forwardMap_x, forwardMap_y);
#endif

        // 生成系数表
        cv::Mat fitMap_x = GetFitParamMap(forwardMap_x, 1);
        cv::Mat fitMap_y = GetFitParamMap(forwardMap_y, 1);
        //输出系数文件
        char calibParamName[256];
        sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_x);
        sprintf_s(calibParamName, "%scalib_param_y.txt", calibDataPath[grp]);
        sg_outputCalibK(calibParamName, fitMap_y);

        //比较误差
        cv::Mat mapGen_x = GetMapFromFitMap(fitMap_x, imageSize, 1);
        cv::Mat mapGen_y = GetMapFromFitMap(fitMap_y, imageSize, 1);
        //搜索最大和平均误差
            // 计算绝对差异
        cv::Mat diff_x, diff_y;
        cv::absdiff(forwardMap_x, mapGen_x, diff_x);
        cv::absdiff(forwardMap_y, mapGen_y, diff_y);
        // 查找最大值和最小值
        double minVal_x, maxVal_x;
        cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x);
        double minVal_y, maxVal_y;
        cv::minMaxLoc(diff_y, &minVal_y, &maxVal_y);
        // 计算平均值
        cv::Scalar meanVal_x = cv::mean(diff_x);
        cv::Scalar meanVal_y = cv::mean(diff_y);

        std::cout << "X Max_difference: " << maxVal_x << std::endl;
        std::cout << "X Mean difference: " << meanVal_x[0] << std::endl;
        std::cout << "Y Max difference: " << maxVal_y << std::endl;
        std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;

        //生成矫正图像
        for (index = startIndex; index <= endIndex; index++) {
            char filename[256];
            sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
            cv::Mat srcImg = cv::imread(filename);
            if (srcImg.empty())
                break;

            cv::Mat img;
            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
            cv::Mat calibImg;
            cv::remap(img,
                calibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, calibImg);
        }

#else
        char calibKDName[256];
        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
        cv::Mat K, D;
        sg_readCalibKD(calibKDName, K, D);

        //生成opencv校正表
        cv::Mat backwardMap_x, backwardMap_y;
        double alpha = 0.4; // 0.4;
        imageSize = cv::Size(1200, 2048);
        cv::Mat newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);
#endif

#if ENABLE_GEN_IMAGE
        cv::Vec4f laserPE;
        generateLaserLine(10.f, 5.f, laserPE);
        std::cout << "generateLaserLine pe: " << laserPE << std::endl;

        index = 3;
        for (;; index++) {

            char filename[256];
            sprintf_s(filename, "%s%03d.bmp", cbImagePath, index);
            cv::Mat img = cv::imread(filename);
            if (img.empty())
                break;

            std::vector<cv::Point2f> corners;
            detectCorners(img, cbPattern, corners);
            if (corners.empty())
                continue;

            cv::Vec4f pe;
            fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);

            cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize);

#if ENABLE_DEBUG
            cv::Mat color = image.clone();
            cv::resize(color, color, cv::Size(), 0.5, 0.5);
            cv::imshow("image", color);
            cv::waitKey(10); ·
#endif

                sprintf_s(filename, "%s%d.bmp", laserImagePath, index);
            cv::imwrite(filename, image);
        }

#endif

        std::vector<cv::Point3f> all_pts3d;
        for (index = startIndex; index <= endIndex; index++) {

            char filename[256];
            sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
            cv::Mat srcImg = cv::imread(filename);
            if (srcImg.empty())
                break;

            cv::Mat img;
            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
#if 1
            cv::Mat charucoCalibImg;
            cv::remap(img,
                charucoCalibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            cv::Size _size = charucoCalibImg.size();
            _size.width = _size.width * 5;
            //cv::resize(charucoCalibImg, charucoCalibImg, _size, 0, 0, cv::INTER_NEAREST);
            sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, charucoCalibImg);
#endif

            std::vector<cv::Point2f> corners;
            std::vector<int> charucoIds;
            if (CALIB_CHESS_BOARD == calibType)
            {
                detectCorners(img, cbPattern, corners);
            }
            else if (CALIB_CIRCLE_GRID == calibType)
            {
                detectCirclePoints(img, cbPattern, corners);
            }
            else if (CALIB_CHARUCO == calibType)
            {
                std::vector<int> markerIds;
                std::vector<std::vector<cv::Point2f> > markerCorners;
                detectCharucoCorners(img,
                    cbPattern,
                    cbSquareSize,
                    markSize,
                    markerIds,
                    markerCorners,
                    charucoIds,
                    corners);
            }
            if (corners.empty())
                continue;

            // 创建棋盘格区域的掩码
            cv::Mat chessMask;
            if (CALIB_CHARUCO == calibType)
                chessMask = cv::Mat::ones(img.size(), CV_8UC1);
            else
            {
                chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                // 使用多边形近似来填充角点之间的区域
                // 棋盘格区域需要比角点区域大一圈
                std::vector<cv::Point2f> contour_line[4];
                for (int i = 0; i < cbPattern.width; i++)
                {
                    cv::Point2f pt_c = corners[i];
                    cv::Point2f pt_2 = corners[cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[0].push_back(pt_1);
                }
                for (int i = 0; i < cbPattern.height; i++)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[1].push_back(pt_1);
                }
                for (int i = cbPattern.width - 1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
                    cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[2].push_back(pt_1);
                }
                for (int i = cbPattern.height - 1; i >= 0; i--)
                {
                    cv::Point2f pt_c = corners[i * cbPattern.width];
                    cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contour_line[3].push_back(pt_1);
                }
                std::vector<cv::Point> contours;
                //生成轮廓点
                for (int n = 0; n < 4; n++)
                {
                    int num = contour_line[n].size();
                    for (int i = 0; i < num; i++)
                        contours.push_back(contour_line[n][i]);
                    cv::Point2f pt_c = contour_line[n][num - 1];
                    cv::Point2f pt_2 = contour_line[n][num - 2];
                    cv::Point2f pt_1;
                    pt_1.x = pt_c.x * 2 - pt_2.x;
                    pt_1.y = pt_c.y * 2 - pt_2.y;
                    contours.push_back(pt_1);
                }
                // 使用 fillPoly 填充多边形
                cv::Scalar color(255); // 红色
                cv::fillPoly(chessMask, contours, color);
#if 1
                sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
                cv::imwrite(filename, chessMask);
#endif
            }

            cv::Vec4f pe;
            if (CALIB_CHARUCO == calibType)
                fitChessboardPlane_charuco(charucoIds, corners, K, D, cbPattern, cbSquareSize, pe);
            else
                fitChessboardPlane_chessboard(corners, K, D, cbPattern, cbSquareSize, pe);

            sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
            cv::Mat srcLaserImg = cv::imread(filename);
            if (srcLaserImg.empty())
                break;

            cv::Mat laserImg_unMask;
            cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
            //与Mask相与，保证待处理的激光线在标定板上
            cv::Mat laserImg;
            cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
#if 1
            sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
            cv::imwrite(filename, laserImg);

            cv::Mat laserCalibImg;
            cv::remap(laserImg,
                laserCalibImg,
                backwardMap_x,
                backwardMap_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            cv::Size laserImgSize = laserCalibImg.size();
            laserImgSize.width = laserImgSize.width * 5;
            cv::resize(laserCalibImg, laserCalibImg, laserImgSize, 0, 0, cv::INTER_NEAREST);
            sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, laserCalibImg);
#endif
            std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
            //显示亚像素点
            cv::Mat enlargeImg;
            if (laserImg.channels() == 1)
                laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
            else
                enlargeImg = laserImg.clone();
            cv::Size objSize = laserImg.size();
            objSize.width = objSize.width * 5;
            cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);

            if (pts2d.size() > 0)
            {
                sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index);
                saveSubpixData(filename, pts2d);
            }
            for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++)
            {
                cv::Point2f a_subPix = pts2d[i];
                int row = (int)(a_subPix.y + 0.5);
                int col = (int)(a_subPix.x * 5 + 0.5);
                enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
                enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
                enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
            }
            sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
            cv::imwrite(filename, enlargeImg);
            std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
#if 1
            //保存3D点

#endif
            all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
        }

        cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
        std::cout << "pe: " << pe << std::endl;

        //output K and D
        char calibKDPName[256];
        sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]);
        sg_outputCalibKD(calibKDPName, K, D, pe);
    }
    return 0;
}
#endif

int main(void)
{
    MonoCamLaserCalib();
    //StereoCamCalib();
}