camAlgo/camAlgoSW_test/camAlgoSW_test.cpp

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

#include <iostream>
#include <fstream>
#include "..\camAlgoSW\sourceCode\camAlgoSW_Export.h"
#include <opencv2/opencv.hpp>
#include <vector>
#include "..\camAlgoSW\sourceCode\algoGlobals.h"

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

void saveFrontEndData(char* fileName, std::vector<Luma_rgnData>& frontendData)
{
    if (0 == frontendData.size())
        return;

    std::ofstream TXTFile(fileName);
    char TXTData[250];
    snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
    TXTFile << TXTData << std::endl;

    for (int i = 0, i_max = (int)frontendData.size(); i < i_max; i++)
    {
        Luma_rgnData data = frontendData[i];
        snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.WinRdx, data.y, data.Rid, data.Flag, data.PeakRltvRdx);
        TXTFile << TXTData << std::endl;
        for (int j = 0; j < 16; j++)
        {
            snprintf(TXTData, sizeof(TXTData), "%02x", data.data[j]);
            TXTFile << TXTData << std::endl;
        }
    }
    TXTFile.close();
}

void readFrontEndDataFile(char* fileName, std::vector<Luma_rgnData>& frontendData)
{
    std::ifstream inputFile(fileName);
    std::string linedata;

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

    std::getline(inputFile, linedata);  //第一行

    int idx = 0;
    Luma_rgnData a_pk;
    while (std::getline(inputFile, linedata))
    {
        if (idx == 0)
        {
            int WinRdx, y, Rid, Flag, PeakRltvRdx;
            sscanf_s(linedata.c_str(), "%04x %04x %04x %01x %02x", &WinRdx, &y, &Rid, &Flag, &PeakRltvRdx);
            a_pk.WinRdx = WinRdx;
            a_pk.y = y;
            a_pk.Rid = Rid;
            a_pk.Flag = Flag;
            a_pk.PeakRltvRdx = PeakRltvRdx;
            idx++;
        }
        else
        {
            int value;
            sscanf_s(linedata.c_str(), "%02x", &value);
            a_pk.data[idx - 1] = value;
            idx++;
        }
        if (idx == 17)
        {
            frontendData.push_back(a_pk);
            idx = 0;
        }
    }
    inputFile.close();
    return;
}

void saveCenteringData(char* filename, std::vector<RgnPix>& centeringPnt)
{
    if (centeringPnt.size() < 6)
        return;

    int vldSize = (int)centeringPnt.size() - 6;
    if ( (vldSize % 9) != 0)
        return;
    int grp = vldSize / 9;

    std::ofstream TXTFile(filename);
    char TXTData[250];
    snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
    TXTFile << TXTData << std::endl;

    int headOffset = 6;
    for (int i = 0; i < grp; i++)
    {
        RgnPix data = centeringPnt[i * 9 + headOffset];
        snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.LazerWinX, data.LazerWinY, data.LazerWinRid, data.LazerWinFlag, data.RltvRdx);
        TXTFile << TXTData << std::endl;
        for (int j = 1; j <= 8; j++)
        {
            data = centeringPnt[i * 9 + j + headOffset];
            snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinX);
            TXTFile << TXTData << std::endl;
            snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinY);
            TXTFile << TXTData << std::endl;
        }
    }
    TXTFile.close();

}

void saveSubpixData(char* filename, std::vector<RgnSubPix>& 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++)
    {
        RgnSubPix data = subpixPnt[i];
        snprintf(TXTData, sizeof(TXTData), "%.5f %04d rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
        TXTFile << TXTData << std::endl;
    }
    TXTFile.close();
}

void saveCalibSubpixData(char* filename, std::vector<RgnSubPixCalib>& 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++)
    {
        RgnSubPixCalib data = subpixPnt[i];
        snprintf(TXTData, sizeof(TXTData), "%.5f %.5f rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
        TXTFile << TXTData << std::endl;
    }
    TXTFile.close();
}


