//
// Created by zx on 22-11-9.
//

#include "ParametersSolver.h"
#include "ceres/ceres.h"
#include <chrono>
class CostFunctor{
 public:
    std::vector<cv::Point2d> ixy_;
    std::vector<cv::Point3d> wpt_;
 public:
    CostFunctor(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt)
    {
        ixy_=ixy;
        wpt_=wpt;
    }
    template <typename T>
    bool operator()(const T* const value, T* residual) const
    {
        if (ixy_.size()!=wpt_.size())
            printf("Error : ixy_.size()!=wpt_.size() \n");
        //参数拜访顺序：R P Y tx ty tz fx fy cx cy k1 k2 k3 r 共14个参数
        T R=value[0];
        T P=value[1];
        T Y=value[2];
        T tx=value[3];
        T ty=value[4];
        T tz=value[5];

        T fx=value[6];
        T fy=value[7];

        T cx=value[8];
        T cy=value[9];

        T k1=value[10];
        T k2=value[11];
        T p1=value[12];
        T p2=value[13];
        T k3=value[14];


        Eigen::AngleAxis<T> rollAngle(Eigen::AngleAxis<T>(R,Eigen::Matrix<T,3,1>::UnitX()));
        Eigen::AngleAxis<T> pitchAngle(Eigen::AngleAxis<T>(P,Eigen::Matrix<T,3,1>::UnitY()));
        Eigen::AngleAxis<T> yawAngle(Eigen::AngleAxis<T>(Y,Eigen::Matrix<T,3,1>::UnitZ()));

        Eigen::Matrix<T,3,3> rotation_matrix;
        rotation_matrix=yawAngle*pitchAngle*rollAngle;


        //每个样本，产生2n维残差项

        for (int i=0;i<ixy_.size();++i)
        {
            Eigen::Matrix<T,3,1> wp;
            wp[0]=T(wpt_[i].x)-tx;
            wp[1]=T(wpt_[i].y)-ty;
            wp[2]=T(wpt_[i].z)-tz;

            Eigen::Matrix<T,3,1> Rwp=rotation_matrix.inverse()*wp;
            T X=Rwp[0];
            T Y=Rwp[1];
            T Z=Rwp[2];

            T ix=T(ixy_[i].x);
            T iy=T(ixy_[i].y);

            T rr=X*X+Y*Y;
            T nx=X*(T(1.0)+k1*rr+k2*rr*rr+k3*rr*rr*rr) + T(2.0)*p1*X*Y + p2*(rr+T(2.0)*X*X);
            T ny=Y*(T(1.0)+k1*rr+k2*rr*rr+k3*rr*rr*rr) + p1*(rr+T(2.0)*Y*Y) + T(2.0)*p2*X*Y;

            residual[2*i]=nx*fx/Z+cx-ix;
            residual[2*i+1]=ny*fy/Z+cy-iy;
        }

    return true;
    }
};

ParametersSolver::ParametersSolver()
{

}
ParametersSolver::~ParametersSolver()
{

}

void ParametersSolver::Solve(std::vector<cv::Point2d> ixy,std::vector<cv::Point3d> wpt)
{
    if(ixy.size()!=wpt.size())
    {
        printf("Error : ixy.size()!=wpt.size()\n");
        return ;
    }
    // 寻优参数x的初始值
    auto t0 = std::chrono::steady_clock::now();

    double R=0;
    double P=0;
    double Y=0;
    double tx=0;
    double ty=0;
    double tz=0;

    double fx=0;
    double fy=0;

    double cx=0;
    double cy=0;

    double k1=0;
    double k2=0;
    double p1=0;
    double p2=0;
    double k3=0;

    const int NUM=15;
    double x[NUM]={R,P,Y,tx,ty,tz,fx,fy,cx,cy,k1,k2,p1,p2,k3};

    const int samples=ixy.size();

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    //使用自动求导，将之前的代价函数结构体传入，残差维度2n，参数x的维度NUM。
    ceres::CostFunction* cost_function =new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, NUM>(
            new CostFunctor(ixy,wpt),2*samples);
    problem.AddResidualBlock(cost_function, NULL, x); //向问题中添加误差项

    //第三部分： 配置并运行求解器
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
    options.minimizer_progress_to_stdout = true;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

    auto t1 = std::chrono::steady_clock::now();

    std::cout << summary.BriefReport() << "\n";//输出优化的简要信息

    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0);
    double time=double(duration.count()) * std::chrono::microseconds::period::num /
            std::chrono::microseconds::period::den;
    std::cout << "time : "<<time<<std::endl;

}