欧拉角欧拉矩阵

```

//计算旋转角
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(0,0),extrinsic_matrix_r.at(0,1),extrinsic_matrix_r.at(0,2),extrinsic_matrix_t.at(0,0),
extrinsic_matrix_r.at(1,0),extrinsic_matrix_r.at(1,1),extrinsic_matrix_r.at(1,2),extrinsic_matrix_t.at(1,0),
extrinsic_matrix_r.at(2,0),extrinsic_matrix_r.at(2,1),extrinsic_matrix_r.at(2,2),extrinsic_matrix_t.at(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_cadgzr_p));
Eigen::Matrix<double,3,4>intrex_prame;
intrex_prame<<intrinsic_matrix.at(0,0),intrinsic_matrix.at(0,1),intrinsic_matrix.at(0,2),0,
intrinsic_matrix.at(1,0),intrinsic_matrix.at(1,1),intrinsic_matrix.at(1,2),0,
intrinsic_matrix.at(2,0),intrinsic_matrix.at(2,1),intrinsic_matrix.at(2,2),0;
Eigen::Vector3d out=intrex_pramegzr_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_cadpoint1_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_cadpoint2_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_cadtemp_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;

}

posted @ 2021-02-17 19:43 gzr2018 阅读(130) 评论(0) 编辑收藏举报

刷新页面返回顶部

流照君

欧拉角欧拉矩阵