知道旋转前后矩阵向量值 如何去求旋转矩阵 R 的 c++/c# 代码???
因为需要用到矩阵处理库所以需要先配置
一, Eigen 库的配置(VS2017)
Eigen 库下载:
下载文件并解压:
然后在自己的 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. 由点积定义可知:
可推出 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) 面上的旋转公式可得:
推出:
已知:
将上式带入 v'的公式:
将 cz 替换掉, 可得:
将上式中的叉乘表示为反对称矩阵得:
另外:
最终可以推出:
上式即为罗德里格旋转公式.
参考博客: https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html
参考博客里的是 c# 的实现代码
我是参考完之后改了一个 c++ 的版本出来
若有兴趣交流分享技术, 可关注本人公众号, 里面会不定期的分享各种编程教程, 和共享源码, 诸如研究分享关于 c/c++,python, 前端, 后端, opencv,halcon,opengl, 机器学习深度学习之类有关于基础编程, 图像处理和机器视觉开发的知识
来源: https://www.cnblogs.com/DOMLX/p/12115244.html