法向量、旋转矩阵计算(五十一)
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.1415926
const 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)