欧拉角欧拉矩阵

//计算旋转角
double calculateAngle(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter)
{
    double ab, a1, b1, cosr;
    ab = vectorBefore.x()*vectorAfter.x() + vectorBefore.y()*vectorAfter.y() + vectorBefore.z()*vectorAfter.z();
    a1 = sqrt(vectorBefore.x()*vectorBefore.x() + vectorBefore.y()*vectorBefore.y() + vectorBefore.z()*vectorBefore.z());
    b1 = sqrt(vectorAfter.x()*vectorAfter.x() + vectorAfter.y()*vectorAfter.y() + vectorAfter.z()*vectorAfter.z());
    cosr = ab / a1 / b1;
    return acos(cosr);
}
//计算旋转轴
inline Eigen::Vector3d calculateRotAxis(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter)
{
    return Eigen::Vector3d(vectorBefore.y()*vectorAfter.z() - vectorBefore.z()*vectorAfter.y(), 
        vectorBefore.z()*vectorAfter.x() - vectorBefore.x()*vectorAfter.z(), 
        vectorBefore.x()*vectorAfter.y() - vectorBefore.y()*vectorAfter.x());
}
//计算旋转矩阵
void rotationMatrix(const Eigen::Vector3d &vectorBefore, const Eigen::Vector3d &vectorAfter, Eigen::Matrix3d &rotMatrix)
{
    Eigen::Vector3d vector = calculateRotAxis(vectorBefore, vectorAfter);
    double angle = calculateAngle(vectorBefore, vectorAfter);
    Eigen::AngleAxisd rotationVector(angle, vector.normalized());
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Identity();
    rotMatrix =  rotationVector.toRotationMatrix();//所求旋转矩阵
}

