C++/Python:罗德里格斯旋转矩阵
背景:给定两个向量V1,V2,怎么求之间的转换矩阵?
[羽量级公司发月饼了,比去年强,手提式的了\dog]
1、求旋转轴
通过V1和V2的叉积,可以知道旋转轴Vcross(垂直屏幕指向外侧或内侧的向量)
vcross = np.cross(v1, v2)
这里还可以将旋转轴向量单位化,方便后面使用
vcross_normalize = 1 / np.linalg.norm(vcross) * vcross
2、求旋转角度
还可以知道两向量的夹角(这个夹角是沿着旋转轴的角度,因为两向量确定了一个平面,而旋转轴是垂直平面的)
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 npnorm_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-10y = 10p_arr.append([x,y,0])p_out = []
for p in p_arr:tmp = R*np.matrix(p).Tp_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).Tp_out.append(tmp)
最后贴一个根据两个向量计算它们之间的旋转矩阵 - 朔月の流光 - 博客园
2022年9月1日add
上面贴的链接图有误,加减号有问题。如下例子。
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--
C++/Python:罗德里格斯旋转矩阵相关推荐
- 转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换
文章目录 1. 转换矩阵.平移矩阵.旋转矩阵之间的关系 2. 缩放变换.平移变换和旋转变换 2. python实现旋转矩阵.四元数.欧拉角互相转化 由于在平时总是或多或少的遇到平移旋转的问题,每次都是 ...
- python实现:旋转矩阵转换为四元数
1 python实现旋转矩阵转换为四元数 例如:下面把3x3的旋转矩阵转换为四元数 from pyquaternion import Quaternionrotate_matrix = [[-0.01 ...
- 2、计算机图形学——3D变换
1.1.3D点和向量的齐次坐标表示 根据1中的第二部分齐次坐标可知,3D点的齐次坐标可表示为 1.2.3D仿射变换的一般形式 根据1中的第三部分变换的组合可知,3D点的仿射变换的齐次坐标方程可表示为 ...
- 详解基于罗德里格斯(Rodrigues)公式由旋转向量到旋转矩阵的 Python 实现
文章目录 旋转向量 rotation vector 旋转矩阵 rotation matrix 罗德里格斯公式 Rodrigues' formula 基于 Python 和 NumPy 实现 Rodri ...
- python 实现罗德里格斯公式Rodrigues 旋转向量到旋转矩阵转化
罗德里格斯公式Rodrigues: 代码: import numpy as npdef rodrigues_rotation(r, theta):# n旋转轴[3x1]# theta为旋转角度# 旋转 ...
- python二郎成长笔记(三)(matlab标定工具箱详解,旋转矩阵旋转向量,matlab标定数据传入opencv)
这一章由来:二郎之前研究matlab的双目立体视觉,已经得到了需要的信息,可是,二郎想要对代码进行修改使其更适宜自己的应用目的. 修改:标定不用修改--matlab内置的已经很强大了,而且没有必要用p ...
- 罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
- python旋转矩阵_python实现回旋矩阵方式(旋转矩阵)
我们知道Python中是没有数组 这种数据结构的,所以要想实现回旋矩阵,需要先导入一个numpy包, 它是一个由多维数组对象和用于处理数组的例程集合组成的python扩充程序库,可以用来做数组算数和逻 ...
- python中将四元数转换为旋转矩阵
在制作bundlefusion时,想测试TUM数据集,并且将groundtruth写入到数据集中,TUM中给定的groundtruth中的旋转是使用四元数表示的,而bundlefusion中需要SE3 ...
最新文章
- C++ OJ 中多行数据输入(大小写转换、通过移位运算实现2的n次方、多组输入,每行输入数量不一样)
- 【西安交大】2020年最新深度学习目标检测方法综述
- 【一周速递】计算机视觉/图像处理论文集
- 通用印刷体识别 php,实战腾讯云ORC文字识别
- mysql 如何调用函数结果_MySQL自定义函数调用不出结果
- xxljob默认登录_xxl-job安装部署整理
- 图注意力网络_EMNLP 2019开源论文:针对短文本分类的异质图注意力网络
- java.sql.SQLException: null, message from server: “Host ‘xxx.xxx.xxx.xxx‘ is not allowed to
- https://127.0.0.1:8080/test?param={%22..报错
- 9.1 正则介绍_grep(上)
- resultset需要关闭吗_你给家里的采暖壁挂炉做保养了吗?
- python做正态分布的例子_python 绘制正态曲线的示例
- onlyoffice mysql_Onlyoffice部署文档
- Python中的shuffle()函数
- Siege(开源Web压力测试工具)——多线程编程最佳实例
- 磁盘格式怎么转换?Tuxera NTFS对磁盘进行不同格式转换教程
- impala添加kerberos认证
- 怎么看自己的操作系统?
- 【不知道发啥】Win7网页版使用方法
- JAVA面向对象编程学习 (1)语法基础与类与对象
热门文章
- LineageOS 19 现已正式发布
- singleLine 过时
- 大厂竞业协议是“巨坑”?科大讯飞前员工、团队创始人跳槽腾讯被判赔 1200 万...
- 川西红叶:谁能抵挡如此色诱
- AI时代的视频云转码移动端化——更快、更好,更低,更广
- vk32xx linux,将自己的驱动加到内核
- 【无标题】VMware开启虚拟机提示此主机支持 Intel VT-x,但 Intel VT-x 处于禁用状态。
- 微信小程序自带的referer (附:微信,头条,支付宝,百度小程序的自带referer)
- “智能之约,链动未来”——区块链下的创新思考(深圳)成功举行
- x38和x48是服务器芯片吗,X38的翻版:微星X48主板首度曝光