void readCalibK(char* paraFilename, LineCalibK* lineCalibK)
{
    std::ifstream inputFile(paraFilename);
    std::string linedata;

    if (inputFile.is_open() == false)
        return;
    int i = 0;
    while (std::getline(inputFile, linedata))
    {
        LineCalibK a_para;
        sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", &a_para.calibK[0], &a_para.calibK[1], &a_para.calibK[2], &a_para.calibK[3], &a_para.calibK[4], &a_para.calibK[5], &a_para.calibK[6], &a_para.calibK[7]);
        lineCalibK[i] = a_para;
        i++;
    }
    inputFile.close();
    return;
}

CamPara readLaserPlanePara(char* filename)
{

    CamPara para;
    memset(&para, 0, sizeof(CamPara));
    std::ifstream inputFile(filename);
    std::string linedata;

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

    //读取: fx, 0, cx
    if (std::getline(inputFile, linedata))
    {
        double tmp;
        sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &para.F_inv, &tmp, &para.u0);
        para.F_inv = 1.0 / para.F_inv;
    }
    else
        return para;

    //读取: 0, fy, cy
    if (std::getline(inputFile, linedata))
    {
        double tmp_1, tmp_2;
        sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &tmp_1, &tmp_2, &para.v0);
    }
    else
        return para;

    //跳过现行
    if (!std::getline(inputFile, linedata))
        return para;
    if (!std::getline(inputFile, linedata))
        return para;

    //读取: a, b, c
    if (std::getline(inputFile, linedata))
        sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &para.plane_a, &para.plane_b, &para.plane_c);

    return para;
}

void saveLineData(char* fileName, std::vector< Pnt3D>& resul3DPnts)
{
    std::ofstream sw(fileName);
    sw << "LineNum:1" << std::endl;
//    sw << "DataType: 0" << std::endl;
//   sw << "ScanSpeed: 0" << std::endl;
//    sw << "PointAdjust:1" << std::endl;
//    sw << "MaxTimeStamp:0_0" << std::endl;

    int ptCounter = (int)resul3DPnts.size() - 4;
    sw << "Line_0_0_" << ptCounter << std::endl;
    for (int i = 4; i < (int)resul3DPnts.size(); i++)
    {
        Pnt3D a_pt = resul3DPnts[i];
        float x = (float)a_pt.x;
        float y = (float)a_pt.y;
        float z = (float)a_pt.z;
        sw << "{" << x << "," << y << "," << z << "}-";
        sw << "{0,0}-{0,0}" << std::endl;
    }
    sw.close();
}

