C++/Python:罗德里格斯旋转矩阵
背景：给定两个向量V1,V2，怎么求之间的转换矩阵?
[羽量级公司发月饼了，比去年强，手提式的了\dog]
1、求旋转轴
通过V1和V2的叉积，可以知道旋转轴Vcross（垂直屏幕指向外侧或内侧的向量）
vcross = np.cross(v1, v2)这里还可以将旋转轴向量单位化，方便后面使用
vcross_normalize = 1 / np.linalg.norm(vcross) * vcross2、求旋转角度
还可以知道两向量的夹角（这个夹角是沿着旋转轴的角度，因为两向量确定了一个平面，而旋转轴是垂直平面的）
theta = math.acos(v1.dot(v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))3、求旋转矩阵
知道了旋转轴以及旋转角度，就可以祭出罗德里格斯(Rodriguez)公式了，可以直接代入得出旋转矩阵
上式中的分别指单位化后的旋转轴向量vcross_normalize的xyz
Rot = np.zeros((3, 3), dtype=np.float32)
Rot[0][0] = math.cos(theta)+(1-math.cos(theta))*(vcross_normalize[0]**2)
Rot[0][1] = vcross_normalize[0]*vcross_normalize[1]*(1-math.cos(theta))-vcross_normalize[2]*math.sin(theta)
Rot[0][2] = vcross_normalize[1]*math.sin(theta)+vcross_normalize[0]*vcross_normalize[2]*(1-math.cos(theta))
Rot[1][0] = vcross_normalize[2]*math.sin(theta)+vcross_normalize[0]*vcross_normalize[1]*(1-math.cos(theta))
Rot[1][1] = math.cos(theta)+(1-math.cos(theta))*(vcross_normalize[1]**2)
Rot[1][2] = -vcross_normalize[0] * math.sin(theta) + vcross_normalize[1] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][0] = -vcross_normalize[1] * math.sin(theta) + vcross_normalize[0] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][1] = vcross_normalize[0] * math.sin(theta) + vcross_normalize[1] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][2] = math.cos(theta) + (1 - math.cos(theta)) * (vcross_normalize[2] ** 2)
R = np.matrix(Rot) # 转为矩阵方便后续运算这样，就求出了旋转矩阵R。
4、验证
毕竟这里没有推导，拿过来就用，多少有点犯嘀咕，那就验证一下吧。
旋转后的点=R*旋转前的点
旋转前的点 = *旋转后的点
预期效果：
假如在水平方向有一条线，所有点的y坐标都相同，顺时针旋转45度后，所有点的y坐标应该是一个等差的，斜率为-1。（此时的两个向量v1，v2分别取（0,1,0）和（1,1,0），刚好是45度）
测试结果：
import matplotlib.pyplot as plt
import mpl_toolkits.axisartist as axisartist
import cv2
import math
import numpy as np
norm_wall = np.array([0, 1, 0],dtype=np.float32)
norm_pro = np.array([1,1,0],dtype=np.float32)
theta = math.acos(norm_wall.dot(norm_pro)/(np.linalg.norm(norm_wall)*np.linalg.norm(norm_pro)))
vcross = np.cross(norm_wall, norm_pro)
print(vcross)
vcross_normalize = 1 / np.linalg.norm(vcross) * vcross
print(vcross_normalize)
Rot = np.zeros((3, 3), dtype=np.float32)
Rot[0][0] = math.cos(theta)+(1-math.cos(theta))*(vcross_normalize[0]**2)
Rot[0][1] = vcross_normalize[0]*vcross_normalize[1]*(1-math.cos(theta))-vcross_normalize[2]*math.sin(theta)
Rot[0][2] = vcross_normalize[1]*math.sin(theta)+vcross_normalize[0]*vcross_normalize[2]*(1-math.cos(theta))
Rot[1][0] = vcross_normalize[2]*math.sin(theta)+vcross_normalize[0]*vcross_normalize[1]*(1-math.cos(theta))
Rot[1][1] = math.cos(theta)+(1-math.cos(theta))*(vcross_normalize[1]**2)
Rot[1][2] = -vcross_normalize[0] * math.sin(theta) + vcross_normalize[1] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][0] = -vcross_normalize[1] * math.sin(theta) + vcross_normalize[0] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][1] = vcross_normalize[0] * math.sin(theta) + vcross_normalize[1] * vcross_normalize[2] * (
1 - math.cos(theta))
Rot[2][2] = math.cos(theta) + (1 - math.cos(theta)) * (vcross_normalize[2] ** 2)
R = np.matrix(Rot)
p_arr = []
for i in range(20):
x = i-10
y = 10
p_arr.append([x,y,0])
p_out = []
for p in p_arr:
tmp = R*np.matrix(p).T
p_out.append(tmp)
################
fig = plt.figure(figsize=(80, 80)) # 创建画布
#使用axisartist.Subplot方法创建一个绘图区对象ax
ax = axisartist.Subplot(fig, 111) # 111 代表1行1列的第1个，subplot()可以用于绘制多个子图
fig.add_axes(ax) # 将绘图区对象添加到画布中
# ----------2. 绘制带箭头的x-y坐标轴#通过set_visible方法设置绘图区所有坐标轴隐藏-------
ax.axis[:].set_visible(False) # 隐藏了四周的方框
#ax.new_floating_axis代表添加新的坐标轴
ax.axis["x"] = ax.new_floating_axis(0,0)
ax.axis["x"].set_axisline_style("->", size = 1.0) # 给x坐标轴加上箭头
ax.axis["y"] = ax.new_floating_axis(1,0) # 添加y坐标轴，且加上箭头
ax.axis["y"].set_axisline_style("-|>", size = 1.0)
#设置x、y轴上刻度显示方向
ax.axis["x"].set_axis_direction("top")
ax.axis["y"].set_axis_direction("right")
##################
##画原始点
p_arr = np.array(p_arr)
plt.plot(p_arr[:,0],p_arr[:,1],color='red')
''' 设置x轴的刻度：plt.xlim() '''
plt.xlim(-20,20) # 设置x轴的刻度从-2到12
''' 设置y轴的刻度：plt.ylim() '''
plt.ylim(-2,20) # 设置x轴的刻度从2到10
##画旋转后的点
p_out = np.array(p_out)
plt.plot(p_out[:,0],p_out[:,1],color='blue')
plt.show()5、验证2
上面是正向验证，在反向验证一下，给一些斜线上的点比如y=-0.5x+10，将其旋转到水平（即y轴相等），可以设置V1=（0,1,0），V2=（0.5,1,0），计算的R旋转矩阵还是从V1向V2旋转，此时想将旋转后的点变换到旋转前，需要使用R的逆（这里旋转矩阵应该是正交矩阵，转置即为逆）
旋转后的点=R*旋转前的点
旋转前的点 = *旋转后的点
预期效果：
测试结果：
p_out = []
for p in p_arr:
tmp = R.I*np.matrix(p).T
p_out.append(tmp)最后贴一个根据两个向量计算它们之间的旋转矩阵 - 朔月の流光 - 博客园
--end--