法向量、旋转矩阵计算（五十一） / 开普饭

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)//计算法向量

//计算单位法向量(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];

//计算旋转角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;}

结果如下：

法向量、旋转矩阵计算（五十一）

相关推荐