triangle/solver/sample_calib_all.cpp

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

#include <iostream>

#include "ParametersSolver.h"
#include "ceres/ceres.h"

std::vector<cv::Point2d> generate_2dby3d(std::vector<cv::Point3d> wpts,double* value)
{

    double R=value[0];
    double P=value[1];
    double Y=value[2];
    double tx=value[3];
    double ty=value[4];
    double tz=value[5];

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

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

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

    std::vector<cv::Point2d> rets;

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

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

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

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

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

        cv::Point2d pt2d(nx*fx/Z+cx,ny*fy/Z+cy);
        rets.push_back(pt2d);

    }
    return rets;
}

int main()
{
    std::vector<cv::Point2d> pt2ds;
    std::vector<cv::Point3d> pt3ds;



    ParametersSolver solver;
    solver.Solve(pt2ds,pt3ds);
    return 0;
}