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

using namespace cv;
using namespace std;

//求解绝对定向的步骤
/*
① 给定七个参数的初始值 phi = omega = kappa = 0 scale = 1 X = Y =Z= 0；
② 计算控制点地面坐标系的重心坐标和各控制点重心化的坐标
③ 计算模型点空间坐标系的重心坐标和各模型点重心化的坐标
④ 计算常数项 误差方程的常数项（矩阵表示）【lx ly lz】^T 书本71
⑤ 计算误差方程的系数矩阵 一般为3*7 矩阵（多个控制点则表示为3n * 7） 
⑥ 逐点法化求解法方程
⑦ 计算待定系数（scale phi omega kappa）的新值
⑧ 判断待定系数改正数是否均小于限定阈值xita
⑨ 根据求解的系数（scale phi omega kappa）代入重心变换方程求解X Y Z
*/

#define NUM 4 //地面控制点数量
#define JINDU 0.001//阈值
#define PI 3.14159265358975

//矩阵的数乘
Mat muul(Mat& p, double s) {
    int row = p.rows;
    int col = p.cols;
    Mat res = Mat_<double>(row, col);

    for (int i = 0; i < row; i++)
    {
        for (int j = 0; j < col; j++)
        {

            res.at <double>(i, j) = s;
        }

    }

    res = p.mul(res);
    double a = res.at<double>(0, 0);
    return res;

}


//求重心
Mat getTheCore(vector<Mat> &vec1) {

    int num = vec1.size();
    Mat p1 = (Mat_<double>(3, 1) << 0.0, 0.0, 0.0);
    for (int i = 0; i < num; i++)
    {
        p1 = p1 + vec1[i];


    }
    double cc = 1 / (double)num;
    Mat res = muul(p1,cc);
    double a = res.at<double>(0, 0);

    return res;

}

//摄影测量R矩阵
Mat getTheR(double omiga, double phi, double kamma) {
    double a1 = cos(phi) * cos(kamma) - sin(phi) * sin(omiga) * sin(kamma);
    double a2 = -cos(phi) * sin(kamma) - sin(phi) * sin(omiga) * cos(kamma);
    double a3 = -sin(phi) * cos(omiga);
    double b1 = cos(omiga) * sin(kamma);
    double b2 = cos(omiga) * cos(kamma);
    double b3 = -sin(omiga);
    double c1 = sin(phi) * cos(kamma) + cos(phi) * sin(omiga) * sin(kamma);
    double c2 = -sin(phi) * sin(kamma) + cos(phi) * sin(omiga) * cos(kamma);
    double c3 = cos(phi) * cos(omiga);

    Mat result = (Mat_<double>(3, 3) << a1, a2, a3, b1, b2, b3, c1, c2, c3);
    return result;

}

//多个控制点的矩阵形式转换
Mat transMat(vector<Mat>& vec, int num) {

    Mat ret = vec[0];

    for (int i = 1; i < vec.size(); i++)
    {
        vconcat(ret, vec[i], ret);
    }
    return ret;
}

//L

Mat getL(vector<Mat>& vec_tp, vector<Mat>& vec, double scale, Mat& r) {

    vector<Mat> vec_L;

    for (int i = 0; i < vec.size(); i++)
    {
        Mat m_tp = vec_tp[i];
        Mat m = vec[i];
        Mat r_m = r * m;
        Mat L = m_tp - muul(r_m,scale);
        vec_L.push_back(L);

    }

    Mat LL = transMat(vec_L, NUM);

    return LL;
}

//A

Mat getA(vector<Mat> &vec) {

    vector<Mat> vec_A;
    for (int i = 0; i < vec.size(); i++)
    {
        Mat m = vec[i];
        double X = m.at<double>(0, 0);
        double Y = m.at<double>(1, 0);
        double Z = m.at<double>(2, 0);

        Mat A = (Mat_<double>(3, 7) << 1, 0, 0, X, -Z, 0, -Y,
            0, 1, 0, Y, 0, -Z, X,
            0, 0, 1, Z, X, Y, 0);

        vec_A.push_back(A);
    }

    
    Mat AA = transMat(vec_A, NUM);


    return AA;

}

