c++ 知道旋转前后矩阵向量值 求旋转矩阵c++/c#代码 知道两个向量求他们的旋转矩阵
Posted aircraft
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了c++ 知道旋转前后矩阵向量值 求旋转矩阵c++/c#代码 知道两个向量求他们的旋转矩阵相关的知识,希望对你有一定的参考价值。
原文作者:aircraft
原文链接:https://www.cnblogs.com/DOMLX/p/12115244.html
知道旋转前后矩阵向量值 如何去求旋转矩阵R 的c++/c#代码???
因为需要用到矩阵处理库所以需要先配置
一、Eigen库的配置(VS2017)
- Eigen库下载: http://eigen.tuxfamily.org/index.php?title=Main_Page
下载文件并解压:
然后在自己的VS工程属性中的这个附加包含进去
注意看清楚了 是D:\\Dependencies\\eigen-eigen\\eigen-eigen; 前面部分是你们自己的路径 后面的这个eigen-eigen\\eigen-eigen; 代表的意思解压是点击进去选择里面那个名字跟外面一样的
二、实现代码
c++代码:
#include <cmath> #include <iostream> #include "Eigen/Dense" #include "Eigen/LU" #include "Eigen/Core" #define PI 3.1415926 //计算旋转角 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); rotationMatrix(vectorBefore, vectorAfter, rotMatrix); std::cout << rotMatrix << std::endl; system("pause"); return 0; }
打印结果:
c#代码:
void Calculation(double[] vectorBefore, double[] vectorAfter) { double[] rotationAxis; double rotationAngle; double[,] rotationMatrix; rotationAxis = CrossProduct(vectorBefore, vectorAfter); rotationAngle = Math.Acos(DotProduct(vectorBefore, vectorAfter) / Normalize(vectorBefore) / Normalize(vectorAfter)); rotationMatrix = RotationMatrix(rotationAngle, rotationAxis); } double[] CrossProduct(double[] a, double[] b) { double[] c = new double[3]; c[0] = a[1] * b[2] - a[2] * b[1]; c[1] = a[2] * b[0] - a[0] * b[2]; c[2] = a[0] * b[1] - a[1] * b[0]; return c; } double DotProduct(double[] a, double[] b) { double result; result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; return result; } double Normalize(double[] v) { double result; result = Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); return result; } double[,] RotationMatrix(double angle, double[] u) { double norm = Normalize(u); double[,] rotatinMatrix = new double[3,3]; u[0] = u[0] / norm; u[1] = u[1] / norm; u[2] = u[2] / norm; rotatinMatrix[0, 0] = Math.Cos(angle) + u[0] * u[0] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = u[0] * u[1] * (1 - Math.Cos(angle) - u[2] * Math.Sin(angle)); rotatinMatrix[0, 0] = u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = u[2] * Math.Sin(angle) + u[0] * u[1] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = Math.Cos(angle) + u[1] * u[1] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = -u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = -u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle)); rotatinMatrix[0, 0] = Math.Cos(angle) + u[2] * u[2] * (1 - Math.Cos(angle)); return rotatinMatrix; }
三、实现原理
1.旋转角度
已知旋转前向量为P, 旋转后变为Q。由点积定义可知:
2. 旋转轴
由1中可知,旋转角所在的平面为有P和Q所构成的平面,那么旋转轴必垂直该平面。
假定旋转前向量为a(a1, a2, a3), 旋转后向量为b(b1, b2, b3)。由叉乘定义得:
所以旋转轴c(c1, c2, c3)为:
3. 罗德里格旋转公式(Rodrigues\' rotation formula)
3.1 公式
已知单位向量 , 将它旋转θ角。由罗德里格旋转公式,可知对应的旋转矩阵 :
其中I是3x3的单位矩阵,
是叉乘中的反对称矩阵r:
3.2 公式证明
假设在坐标系(x, y, z)中,向量v=ax+by+cz,v绕z轴逆时针旋转θ角后得到新的向量v’。
根据2维(x,y)面上的旋转公式可得:
推出:
将上式中的叉乘表示为反对称矩阵得:
另外:
最终可以推出:
上式即为罗德里格旋转公式。
参考博客:https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html
参考博客里的是c#的实现代码
我是参考完之后改了一个c++的版本出来
若有兴趣交流分享技术,可关注本人公众号,里面会不定期的分享各种编程教程,和共享源码,诸如研究分享关于c/c++,python,前端,后端,opencv,halcon,opengl,机器学习深度学习之类有关于基础编程,图像处理和机器视觉开发的知识
以上是关于c++ 知道旋转前后矩阵向量值 求旋转矩阵c++/c#代码 知道两个向量求他们的旋转矩阵的主要内容,如果未能解决你的问题,请参考以下文章