// MonoLaserCalibrate.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include <math.h>
#include "MonoLaserCalibrate.h"
#include "../lineDetection_steger.h"

/*Breif：角点检测函数*/
void detectCorners(const cv::Mat& img,
    const cv::Size& patternSize,
    std::vector<cv::Point2f>& corners)
{
    cv::Mat gray;
    if (img.channels() == 3)
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    else
        gray = img.clone();

    bool found = cv::findChessboardCorners(gray, patternSize, corners);
#if 1
    if (found) {
        // 亚像素精确化
        cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
            cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));
    }
#endif
   return;
}

/*Breif：圆形网格函数*/
void detectCirclePoints(const cv::Mat& img,
    const cv::Size& patternSize,
    std::vector<cv::Point2f>& corners)
{
    cv::Mat gray;
    if (img.channels() == 3)
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    else
        gray = img.clone();

    //需要将圆点外的背景去除，否则复杂的背景会导致检测失败
    //使用连通域检测
    cv::Mat grayBin;


    // 检测圆形网格
    bool found = cv::findCirclesGrid(gray, patternSize, corners, cv::CALIB_CB_SYMMETRIC_GRID);
    return;
}

/*Breif：单目相机标定函数*/
void monocularCalibration(
    const std::vector<std::vector<cv::Point2f>>& imagePoints,
    const cv::Size& imageSize,
    const cv::Size& patternSize,
    const float squareSize,
    cv::Mat& cameraMatrix,
    cv::Mat& distCoeffs,
    std::vector<double>& reprojectionError)
{
    // 根据角点生成3d点
    std::vector<std::vector<cv::Point3f>> objectPoints;
    for (const auto& corners : imagePoints) {

        // 准备3D世界坐标点 (z=0)
        std::vector<cv::Point3f> obj;
        for (int i = 0; i < patternSize.height; ++i)
            for (int j = 0; j < patternSize.width; ++j)
                obj.emplace_back(j * squareSize, i * squareSize, 0);

        objectPoints.push_back(obj);
    }

    // 执行相机标定
    std::vector<cv::Mat> rvecs, tvecs;
#if ENABLE_FISH_EYE
    // 步骤十(2)：计算内参和畸变系数，设 flags 和 增加迭代终止条件 需要 图象数>=2张，这是鱼眼摄像头常规方式, 但是使用 undistortImage
    int flags = 0;
    flags |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
    flags |= cv::fisheye::CALIB_CHECK_COND;
    flags |= cv::fisheye::CALIB_FIX_SKEW;
    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);// , flags, cv::TermCriteria(3, 20, 1e-6));
#else
     cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);
#endif
    // 重投影三维点到二维图像点
    // 计算重投影误差
    double totalError = 0.0;
    int totalSize = 0;
    for (int i = 0; i < objectPoints.size(); i++)
    {
        std::vector<cv::Point2f> reprojectedPoints;
#if ENABLE_FISH_EYE
        {
            cv::fisheye::projectPoints(objectPoints[i], reprojectedPoints, rvecs[i], tvecs[i], cameraMatrix,
                distCoeffs);
        }
#else
        {
            projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, reprojectedPoints);
        }
#endif

        //totalSize += objectPoints[i].size();
        //double localError = 0;
        double err = norm(imagePoints[i], reprojectedPoints, cv::NORM_L2);
        size_t n = objectPoints[i].size();
        double localError = (double)std::sqrt(err * err / n);
        totalError += err * err;
        totalSize += n;
#if 0
        for (int j = 0; j < objectPoints[i].size(); j++)
        {
            totalError += cv::norm(imagePoints[i][j] - reprojectedPoints[j]);
            localError += cv::norm(imagePoints[i][j] - reprojectedPoints[j]);
            //totalError += sqrt(pow(imagePoints[i][j].x - reprojectedPoints[j].x,2) + pow(imagePoints[i][j].y - reprojectedPoints[j].y, 2)); // 使用L2范数计算误差
        }
        int localSize = (int)objectPoints[i].size();
        localError = localError / (double)localSize;
#endif
        reprojectionError.push_back(localError);  
    }
    totalError = std::sqrt(totalError / totalSize);
    //totalError /= totalSize; // 取平均误差
    reprojectionError.push_back(totalError);
    return;
}