cv::Mat find_homography(template_data_h Hole_data,string c_name,Eigen::Vector3d CenterPoint,int type_planNormal,double & radius,Point2d & circle_in_PC){
    Eigen::Matrix4d trans_cad=Hole_data.trans_cad;
    cv::Mat intrinsic_matrix;
    intrinsic_matrix=Hole_data.intrinsic_matrix;
    cv::Mat extrinsic_matrix_r;
    extrinsic_matrix_r=Hole_data.extrinsic_matrix_r;
    cv::Mat extrinsic_matrix_t;
    extrinsic_matrix_t=Hole_data.extrinsic_matrix_t;
    //计算当前相机的光心坐标
    Eigen::Matrix4d extrinsic_matrix;
    extrinsic_matrix<<extrinsic_matrix_r.at<double>(0,0),extrinsic_matrix_r.at<double>(0,1),extrinsic_matrix_r.at<double>(0,2),extrinsic_matrix_t.at<double>(0,0),
            extrinsic_matrix_r.at<double>(1,0),extrinsic_matrix_r.at<double>(1,1),extrinsic_matrix_r.at<double>(1,2),extrinsic_matrix_t.at<double>(1,0),
            extrinsic_matrix_r.at<double>(2,0),extrinsic_matrix_r.at<double>(2,1),extrinsic_matrix_r.at<double>(2,2),extrinsic_matrix_t.at<double>(2,0),
            0,0,0,1;

    Eigen::Vector4d coor_src1;
    Eigen::Vector4d coor_src2;
    coor_src1<<0,0,0,1;//光心点
    coor_src2<<0,0,1,1;//光轴上的另一点
    Eigen::Vector4d coor_dst1;
    Eigen::Vector4d coor_dst2;
    coor_dst1=trans_cad.inverse()*(extrinsic_matrix.inverse()*coor_src1);//光心点在cad坐标系下的坐标
    coor_dst2=trans_cad.inverse()*(extrinsic_matrix.inverse()*coor_src2);//另外一点在cad坐标系下的坐标
    // 得到相机光轴的的向量
    Eigen::Vector4d optic_axis_vec;
    optic_axis_vec=(coor_dst2-coor_dst1);
    optic_axis_vec.normalize();
    //计算光轴与工件平面的交点。
    Eigen::Vector3d planeVecotr=Hole_data.planeNormal;//{0,0,1}/{0,1,0}
    Eigen::Vector3d planePoint_cad; //平面点
    planePoint_cad << CenterPoint[0]/1000.0, CenterPoint[1] / 1000.0, CenterPoint[2] / 1000.0;
    //直线点：coor_dst1
    //计算光轴与工件平面的交点
    double vpt = optic_axis_vec[0]*planeVecotr[0]+optic_axis_vec[1]*planeVecotr[1]+optic_axis_vec[2]*planeVecotr[2];
    if(abs(vpt)<1e-08){
        cout<<"相机轴线与平面平行"<<endl;
        exit(1);
    }
    double t= ((planePoint_cad[0] - coor_dst1[0]) * planeVecotr[0] + (planePoint_cad[1] - coor_dst1[1]) * planeVecotr[1] + (planePoint_cad[2] - coor_dst1[2]) * planeVecotr[2]) / vpt;
    Eigen::Vector4d inter_point_cad={coor_dst1[0] + optic_axis_vec[0] * t, coor_dst1[1] + optic_axis_vec[1] * t, coor_dst1[2] + optic_axis_vec[2] * t,1};
    //cout << "光轴与工件平面的交点in cad :" << inter_point_cad << endl;

    Eigen::Vector3d src_axis={0,0,-1};
    src_axis.normalize();
    Eigen::Vector4d inter_point_camera1 = extrinsic_matrix*(trans_cad*inter_point_cad);//交点
    // cout<<"交点在第一个坐标系的坐标"<<endl; cout<<inter_point_camera1<<endl;
    Eigen::Vector4d new_point_cad;
    if(type_planNormal==1) {//{0,0,1
        new_point_cad={inter_point_cad[0], inter_point_cad[1], inter_point_cad[2] + inter_point_camera1[2], 1};
    }
    else{
        if(inter_point_cad[1] > 0)
            new_point_cad={inter_point_cad[0], inter_point_cad[1] + inter_point_camera1[2], inter_point_cad[2], 1};
        else
            new_point_cad={inter_point_cad[0], inter_point_cad[1] - inter_point_camera1[2], inter_point_cad[2], 1};
    }

    Eigen::Vector4d new_point_camera1 = extrinsic_matrix*(trans_cad*new_point_cad);//另外一个点在camera1下的坐标
    Eigen::Vector3d new_axis={new_point_camera1[0]-inter_point_camera1[0],new_point_camera1[1]-inter_point_camera1[1],new_point_camera1[2]-inter_point_camera1[2]};
    new_axis.normalize();
    Eigen::Matrix3d R_Matrix;

    rotationMatrix(src_axis,new_axis,R_Matrix);

    Eigen::Vector3d T_Matrix;
    Eigen::Vector4d cirle_center_cad={CenterPoint[0] / 1000.0, CenterPoint[1] / 1000.0, CenterPoint[2] / 1000.0, 1};
    auto c_c_camera1=extrinsic_matrix*(trans_cad * cirle_center_cad);
    double dis=sqrt(c_c_camera1[0]*c_c_camera1[0]+c_c_camera1[1]*c_c_camera1[1]+c_c_camera1[2]*c_c_camera1[2]);
    //cout<<dis<<" dis   "<<endl;
    if(type_planNormal==1) {//{0,0,1
        new_point_cad={CenterPoint[0]/1000,CenterPoint[1]/1000, CenterPoint[2]/1000+dis,1};
    }
    else{
        inter_point_cad[1] + inter_point_camera1[2];
        if(CenterPoint[1]>0)
            new_point_cad={CenterPoint[0]/1000,CenterPoint[1]/1000+dis,CenterPoint[2]/1000,1};
        else
            new_point_cad={CenterPoint[0]/1000,CenterPoint[1]/1000-dis,CenterPoint[2]/1000,1};
    }
    Eigen::Vector4d vector_d=extrinsic_matrix*(trans_cad*new_point_cad);//交点
    Eigen::Vector3d V_pd = {vector_d[0]-inter_point_camera1[0],vector_d[1]-inter_point_camera1[1],vector_d[2]-inter_point_camera1[2]};

    Eigen::Vector3d V_pc = {0-inter_point_camera1[0],0-inter_point_camera1[1],0-inter_point_camera1[2]};
    T_Matrix = V_pd-V_pc;
    Eigen::Matrix4d Rele_Matrix;
    Rele_Matrix<<R_Matrix(0,0),R_Matrix(0,1),R_Matrix(0,2),T_Matrix(0,0),
            R_Matrix(1,0),R_Matrix(1,1),R_Matrix(1,2),T_Matrix(1,0),
            R_Matrix(2,0),R_Matrix(2,1),R_Matrix(2,2),T_Matrix(2,0),
            0,0,0,1;
//
//        cout<<"交点在第二个坐标系的坐标"<<endl;
//        cout<<Rele_Matrix.inverse()*inter_point_camera1<<endl;
//        cout<<"圆心点在第1个坐标系的坐标"<<endl;
    Eigen::Vector4d gzr_p={CenterPoint[0]/1000,CenterPoint[1]/1000,CenterPoint[2]/1000,1};
    Eigen::Vector4d gzr_p_camera1=(extrinsic_matrix*(trans_cad*gzr_p));
    Eigen::Matrix<double,3,4>intrex_prame;
    intrex_prame<<intrinsic_matrix.at<double>(0,0),intrinsic_matrix.at<double>(0,1),intrinsic_matrix.at<double>(0,2),0,
            intrinsic_matrix.at<double>(1,0),intrinsic_matrix.at<double>(1,1),intrinsic_matrix.at<double>(1,2),0,
            intrinsic_matrix.at<double>(2,0),intrinsic_matrix.at<double>(2,1),intrinsic_matrix.at<double>(2,2),0;
    Eigen::Vector3d out=intrex_prame*gzr_p_camera1;//圆心点第1个cameraz
    out[0]/=out[2];out[1]/=out[2];out[2]=1;
    //rele_Matix矩阵是目标位置到原始位置的相对矩阵
    Eigen::Vector4d point1_in_cad={1,1,1,1};
    Eigen::Vector4d point1_in_camera=extrinsic_matrix*(trans_cad*point1_in_cad);
    Eigen::Vector4d point2_in_cad;
    if(type_planNormal==1) {//{0,0,1
        point2_in_cad ={1,1,2,1};
    }
    else{
        point2_in_cad ={1,2,1,1};
    }
    Eigen::Vector4d point2_in_camera=extrinsic_matrix*(trans_cad*point2_in_cad);
    Eigen::Vector3d planeNolmal={point2_in_camera[0]-point1_in_camera[0], point2_in_camera[1]-point1_in_camera[1], point2_in_camera[2]-point1_in_camera[2]};//相机坐标系下的法向
    planeNolmal.normalize();//平面在第一个相机坐标系的法向
    Eigen::Vector4d temp_matrix={planePoint_cad[0], planePoint_cad[1], planePoint_cad[2], 1};//平面上随机一个点
    temp_matrix=extrinsic_matrix*(trans_cad*temp_matrix);//得到在相机坐标系点
    double d = temp_matrix[0]*planeNolmal[0]+temp_matrix[1]*planeNolmal[1]+temp_matrix[2]*planeNolmal[2];//平面的点法式确定,相机坐标系下
    cv::Mat Homography;
    Eigen::Matrix4d new_Matrix=Rele_Matrix.inverse();
    R_Matrix<<new_Matrix(0,0),new_Matrix(0,1),new_Matrix(0,2),
            new_Matrix(1,0),new_Matrix(1,1),new_Matrix(1,2),
            new_Matrix(2,0),new_Matrix(2,1),new_Matrix(2,2);
    T_Matrix<<new_Matrix(0,3),new_Matrix(1,3),new_Matrix(2,3);

    Eigen::Matrix3d Eigen_Homo= R_Matrix+(T_Matrix*planeNolmal.transpose())/d;
    cv::eigen2cv(Eigen_Homo,Homography);
    Homography = (intrinsic_matrix*Homography)*(intrinsic_matrix.inv());
    //生成圆
    Eigen::Vector4d point_around[4];
    if(type_planNormal==1) {
        point_around[0] = {(CenterPoint[0] - radius) / 1000, CenterPoint[1] / 1000, CenterPoint[2] / 1000, 1};
        point_around[1] = {(CenterPoint[0] + radius) / 1000, CenterPoint[1] / 1000, CenterPoint[2] / 1000, 1};
        point_around[2] = {CenterPoint[0] / 1000, (CenterPoint[1] - radius) / 1000, CenterPoint[2] / 1000, 1};
        point_around[3] = {CenterPoint[0] / 1000, (CenterPoint[1] + radius) / 1000, CenterPoint[2] / 1000, 1};
    }
    else{
        point_around[0] = {(CenterPoint[0] - radius) / 1000, CenterPoint[1] / 1000, CenterPoint[2] / 1000, 1};
        point_around[1] = {(CenterPoint[0] + radius) / 1000, CenterPoint[1] / 1000, CenterPoint[2] / 1000, 1};
        point_around[2] = {CenterPoint[0] / 1000, CenterPoint[1] / 1000, (CenterPoint[2]- radius) / 1000, 1};
        point_around[3] = {CenterPoint[0] / 1000, CenterPoint[1] / 1000, (CenterPoint[2]+radius) / 1000, 1};
    }
    double total_radius=0;
    double cen_x=intrex_prame(0,2);
    double cen_y=intrex_prame(1,2);
    circle_in_PC.x=cen_x;circle_in_PC.y=cen_y;
    //cout<<cen_x<<"  "<<cen_y<<endl;
    for(int k=0;k<4;k++){
        Eigen::Vector4d  point_around_camera=Rele_Matrix.inverse()*(extrinsic_matrix*(trans_cad*point_around[k]));
        Eigen::Vector3d point_pixel=intrex_prame*point_around_camera;
        point_pixel[0]/=point_pixel[2];point_pixel[1]/=point_pixel[2];point_pixel[2]=1;
       // cout<<point_pixel[0]<<" "<<point_pixel[1]<<endl;
        total_radius+=sqrt((point_pixel[0]-cen_x)*(point_pixel[0]-cen_x)+(point_pixel[1]-cen_y)*(point_pixel[1]-cen_y));
    }
    radius=total_radius/4;

    return Homography;
}