//求重心化
vector<Mat> CoreMat(vector<Mat> &p1, Mat &core) {

    vector<Mat> result;

    for (int i = 0; i < p1.size(); i++)
    {
        Mat m = p1[i];
        Mat res_m = m - core;
        result.push_back(res_m);
    }

    return result;
}

//计算改正数
Mat getX(Mat& A, Mat& L) {

    Mat AtA = (A.t()) * A;
    Mat nAtA;
    invert(AtA, nAtA, DECOMP_LU);
    Mat X = nAtA * (A.t()) * L;
    return X;
}

int main(int argc, char* argv[]) {



    // 初始化参数
    double scale = 1.0;
    double omega = 0.0;
    double phi = 0.0;
    double kappa = 0.0;
    double X = 0.0;
    double Y = 0.0;
    double Z = 0.0;

    //改正数
    double d_scale = 1.0;
    double d_phi = 1.0;
    double d_omega = 1.0;
    double d_kappa = 1.0;

    double jindu = JINDU;

    /*测试一下*/



    vector<Mat> vec1 = {(Mat_<double>(3,1)<< 153.706,767.599, 4.98951),(Mat_<double>(3,1) << 826.447,757.146,45.0979) ,(Mat_<double>(3,1) << 130.021,-772.319,-14.688) ,(Mat_<double>(3,1) << 804.556, -782.906 ,-22.5479) };//像辅助原始点坐标
    vector<Mat> vec2 = {(Mat_<double>(3,1) << 5083.26,5852.04,527.963),(Mat_<double>(3,1) << 5779.98,5906.4,571.513) ,(Mat_<double>(3,1) << 5210.76,4258.46,461.775) ,(Mat_<double>(3,1) << 5909.37,4314.29,455.517) };//控制点地面原始点坐标

    vector<Mat> core_vec1;//像辅助重心化点坐标
    vector<Mat> core_vec2;//控制点地面重心化点坐标

    //求解重心和重心化坐标
    Mat core1 = getTheCore(vec1);
    Mat core2 = getTheCore(vec2);


    core_vec1 = CoreMat(vec1, core1);
    core_vec2 = CoreMat(vec2, core2);


    while (!(!(fabs(d_kappa) > jindu) && !(fabs(d_omega) > jindu) && !(fabs(d_phi) > jindu)))
    {
        //获得矩阵形式的R,A L
        Mat R = getTheR(omega, phi, kappa);
        Mat A = getA(core_vec1);
        Mat L = getL(core_vec2, core_vec1, scale, R);

        //计算改正数
        Mat XX = getX(A, L);
    

        //计算新值
        d_scale = XX.at<double>(3, 0);
        d_phi = XX.at<double>(4, 0);
        d_omega = XX.at <double>(5, 0);
        d_kappa = XX.at<double>(6, 0);

        scale = scale *(1 + d_scale);
        phi += d_phi;
        omega += d_omega;
        kappa += d_kappa;
    }
    

    //计算最终的XYZ
    Mat R = getTheR(omega, phi, kappa);
    Mat XYZ;
    Mat r_core1 = R * core1;
    XYZ = core2 - muul(r_core1, scale);
 
    double pi = PI;
    double dS_omega = (180/pi)*(omega - int(omega / (2*pi)) * (2*pi));
    double dS_phi = (180 / pi) * (phi - int(phi / (2 * pi)) * (2 * pi));
    double dS_kappa = (180 / pi) * (kappa - int(kappa / (2 * pi)) * (2 * pi));
    X = XYZ.at<double>(0, 0);
    Y = XYZ.at<double>(1, 0);
    Z = XYZ.at<double>(2, 0);

    return 1;
}