/*Brief: 拟合棋盘格角点平面*/
void fitChessboardPlane(
    const std::vector<cv::Point2f>& corners,
    const cv::Mat& cameraMatrix,
    const cv::Mat& distCoeffs,
    const cv::Size& patternSize,
    const float squareSize,
    cv::Vec4f& planeEquation)
{
    // 1. 生成3D世界坐标点 (z=0)
    std::vector<cv::Point3f> objectPoints;
    for (int i = 0; i < patternSize.height; ++i) {
        for (int j = 0; j < patternSize.width; ++j) {
            objectPoints.emplace_back(j * squareSize, i * squareSize, 0);
        }
    }

    // 2. 解算PnP获取棋盘格位姿
    cv::Mat rvec, tvec;
    cv::solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec);

    // 3. 转换旋转向量为旋转矩阵
    cv::Mat R;
    cv::Rodrigues(rvec, R);

    // 4. 计算平面法向量 (世界坐标系Z轴转换到相机坐标系)
    cv::Mat normal_world = (cv::Mat_<double>(3, 1) << 0, 0, 1);
    cv::Mat normal_camera = R * normal_world;

    // 5. 获取平面上的一个点 (棋盘格原点在相机坐标系的位置)
    cv::Mat origin_camera = tvec;

    // 6. 构建平面方程: n·(X - P0) = 0
    float a = normal_camera.at<double>(0);
    float b = normal_camera.at<double>(1);
    float c = normal_camera.at<double>(2);
    float d = -(a * origin_camera.at<double>(0) +
        b * origin_camera.at<double>(1) +
        c * origin_camera.at<double>(2));

    planeEquation = cv::Vec4f(a, b, c, d);

    return;
}

/*Brief: 构造激光出射平面方程*/
void generateLaserLine(
    const float baseLine,
    const float angle,
    cv::Vec4f& planeEquation)
{
    // aX + bY + cZ + d = 0
    const float a = cos(angle * CV_PI / 180.f);
    const float b = 0.f;
    const float c = sin(angle * CV_PI / 180.f);
    const float d = baseLine * cos(angle * CV_PI / 180.f);
    planeEquation = cv::Vec4f(a, b, c, d);

    return;
}

/*Breif: 构造虚拟仿真图*/
cv::Mat generateVirtualLaserLineImage(
    const cv::Vec4f& laserPlane,    // 激光出射面方程 ax+by+cz+d=0
    const cv::Vec4f& targetPlane,   // 目标平面方程(棋盘格平面)
    const cv::Mat& cameraMatrix,    // 相机内参K
    const cv::Mat& distCoeffs,      // 畸变系数D
    const cv::Size& imageSize)      // 输出图像尺寸
{

    // 生成直线上的3D点集（Z范围1mm~200mm）
    std::vector<cv::Point3f> linePoints3D;
    for (float z = 400; z <= 600.0; z += 0.05f) {
        //根据激光出射面(b=0)确定x
        const float x = -(laserPlane[2] * z + laserPlane[3]) / laserPlane[0];
        //根据棋盘格平面确定y
        const float y = -(targetPlane[0] * x + targetPlane[2] * z + targetPlane[3]) / targetPlane[1];
        linePoints3D.push_back(cv::Point3f(x, y, z));
    }

    // 投影到图像平面
    std::vector<cv::Point2f> imagePoints;
    cv::projectPoints(linePoints3D,
        cv::Vec3f::zeros(), cv::Vec3f::zeros(),
        cameraMatrix, distCoeffs, imagePoints);

    // 绘制激光线
    cv::Mat laserImage = cv::Mat::zeros(imageSize, CV_8UC1);
    for (size_t i = 1; i < imagePoints.size(); ++i) {
        if (cv::norm(imagePoints[i] - imagePoints[i - 1]) < 50) { // 过滤异常投影
            cv::line(laserImage, imagePoints[i - 1], imagePoints[i],
                cv::Scalar(255), 2, cv::LINE_AA);
        }
    }

    return laserImage;
}

