camAlgo/camCalib/camCalib.cpp

// 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    0

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
        {
            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

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;
}

#define CALIB_TEST_GROUP 4
int main()
{
    std::cout << "Hello World!\n";

    const char* calibDataPath[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
    };
    const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
        {3,39},{1,33},{1,33},{1,10}
    };
    const int boardType[CALIB_TEST_GROUP] =
    {
        CALIB_CHESS_BOARD,
        CALIB_CHESS_BOARD,
        CALIB_CIRCLE_GRID,
        CALIB_CHARUCO
    };

    for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
    {
        grp = 1;
        int calibType = boardType[grp];
        cv::Size cbPattern;
        float cbSquareSize;
        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(20, 46); // 10);
            cbSquareSize = 50.0f; // 10.f;
        }
        else
            continue;

        std::vector<std::vector<cv::Point2f>> cbCornersList;
        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 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);
            char rotateFilename[256];
            sprintf_s(rotateFilename, "%scalib_%03d_rotate.bmp", calibDataPath[grp], index);
            cv::String rFileName(rotateFilename);
            cv::imwrite(rFileName, img);

            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
            }
            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, "%scalib_%03d_center.bmp", calibDataPath[grp], index);
                    cv::imwrite(filename, color);
                }
#endif

            }
            else if (CALIB_CHARUCO == calibType)
            {
            }
            if (corners.empty())
                continue;

            imageSize = cv::Size(img.cols, img.rows);
            cbCornersList.push_back(corners);
        }

        cv::Mat K, D;
        std::vector<double> reprojectionError;
        monocularCalibration(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
        for(int i = 0; i < reprojectionError.size(); i ++)
            std::cout << reprojectionError[i] << std::endl;

        // 输出映射类型，通常使用CV_32FC1或CV_16SC2
        cv::Mat map_x, map_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, map_x, map_y);
        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, map_x, map_y);
#endif

        // 生成系数表
        cv::Mat fitMap_x = GetFitParamMap(map_x, 1);
        cv::Mat fitMap_y = GetFitParamMap(map_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(map_x, mapGen_x, diff_x);
        cv::absdiff(map_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;
            remap(img,
                calibImg,
                mapGen_x,
                mapGen_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);
#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);
            std::vector<cv::Point2f> corners;
            detectCorners(img, cbPattern, corners);
            if (corners.empty())
                continue;

            // 创建棋盘格区域的掩码
            cv::Mat 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;
            fitChessboardPlane(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 calibImg;
            remap(laserImg,
                calibImg,
                mapGen_x,
                mapGen_y,
                cv::INTER_LINEAR,
                cv::BORDER_CONSTANT,
                cv::Scalar(0, 0, 0));
            sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
            cv::imwrite(filename, calibImg);
*/

#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(calibKDName, K, D, pe);
    }

    return 0;
}