1、由空間三個(gè)點(diǎn)確定所在平面的法向量
int Cal_norm(const double Point_sta[], const double Point_mid[], const double Point_end[], Point3f &Point_norm)//計算法向量{ double s_x;//起始點(diǎn)笛卡爾坐標 double s_y; double s_z; double m_x;//中間點(diǎn)笛卡爾坐標 double m_y; double m_z; double e_x;//結束點(diǎn)笛卡爾坐標 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;//計算圓心坐標
//計算當前節點(diǎn)在基坐標系下
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); //若三點(diǎn)共線(xiàn)則出錯 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、計算兩個(gè)向量之間的旋轉矩陣：
為了更好地推導，我們需要加入三個(gè)軸對齊的單位向量i，j，k。
i，j，k滿(mǎn)足以下特點(diǎn)：
i=jxk；j=kxi；k=ixj；
kxj=–i；ixk=–j；jxi=–k；
ixi=jxj=kxk=0；（0是指0向量）
由此可知，i，j，k是三個(gè)相互垂直的向量。它們剛好可以構成一個(gè)坐標系。
這三個(gè)向量的特例就是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, 旋轉后變?yōu)镼。由點(diǎn)積定義可知：
可推出P，Q之間的夾角為：
2. 旋轉軸
由1中可知，旋轉角所在的平面為有P和Q所構成的平面，那么旋轉軸必垂直該平面。
假定旋轉前向量為a(a1, a2, a3)， 旋轉后向量為b（b1, b2, b3)。由叉乘定義得：
可得旋轉軸c(c1, c2, c3)為：
已知單位向量c.normalize ， 將它旋轉θ角。由羅德里格旋轉公式，可知對應的旋轉矩陣R ：
其中I是3x3的單位矩陣，
wr 是叉乘中的反對稱(chēng)矩陣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、求兩個(gè)平面之間的旋轉矩陣
思路：已知兩個(gè)平面上的兩個(gè)對應點(diǎn)以及平面上的三個(gè)點(diǎn)，
根據三個(gè)點(diǎn)確定平面的法向量，
根據兩個(gè)法向量計算旋轉矩陣，根據對應點(diǎn)坐標之差計算平移矩陣，合成兩個(gè)平面的旋轉矩陣。
代碼：
#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;//起始點(diǎn)笛卡爾坐標 double s_y; double s_z; double m_x;//中間點(diǎn)笛卡爾坐標 double m_y; double m_z; double e_x;//結束點(diǎn)笛卡爾坐標 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;//計算圓心坐標
//計算當前節點(diǎn)在基坐標系下
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); //若三點(diǎn)共線(xiàn)則出錯 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;}結果如下：
聯(lián)系客服