/*Brief: 激光线检测函数*/
#if 0
std::vector<cv::Point2f> detectLaserLine(
    const cv::Mat& inputImage) 
{
    std::vector<cv::Point2f> laserPoints;
    if (inputImage.empty()) 
        return laserPoints;

    // 1. 颜色空间转换（处理彩色图像）
    cv::Mat gray;
    if (inputImage.channels() == 3) {
        cv::cvtColor(inputImage, gray, cv::COLOR_BGR2GRAY);
    }
    else {
        gray = inputImage.clone();
    }

    // 2. 动态阈值二值化（自动适应亮度变化）
    cv::Mat binary;
    double thresh = cv::threshold(gray, binary, 0, 255,
        cv::THRESH_BINARY | cv::THRESH_OTSU);

    // 3. 形态学降噪（可选）
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::morphologyEx(binary, binary, cv::MORPH_OPEN, kernel);

    // 4. 骨架提取（细化激光线）
    cv::Mat skel(binary.size(), CV_8UC1, cv::Scalar(0));
    cv::Mat temp;
    cv::Mat eroded;
    cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
    do {
        cv::erode(binary, eroded, element);
        cv::dilate(eroded, temp, element);
        cv::subtract(binary, temp, temp);
        cv::bitwise_or(skel, temp, skel);
        eroded.copyTo(binary);
    } while (cv::countNonZero(binary) != 0);

    // 6. 提取非零点坐标
    cv::findNonZero(skel, laserPoints);
    return laserPoints;
}
#else
void getLinePeaks(double* lineData, int dataSize, int row, int scaleWin, double minPkValue, std::vector<cv::Point>& linePeaks)
{
    int _state = 0;
    int pre_i = -1;
    int sEdgePtIdx = -1;
    int eEdgePtIdx = -1;
    double* pre_data = NULL;
    std::vector<int> pkTops;
    for (int i = 0; i < dataSize; i++)
    {
        double* curr_data = &lineData[i];
        if (NULL == pre_data)
        {
            sEdgePtIdx = i;
            eEdgePtIdx = i;
            pre_data = curr_data;
            pre_i = i;
            continue;
        }

        eEdgePtIdx = i;
        double px_diff = *curr_data - *pre_data;
        switch (_state)
        {
        case 0:  //初态
            if (px_diff < 0) //下降
                _state = 2;
            else if (px_diff > 0) //上升
                _state = 1;
            break;
        case 1: //上升
            if (px_diff < 0)   //下降
            {
                pkTops.push_back(pre_i);
                _state = 2;
            }
            break;
        case 2: //下降
            if (px_diff > 0)   //  上升
                _state = 1;
            break;
        default:
            _state = 0;
            break;
        }
        pre_data = curr_data;
        pre_i = i;
    }
    //最后一个
    if(1 == _state)
        pkTops.push_back(pre_i);

    //在窗口内搜索
    for (int i = 0, i_max = (int)pkTops.size(); i < i_max; i++)
    {
        double pkValue = lineData[pkTops[i]];
        if (pkValue < minPkValue)
            continue;

        bool isPeak = true;
        //向前搜索
        for (int j = i - 1; j >= 0; j--)
        {
            int dist = pkTops[i] - pkTops[j]; 
            if (dist < 0) dist = -dist;
            if (dist > scaleWin)  //超出尺度窗口
                break;
            if (lineData[pkTops[i]] < lineData[pkTops[j]])
            {
                isPeak = false;
                break;
            }
        }
        //向后搜索
        if (true == isPeak)
        {
            for (int j = i + 1; j < i_max; j++)
            {
                int dist = pkTops[i] - pkTops[j];
                if (dist < 0) dist = -dist;
                if (dist > scaleWin)  //超出尺度窗口
                    break;
                if (lineData[pkTops[i]] < lineData[pkTops[j]])
                {
                    isPeak = false;
                    break;
                }
            }
        }
        if (true == isPeak)
            linePeaks.push_back(cv::Point(pkTops[i], row));
    }
    return;
}

