根据旋转前后的向量值求旋转矩阵

如果已知旋转前后的一向量的变化，那么该如何求这个旋转矩阵呢？本篇结合Rodrigues' rotation formula，介绍一下该旋转矩阵的求法。

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替换掉，可得：

将上式中的叉乘表示为反对称矩阵得：

另外：

最终可以推出：

上式即为罗德里格旋转公式。

4. 求旋转矩阵

假设（旋转轴）的向量n=(nx, ny, nz);

I是单位矩阵，A是向量n的反对称矩阵，即

而θ旋转角度

最后求出的旋转矩阵如下

通过旋转矩阵可以求出（向量a）绕（旋转轴）旋转（角度θ）得到的（向量b）

设3X3的（旋转矩阵）为R

v' = Rv

根据旋转前后的两个向量值，使用上面的方法，先求出旋转角度和旋转轴，然后用罗德里格旋转公式即可求出对应的旋转矩阵。

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, 1] = u[0] * u[1] * (1 - Math.Cos(angle) - u[2] * Math.Sin(angle));rotatinMatrix[0, 2] = u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));rotatinMatrix[1, 0] = u[2] * Math.Sin(angle) + u[0] * u[1] * (1 - Math.Cos(angle));rotatinMatrix[1, 1] = Math.Cos(angle) + u[1] * u[1] * (1 - Math.Cos(angle));rotatinMatrix[1, 2] = -u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));rotatinMatrix[2, 0] = -u[1] * Math.Sin(angle) + u[0] * u[2] * (1 - Math.Cos(angle));rotatinMatrix[2, 1] = u[0] * Math.Sin(angle) + u[1] * u[2] * (1 - Math.Cos(angle));rotatinMatrix[2, 1] = Math.Cos(angle) + u[2] * u[2] * (1 - Math.Cos(angle));return rotatinMatrix;
}

https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

[学习SLAM]两平面法向量与旋转向量/旋转矩阵，根据两平面法向量进行旋转相关推荐

SLAM学习之路（三）--旋转向量与欧拉角
一.旋转向量发明目的:希望有一种方式可以紧凑地描述旋转和平移,如用一个三维向量表达旋转,用六维向量表达变换. 任意坐标系的旋转,都可以用一个旋转轴和一个旋转角刻画.可以使用一个向量,其方向与旋转轴一 ...
从零开始一起学习SLAM | 三维空间刚体的旋转
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达刚体,顾名思义,是指本身不会在运动过程中产生形变的物体,如相机的运 ...
第3讲旋转向量、欧拉角、四元数
旋转向量从上一篇中已经知道,旋转可以用旋转矩阵来表示,变换可以用变换矩阵来表示,那么为什么还需要旋转向量呢? 仔细想一下,矩阵表示方式至少有以下几个缺点: 的旋转矩阵有9个量,但是一次旋转只有3个自 ...
【学习SLAM】ORB_SLAM2 双目测试(1)
针对双目相机和RGB-D相机的ORB-SLAM2建立在单目ORB-SLAM的基础上,它的核心组件,如图2所示. 图2 ORB-SLAM2由三个平行的线程组成,跟踪,局部建图和回环检测.在一次回环检测后 ...
【自动驾驶】24.欧拉角、旋转向量、四元数、万向锁
原文链接:https://blog.csdn.net/shenxiaolu1984/article/details/50639298 原文链接:https://blog.csdn.net/lql071 ...
罗德里格斯（Rodrigues）旋转向量与矩阵的变换
在做双目立体视觉深度图像生成的时候,遇到旋转向量(1x3)与旋转矩阵(3x3)的概念,得知二者可以通过罗德里格斯相互转化. 1.旋转的表示处理三维旋转问题时,通常采用旋转矩阵的方式来描述旋转变换.旋 ...
一、旋转矩阵，旋转向量，单位四元数的相互转换总结
文章目录前言一.要点 1. 旋转矩阵 2. 旋转向量 3. 单位四元数二.旋转向量--->旋转矩阵(罗德里格斯公式) 三.旋转矩阵--->旋转向量四.单位四元数--->旋转矩 ...
旋转向量与欧拉角罗德里格斯公式（Rodrigues's Formula）
旋转向量旋转矩阵表达方式旋转矩阵描述旋转,变换矩阵描述一个6自由度的三维刚体运动.但存在如下缺点: SO(3)的旋转矩阵有9个量,但一次旋转只有3个自由度.因此这种表达方式是冗余的.同理,变换矩阵 ...
方向向量转欧拉角_【姿态表示】旋转向量、旋转矩阵、欧拉角、四元数
1. 旋转矩阵与旋转向量旋转矩阵(Rotation Matrix)用 9 个量描述旋转的3个自由度,有冗余: 9 个量是有约束的:必须是正交矩阵,且行列式为 1 旋转向量(Rotation Vecto ...

[学习SLAM]两平面法向量与旋转向量/旋转矩阵，根据两平面法向量进行旋转