背景：給定兩個(gè)向量V1,V2，怎么求之間的轉(zhuǎn)換矩陣?
[羽量級(jí)公司發(fā)月餅了，比去年強(qiáng)，手提式的了\dog]
1、求旋轉(zhuǎn)軸
通過V1和V2的叉積，可以知道旋轉(zhuǎn)軸Vcross（垂直屏幕指向外側(cè)或內(nèi)側(cè)的向量）
vcross = np.cross(v1, v2)這里還可以將旋轉(zhuǎn)軸向量單位化，方便后面使用
vcross_normalize = 1 / np.linalg.norm(vcross) * vcross?2、求旋轉(zhuǎn)角度
還可以知道兩向量的夾角（這個(gè)夾角是沿著旋轉(zhuǎn)軸的角度，因?yàn)閮上蛄看_定了一個(gè)平面，而旋轉(zhuǎn)軸是垂直平面的）
theta = math.acos(v1.dot(v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))?3、求旋轉(zhuǎn)矩陣
知道了旋轉(zhuǎn)軸以及旋轉(zhuǎn)角度，就可以祭出羅德里格斯(Rodriguez)公式了，可以直接代入得出旋轉(zhuǎn)矩陣
?上式中的分別指單位化后的旋轉(zhuǎn)軸向量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) # 轉(zhuǎn)為矩陣方便后續(xù)運(yùn)算這樣，就求出了旋轉(zhuǎn)矩陣R。?
4、驗(yàn)證
畢竟這里沒有推導(dǎo)，拿過來就用，多少有點(diǎn)犯嘀咕，那就驗(yàn)證一下吧。
旋轉(zhuǎn)后的點(diǎn)=R*旋轉(zhuǎn)前的點(diǎn)
旋轉(zhuǎn)前的點(diǎn) =?*旋轉(zhuǎn)后的點(diǎn)
預(yù)期效果：
?假如在水平方向有一條線，所有點(diǎn)的y坐標(biāo)都相同，順時(shí)針旋轉(zhuǎn)45度后，所有點(diǎn)的y坐標(biāo)應(yīng)該是一個(gè)等差的，斜率為-1。（此時(shí)的兩個(gè)向量v1，v2分別?。?,1,0）和（1,1,0），剛好是45度）
測(cè)試結(jié)果：
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)) # 創(chuàng)建畫布
#使用axisartist.Subplot方法創(chuàng)建一個(gè)繪圖區(qū)對(duì)象ax
ax = axisartist.Subplot(fig, 111) # 111 代表1行1列的第1個(gè)，subplot()可以用于繪制多個(gè)子圖
fig.add_axes(ax) # 將繪圖區(qū)對(duì)象添加到畫布中
# ----------2. 繪制帶箭頭的x-y坐標(biāo)軸#通過set_visible方法設(shè)置繪圖區(qū)所有坐標(biāo)軸隱藏-------
ax.axis[:].set_visible(False) # 隱藏了四周的方框
#ax.new_floating_axis代表添加新的坐標(biāo)軸
ax.axis["x"] = ax.new_floating_axis(0,0)
ax.axis["x"].set_axisline_style("->", size = 1.0) # 給x坐標(biāo)軸加上箭頭
ax.axis["y"] = ax.new_floating_axis(1,0) # 添加y坐標(biāo)軸，且加上箭頭
ax.axis["y"].set_axisline_style("-|>", size = 1.0)
#設(shè)置x、y軸上刻度顯示方向
ax.axis["x"].set_axis_direction("top")
ax.axis["y"].set_axis_direction("right")
##################
##畫原始點(diǎn)
p_arr = np.array(p_arr)
plt.plot(p_arr[:,0],p_arr[:,1],color='red')
''' 設(shè)置x軸的刻度：plt.xlim() '''
plt.xlim(-20,20) # 設(shè)置x軸的刻度從-2到12
''' 設(shè)置y軸的刻度：plt.ylim() '''
plt.ylim(-2,20) # 設(shè)置x軸的刻度從2到10
##畫旋轉(zhuǎn)后的點(diǎn)
p_out = np.array(p_out)
plt.plot(p_out[:,0],p_out[:,1],color='blue')
plt.show()?5、驗(yàn)證2
上面是正向驗(yàn)證，在反向驗(yàn)證一下，給一些斜線上的點(diǎn)比如y=-0.5x+10，將其旋轉(zhuǎn)到水平（即y軸相等），可以設(shè)置V1=（0,1,0），V2=（0.5,1,0），計(jì)算的R旋轉(zhuǎn)矩陣還是從V1向V2旋轉(zhuǎn)，此時(shí)想將旋轉(zhuǎn)后的點(diǎn)變換到旋轉(zhuǎn)前，需要使用R的逆（這里旋轉(zhuǎn)矩陣應(yīng)該是正交矩陣，轉(zhuǎn)置即為逆）
旋轉(zhuǎn)后的點(diǎn)=R*旋轉(zhuǎn)前的點(diǎn)
旋轉(zhuǎn)前的點(diǎn) =?*旋轉(zhuǎn)后的點(diǎn)
?預(yù)期效果：
測(cè)試結(jié)果：
p_out = []
for p in p_arr:
tmp = R.I*np.matrix(p).T
p_out.append(tmp)最后貼一個(gè)根據(jù)兩個(gè)向量計(jì)算它們之間的旋轉(zhuǎn)矩陣 - 朔月の流光 - 博客園
2022年9月1日add
上面貼的鏈接圖有誤，加減號(hào)有問題。如下例子。文章來源：http://www.zghlxwxcb.cn/news/detail-434699.html
Mat get_rot_matrix()
{
Point3d norm_wall = Point3d(0, 1, 0);
Point3d norm_pro = Point3d(1, 0, 0);
Point3d va = norm_wall / norm(norm_wall);
Point3d vb = norm_pro / norm(norm_pro);
Point3d vs = va.cross(vb);
Point3d v = vs / norm(vs);
double ca = vb.dot(va);
Point3d vt = v * (1 - ca);
Mat rot = Mat::zeros(3, 3, CV_64FC1);
rot.at<double>(0, 0) = vt.x * v.x + ca;
rot.at<double>(1, 1) = vt.y * v.y + ca;
rot.at<double>(2, 2) = vt.z * v.z + ca;
vt.x *= v.y;
vt.z *= v.x;
vt.y *= v.z;
rot.at<double>(0, 1) = vt.x - vs.z;
rot.at<double>(0, 2) = vt.z + vs.y;
rot.at<double>(1, 0) = vt.x + vs.z;
rot.at<double>(1, 2) = vt.y - vs.x;
rot.at<double>(2, 0) = vt.z - vs.y;
rot.at<double>(2, 1) = vt.y + vs.x;
cout << rot << endl;
return rot;
}--end--文章來源地址http://www.zghlxwxcb.cn/news/detail-434699.html
到了這里，關(guān)于C++/Python:羅德里格斯旋轉(zhuǎn)矩陣的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