#define CALIB_TEST_GROUP 3
int main()
{
    std::cout << "Hello World!\n";
    const char* calibDataPath[CALIB_TEST_GROUP] = {
    "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] = {
        {1,33},{1,33},{1,10}
    };

    //算法参数
    ProcObj frontendPara;
    frontendPara.StartIdx = 0;
    frontendPara.EndIdx = 0;
    frontendPara.MinStgTh = 5;
    frontendPara.LazerWMin = 2;
    frontendPara.LazerWMax = 10;
    frontendPara.RflctPixTh = 128;
    frontendPara.RflctOutEna = 0;//??
    frontendPara.OverlapPixTh = 32;
    frontendPara.EnhanStep = 2;
    frontendPara.PickEna = 1;
    frontendPara.RgnFltrTh = 0;
    frontendPara.RmvWkEndEna = 1;
    frontendPara.RmvWkEndTh = 2;
    frontendPara.RmvWkEndMultCoe = 16;
    frontendPara.RmvWkEndMinLen = 3;
    frontendPara.PickBFEna = 0;
    frontendPara.PickBkGrnd = 0;
    frontendPara.PickRLenTh = 4;
    frontendPara.EnergyPickEna = 1;
    frontendPara.PickEnergyType = 0;
    frontendPara.RgnEnergyPLen = 32;
    frontendPara.RgnMeanETh = 0;

    LineCalibK* lineCalibKx = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);
    LineCalibK* lineCalibKy = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);

    FramePara inFramePara;
    CamPara calibPara;

    for (int grp = 0; grp < CALIB_TEST_GROUP; grp++)
    {
        memset(lineCalibKx, 0, sizeof(LineCalibK) * MAX_HEIGHT);
        memset(lineCalibKy, 0, sizeof(LineCalibK) * MAX_HEIGHT);
        //读取校正系数
        char paraFilename[256];
        sprintf_s(paraFilename, "%scalib_param_x.txt", calibDataPath[grp]);
        readCalibK(paraFilename, lineCalibKx);
        sprintf_s(paraFilename, "%scalib_param_y.txt", calibDataPath[grp]);
        readCalibK(paraFilename, lineCalibKy);
        //读取光刀面
        sprintf_s(paraFilename, "%scalib_param_K_D.txt", calibDataPath[grp]);
        calibPara = readLaserPlanePara(paraFilename);

        int startIndex = fileIdx[grp].nMin;
        int endIndex = fileIdx[grp].nMax;
        int index;
        for (index = startIndex; index <= endIndex; index++) 
        {
            char filename[256];
            sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
            cv::Mat srcImg = cv::imread(filename);
            if (srcImg.empty())
                break;

            inFramePara.FrmNo = index;
            inFramePara.timeStamp = index;
            inFramePara.encInfo = 0;
            inFramePara.frameROI_w = srcImg.cols;
            inFramePara.frameROI_h = srcImg.rows;
            inFramePara.frameROI_x = 0;
            inFramePara.frameROI_y = 0;

            cv::Mat gray;
            if (srcImg.channels() == 3)
                cv::cvtColor(srcImg, gray, cv::COLOR_BGR2GRAY);
            else
                gray = srcImg.clone();

            unsigned char* dataPtr = gray.data;

            std::vector< Pnt3D> resul3DPnts;
#if _ALGO_MODULE_DEBUG
            std::vector<Luma_rgnData> frontendData;
            std::vector<RgnPix> centeringPnt;
            std::vector<RgnSubPix> subpixPnt;
            std::vector<RgnSubPixCalib> calibSubpixPnt;
#endif
            camAlgoSW(
                gray.rows,
                gray.cols,
                dataPtr,
                frontendPara,
                inFramePara,
                calibPara,
                lineCalibKx,
                lineCalibKy,
#if _ALGO_MODULE_DEBUG
                frontendData,
                centeringPnt,
                subpixPnt,
                calibSubpixPnt,
#endif
                resul3DPnts);
#if _ALGO_MODULE_DEBUG
            //保存frontendData
            sprintf_s(filename, "%sresult\\%d_verilog.txt", calibDataPath[grp], index);
            saveFrontEndData(filename, frontendData);
            //保存centeringData
            sprintf_s(filename, "%sresult\\%d_centeringData.txt", calibDataPath[grp], index);
            saveCenteringData(filename, centeringPnt);
            //保存subpixData
            sprintf_s(filename, "%sresult\\%d_subpixData.txt", calibDataPath[grp], index);
            saveSubpixData(filename, subpixPnt);
            //显示亚像素点
            cv::Mat enlargeImg;
            if (srcImg.channels() == 1)
                srcImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
            else
                enlargeImg = srcImg.clone();
            cv::Size objSize = srcImg.size();
            objSize.width = objSize.width * 5;
            cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
            for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
            {
                RgnSubPix a_subPix = subpixPnt[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, "%sresult\\%d_subpix.png", calibDataPath[grp], index);
            cv::imwrite(filename, enlargeImg);

            //保存校正后的subpixData
            sprintf_s(filename, "%sresult\\%d_calibSubpixData.txt", calibDataPath[grp], index);
            saveCalibSubpixData(filename, calibSubpixPnt);

#endif
            sprintf_s(filename, "%sresult\\%d_3D_data.txt", calibDataPath[grp], index);
            saveLineData(filename, resul3DPnts);
        }
    }

}