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

using namespace cv;
using namespace std;

//求解絕對(duì)定向的步驟
/*
① 給定七個(gè)參數(shù)的初始值 phi = omega = kappa = 0 scale = 1 X = Y =Z= 0屯烦；
② 計(jì)算控制點(diǎn)地面坐標(biāo)系的重心坐標(biāo)和各控制點(diǎn)重心化的坐標(biāo)
③ 計(jì)算模型點(diǎn)空間坐標(biāo)系的重心坐標(biāo)和各模型點(diǎn)重心化的坐標(biāo)
④ 計(jì)算常數(shù)項(xiàng) 誤差方程的常數(shù)項(xiàng)（矩陣表示）【lx ly lz】^T 書(shū)本71
⑤ 計(jì)算誤差方程的系數(shù)矩陣 一般為3*7 矩陣（多個(gè)控制點(diǎn)則表示為3n * 7） 
⑥ 逐點(diǎn)法化求解法方程
⑦ 計(jì)算待定系數(shù)（scale phi omega kappa）的新值
⑧ 判斷待定系數(shù)改正數(shù)是否均小于限定閾值xita
⑨ 根據(jù)求解的系數(shù)（scale phi omega kappa）代入重心變換方程求解X Y Z
*/

#define NUM 4 //地面控制點(diǎn)數(shù)量
#define JINDU 0.001//閾值
#define PI 3.14159265358975

//矩陣的數(shù)乘
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;

}

//攝影測(cè)量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;

}

//多個(gè)控制點(diǎn)的矩陣形式轉(zhuǎn)換
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;
}

//計(jì)算改正數(shù)
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[]) {



    // 初始化參數(shù)
    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;

    //改正數(shù)
    double d_scale = 1.0;
    double d_phi = 1.0;
    double d_omega = 1.0;
    double d_kappa = 1.0;

    double jindu = JINDU;

    /*測(cè)試一下*/



    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) };//像輔助原始點(diǎn)坐標(biāo)
    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) };//控制點(diǎn)地面原始點(diǎn)坐標(biāo)

    vector<Mat> core_vec1;//像輔助重心化點(diǎn)坐標(biāo)
    vector<Mat> core_vec2;//控制點(diǎn)地面重心化點(diǎn)坐標(biāo)

    //求解重心和重心化坐標(biāo)
    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);

        //計(jì)算改正數(shù)
        Mat XX = getX(A, L);
    

        //計(jì)算新值
        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;
    }
    

    //計(jì)算最終的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;
}