法向量、旋转矩阵计算(五十一)

1、由空间三个点确定所在平面的法向量

    int Cal_norm(const double Point_sta[], const double Point_mid[], const double Point_end[], Point3f &Point_norm)//计算法向量{ double s_x;//起始点笛卡尔坐标 double s_y; double s_z; double m_x;//中间点笛卡尔坐标 double m_y; double m_z; double e_x;//结束点笛卡尔坐标 double e_y; double e_z;
    //计算单位法向量(SE×EM) double norm; double vec_x; double vec_y; double vec_z; //定义新坐标系单位法向量 double n_x; double n_y; double n_z;//计算圆心坐标
    //计算当前节点在基坐标系下
    s_x = Point_sta[0]; s_y = Point_sta[1]; s_z = Point_sta[2]; m_x = Point_mid[0]; m_y = Point_mid[1]; m_z = Point_mid[2]; e_x = Point_end[0]; e_y = Point_end[1]; e_z = Point_end[2];
    vec_x = (e_y - s_y) * (m_z - e_z) - (m_y - e_y) * (e_z - s_z); vec_y = (m_x - e_x) * (e_z - s_z) - (e_x - s_x) * (m_z - e_z); vec_z = (e_x - s_x) * (m_y - e_y) - (m_x - e_x) * (e_y - s_y); norm = sqrt(vec_x * vec_x + vec_y * vec_y + vec_z * vec_z); //若三点共线则出错 if (norm < ERR_PRECISION) return -1; //定义新坐标系单位法向量 n_x = vec_x / norm; n_y = vec_y / norm; n_z = vec_z / norm; Point_norm.x= n_x; std::cout << Point_norm.x << std::endl; Point_norm.y = n_y; cout << Point_norm.y << endl; Point_norm.z = n_z; cout << Point_norm.z << endl; cout << Point_norm << endl;}

    2、计算两个向量之间的旋转矩阵:

    为了更好地推导,我们需要加入三个轴对齐的单位向量i,j,k。

    i,j,k满足以下特点:

    i=jxk;j=kxi;k=ixj;

    kxj=–i;ixk=–j;jxi=–k;

    ixi=jxj=kxk=0;(0是指0向量)

    由此可知,i,j,k是三个相互垂直的向量。它们刚好可以构成一个坐标系。

    这三个向量的特例就是i=(1,0,0)j=(0,1,0)k=(0,0,1)。

    对于处于i,j,k构成的坐标系中的向量u,v我们可以如下表示:

    u=Xui+Yuj+Zuk;

    v=Xvi+Yvj+Zvk;

    uxv=(Xui+Yuj+Zuk)x(Xvi+Yvj+Zvk)

    =XuXv(ixi)+XuYv(ixj)+XuZv(ixk)+YuXv(jxi)+YuYv(jxj)+YuZv(jxk)+ZuXv(kxi)+ZuYv(kxj)+ZuZv(kxk)

    1.旋转角度

    已知旋转前向量为P, 旋转后变为Q。由点积定义可知:

    可推出P,Q之间的夹角为:

    2. 旋转轴
    由1中可知,旋转角所在的平面为有P和Q所构成的平面,那么旋转轴必垂直该平面。

    假定旋转前向量为a(a1, a2, a3), 旋转后向量为b(b1, b2, b3)。由叉乘定义得:

    可得旋转轴c(c1, c2, c3)为:

    已知单位向量c.normalize , 将它旋转θ角。由罗德里格旋转公式,可知对应的旋转矩阵R :

    其中I是3x3的单位矩阵,

    wr 是叉乘中的反对称矩阵r:

    代码如下:

      #include <cmath>#include <iostream>#include 'Eigen/Dense'  #include 'Eigen/LU'  #include 'Eigen/Core'  //计算旋转角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) * 180 / PI);}//计算旋转轴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.y() - 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();//所求旋转矩阵}int main(){  Eigen::Matrix3d rotMatrix;  //Eigen::Vector3d vectorBefore(0, 0, 1);  //Eigen::Vector3d vectorAfter(1, 0, 0);  Eigen::Vector3d vectorBefore(3, 8, 1);  Eigen::Vector3d vectorAfter(1, 9, 2);  rotationMatrix(vectorBefore, vectorAfter, rotMatrix);  std::cout << rotMatrix << std::endl;}

      3、求两个平面之间的旋转矩阵

      思路:已知两个平面上的两个对应点以及平面上的三个点,

      根据三个点确定平面的法向量,

      根据两个法向量计算旋转矩阵,根据对应点坐标之差计算平移矩阵,合成两个平面的旋转矩阵。

      代码:

        #include <opencv2//opencv.hpp>#include <cmath>#include <iostream>#include 'Eigen/Dense' #include 'Eigen/LU' #include 'Eigen/Core' #include 'vector'
        using namespace cv;using namespace std;
        #define PI 3.1415926const double ERR_PRECISION = 0.000001;
        int Cal_norm(const double Point_sta[], const double Point_mid[], const double Point_end[], Point3f &Point_norm)//计算法向量{ double s_x;//起始点笛卡尔坐标 double s_y; double s_z; double m_x;//中间点笛卡尔坐标 double m_y; double m_z; double e_x;//结束点笛卡尔坐标 double e_y; double e_z;
        //计算单位法向量(SE×EM) double norm; double vec_x; double vec_y; double vec_z; //定义新坐标系单位法向量 double n_x; double n_y; double n_z;//计算圆心坐标
        //计算当前节点在基坐标系下
        s_x = Point_sta[0]; s_y = Point_sta[1]; s_z = Point_sta[2]; m_x = Point_mid[0]; m_y = Point_mid[1]; m_z = Point_mid[2]; e_x = Point_end[0]; e_y = Point_end[1]; e_z = Point_end[2];
        vec_x = (e_y - s_y) * (m_z - e_z) - (m_y - e_y) * (e_z - s_z); vec_y = (m_x - e_x) * (e_z - s_z) - (e_x - s_x) * (m_z - e_z); vec_z = (e_x - s_x) * (m_y - e_y) - (m_x - e_x) * (e_y - s_y); norm = sqrt(vec_x * vec_x + vec_y * vec_y + vec_z * vec_z); //若三点共线则出错 if (norm < ERR_PRECISION) return -1; //定义新坐标系单位法向量 n_x = vec_x / norm; n_y = vec_y / norm; n_z = vec_z / norm; Point_norm.x= n_x; std::cout << Point_norm.x << std::endl; Point_norm.y = n_y; cout << Point_norm.y << endl; Point_norm.z = n_z; cout << Point_norm.z << endl; cout << Point_norm << endl;}
        //计算旋转角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) * 180 / PI);}
        //计算旋转轴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.y() - 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();//所求旋转矩阵}
        Mat T_circle(4, 4, CV_32F);
        int main(){ Eigen::Matrix3d rotMatrix;
        //Eigen::Vector3d vectorBefore(0, 0, 1); //Eigen::Vector3d vectorAfter(1, 0, 0);
        Eigen::Vector3d vectorBefore(3, 8, 1); Eigen::Vector3d vectorAfter(1, 9, 2); rotationMatrix(vectorBefore, vectorAfter, rotMatrix); std::cout << rotMatrix << std::endl;
        Point3f circleone; circleone.x = 1; circleone.y = 1; circleone.z = 1;
        Point3f circletwo; circletwo.x = 2; circletwo.y = 2; circletwo.z = 2;
        double sta[] = { 772.891, 61.006, -128.258 }; double mid[] = { 809.984, 62.701, -131.170 }; double end[] = { 806.269, 63.659, -158.876 }; int re; Point3f Point_norm; re = Cal_norm(sta, mid, end, Point_norm); float Px, Py, Pz; Px = circletwo.x - circleone.x; Py = circletwo.y - circleone.y; Pz = circletwo.z - circleone.z;
        //将旋转和平移矩阵合并成旋转矩阵 float m3[3][3]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { m3[i][j]= rotMatrix(i, j); } }
        Mat Rz(3, 3, CV_32F, m3); cout<<'Rz' << Rz << endl;
        float m4[3][1] = { Px, Py, Pz }; Mat T1(3, 1, CV_32F, m4); cout << 'T1' << T1 << endl;
        float m5[1][4] = { 0, 0, 0, 1 }; Mat T2(1, 4, CV_32F, m5);
        Mat A(4, 4, CV_32F); hconcat(Rz, T1, A);//A = [b c] vconcat(A, T2, T_circle);//A = [b;c] cout << 'T_circle:' <<endl<< T_circle << endl;
        Mat PW_fpoint(4, 1, CV_32F);//机器人坐标系下坐标 Mat PC_fpoint(4, 1, CV_32F);//机器人坐标系下坐标
        PC_fpoint.at<float>(0) = (float)sta[0]; PC_fpoint.at<float>(1) = (float)sta[1]; PC_fpoint.at<float>(2) = (float)sta[2]; PC_fpoint.at<float>(3) = 1;
        PW_fpoint = T_circle*PC_fpoint; cout << 'PW_fpoint:' << endl << PW_fpoint << endl;
        system('pause'); return 0;}

        结果如下:

        (0)

        相关推荐