std::vector<cv::Point2f> detectLaserLine(
    const cv::Mat& inputImage)
{
    std::vector<cv::Point2f> laserPoints;
    if (inputImage.empty())
        return laserPoints;

    // 1. 颜色空间转换（处理彩色图像）
    cv::Mat gray;
    if (inputImage.channels() == 3) {
        cv::cvtColor(inputImage, gray, cv::COLOR_BGR2GRAY);
    }
    else {
        gray = inputImage.clone();
    }

    //高斯滤波
    cv::Mat img;
    gray.convertTo(img, CV_64FC1);
    cv::GaussianBlur(img, img, cv::Size(3, 3), 0.9, 0.9);
    //cv::imwrite("gauss_blur_src.bmp", img);
    // 提取最大值点
    int scaleWin = 5;
    double minPkValue = 100;
    std::vector< cv::Point> pkPoints;
    for (int i = 0; i < img.rows; i++)
    {
        std::vector< cv::Point> linePeaks;
        double* lineData = img.ptr<double>(i);
        getLinePeaks(lineData, img.cols, i, scaleWin, minPkValue, linePeaks);
        pkPoints.insert(pkPoints.end(), linePeaks.begin(), linePeaks.end());
    }
    //取亚像素值
    std::vector<cv::Point2f> posSubpix;
    int gaussWin = 3;  //5x5
    computePointSubpix(
        gray,  //uchar型单通道输入图像（灰度图像）
        pkPoints,  //峰值像素的整数坐标。
        gaussWin,  //使用steger算法时指定的窗口宽度。此宽度需要与激光线的宽度基本吻合
        posSubpix  //计算出的亚像素坐标
    );
    return posSubpix;
}

#endif
/*Breif: 根据棋盘格平面和2d坐标计算3d值*/
std::vector<cv::Point3f> project2DTo3D(
    const std::vector<cv::Point2f>& imagePoints,
    const cv::Vec4f& planeEquation,  // [a,b,c,d] for ax+by+cz+d=0
    const cv::Mat& cameraMatrix,     // 相机内参K
    const cv::Mat& distCoeffs)       // 畸变系数D
{
    std::vector<cv::Point3f> objectPoints;
    if (imagePoints.empty()) 
        return objectPoints;

    // 1. 去畸变
    std::vector<cv::Point2f> undistortedPoints;
    cv::undistortPoints(imagePoints, undistortedPoints,
        cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);

    // 2. 提取平面参数
    const float a = planeEquation[0], b = planeEquation[1];
    const float c = planeEquation[2], d = planeEquation[3];
    const float denom = a * a + b * b + c * c;
    if (fabs(denom) < 1e-6f) return objectPoints;

    // 3. 构造射线并求交平面
    const cv::Mat invK = cameraMatrix.inv();
    for (const auto& pt : undistortedPoints) {
        // 生成归一化射线方向
        cv::Mat ray = (cv::Mat_<double>(3, 1) << pt.x, pt.y, 1.0f);
        ray = invK * ray;
        const float rx = ray.at<double>(0), ry = ray.at<double>(1), rz = ray.at<double>(2);

        // 计算射线与平面交点
        const float t = -(a * rx + b * ry + c * rz + d) / (a * ray.at<double>(0) + b * ray.at<double>(1) + c * ray.at<double>(2));
        objectPoints.emplace_back(
            rx * t,  // X = t * ray_dir_x
            ry * t,  // Y = t * ray_dir_y
            rz * t   // Z = t * ray_dir_z
        );
    }
    return objectPoints;
}

/*Breif: 拟合平面方程*/
cv::Vec4f fitPlaneToPoints(const std::vector<cv::Point3f>& points) {
    CV_Assert(points.size() >= 3); // 至少需要3个点

    // 构建数据矩阵(每行为[x,y,z,1])
    cv::Mat A(points.size(), 4, CV_32F);
    for (size_t i = 0; i < points.size(); ++i) {
        A.at<float>(i, 0) = points[i].x;
        A.at<float>(i, 1) = points[i].y;
        A.at<float>(i, 2) = points[i].z;
        A.at<float>(i, 3) = 1.0f;
    }

    // SVD分解求最小奇异值对应的右奇异向量
    cv::Mat svdU, svdW, svdVt;
    cv::SVDecomp(A, svdW, svdU, svdVt, cv::SVD::MODIFY_A);

    // 取最后一行作为平面方程系数
    cv::Vec4f plane(
        svdVt.at<float>(3, 0),
        svdVt.at<float>(3, 1),
        svdVt.at<float>(3, 2),
        svdVt.at<float>(3, 3)
    );

    // 归一化前三个系数
    float norm = sqrt(pow(plane[0], 2) + pow(plane[1], 2) + pow(plane[2], 2));
    plane = plane / norm;
    return plane;
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件