C++/Python:罗德里格斯旋转矩阵

这篇具有很好参考价值的文章主要介绍了C++/Python:罗德里格斯旋转矩阵。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

背景:给定两个向量V1,V2,怎么求之间的转换矩阵?

[羽量级公司发月饼了,比去年强,手提式的了\dog]


1、求旋转轴

通过V1和V2的叉积,可以知道旋转轴Vcross(垂直屏幕指向外侧或内侧的向量)

C++/Python:罗德里格斯旋转矩阵

vcross = np.cross(v1, v2)

这里还可以将旋转轴向量单位化,方便后面使用

vcross_normalize = 1 / np.linalg.norm(vcross) * vcross

 2、求旋转角度

还可以知道两向量的夹角(这个夹角是沿着旋转轴的角度,因为两向量确定了一个平面,而旋转轴是垂直平面的)

C++/Python:罗德里格斯旋转矩阵

theta = math.acos(v1.dot(v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))

 3、求旋转矩阵

知道了旋转轴以及旋转角度,就可以祭出罗德里格斯(Rodriguez)公式了,可以直接代入得出旋转矩阵

C++/Python:罗德里格斯旋转矩阵

 上式中的分别指单位化后的旋转轴向量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*旋转前的点

旋转前的点 = *旋转后的点

预期效果:

C++/Python:罗德里格斯旋转矩阵

 假如在水平方向有一条线,所有点的y坐标都相同,顺时针旋转45度后,所有点的y坐标应该是一个等差的,斜率为-1。(此时的两个向量v1,v2分别取(0,1,0)和(1,1,0),刚好是45度)

测试结果:

C++/Python:罗德里格斯旋转矩阵

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*旋转前的点

旋转前的点 = *旋转后的点

 预期效果:

C++/Python:罗德里格斯旋转矩阵

测试结果:

C++/Python:罗德里格斯旋转矩阵

p_out = []
for p in p_arr:
    tmp = R.I*np.matrix(p).T
    p_out.append(tmp)

最后贴一个根据两个向量计算它们之间的旋转矩阵 - 朔月の流光 - 博客园

C++/Python:罗德里格斯旋转矩阵


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--文章来源地址https://www.toymoban.com/news/detail-434699.html

到了这里,关于C++/Python:罗德里格斯旋转矩阵的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 面试金典07(Python)—— 旋转矩阵(中等)

    概述:给你一幅由 N × N 矩阵表示的图像,其中每个像素的大小为 4 字节。请你设计一种算法,将图像旋转 90 度。不占用额外内存空间能否做到? 思路:定义一个 nm 的空矩阵,依次循环替代即可。最后需要注意把原矩阵替换成新矩阵。 思路:和上面思路一致,不同在于这里

    2024年02月16日
    浏览(36)
  • python transforms3d,不同旋转方式与对应的旋转矩阵

    网上常看到的用法: 这种调用默认的是绕着物体上与基坐标系平行的坐标系旋转的,顺序是“x-y-z”,完整的写法是: 这里第四个参数的可选项为: 个人理解,这里的“s”表示的是“static”,即绕静坐标系(基坐标系)旋转;“r”表示“relative”,即相对坐标系(物体的连

    2024年02月13日
    浏览(32)
  • 四元数,旋转矩阵,欧拉角互转(python)

    运行代码之前需要安装pyquaternion和scipy。 pip install pyquaternion pip install scipy 代码之前放下面,main函数有使用的方式

    2024年02月12日
    浏览(46)
  • python 根据两个向量,求的之间的旋转矩阵:

    项目遇到一个问题,就是需要根据 原始向量 和 目标向量 求他们两个之间的 旋转矩阵 ,网上结果很少,也有很多错误,因此写了一个,项目中使用过程没出现问题。         原理参考的是知乎大佬的对   Rodrigues\\\' rotation formula  和 知乎 的总结,主要如下:

    2024年02月13日
    浏览(37)
  • 将欧拉角转换为旋转矩阵(手眼标定)python版本

    1、欧拉角版 2、四元数版 3、旋转矩阵版本 4、齐次矩阵

    2024年02月16日
    浏览(43)
  • 欧拉角,四元数和旋转矩阵互转代码【python版】

    欧拉角以 Roll、Pitch、Yaw 的顺序表示 四元数以[ q w q_w q w ​ , q x q_x q x ​ , q y q_y q y ​ , q z q_z q z ​ ]的顺序表示 代码包括了 欧拉角与四元数互转 , 旋转矩阵与四元数互转 , 欧拉角与旋转矩阵互转 ,输入参数均为 np.array 形式 代码内置了角度制和弧度制😃😃 当时因为这块

    2023年04月22日
    浏览(54)
  • Python:numpy.rot90() 三维矩阵绕着某一轴旋转

      在NumPy中,可以使用 numpy.rot90() 函数对三维矩阵绕着某个轴旋转。 numpy.rot90(m, k=1, axes=(0, 1)) . m: 输入的数组。 k: 旋转的次数,默认为1。 axes: 旋转的轴,默认为(0, 1)。旋转方向:是从第一轴到第二轴 下面是一个具体的例子,演示如何在三维矩阵上绕着某个轴旋转: 这里的

    2024年01月22日
    浏览(41)
  • LeetCode in Python 48. Rotate Image/Matrix (旋转图像/矩阵)

    旋转图像/矩阵的重点是寻找旋转前后对应位置的坐标关系。 示例: 图1 旋转图像/矩阵的输入输出示意图  代码:  解释: 1)外层循环控制需要转的大圈圈数,内层循环控制每一圈需要转的小圈圈数,大小圈数的解释见图2,例如n=4,需要循环n // 2 = 2大圈,其中黄色循环箭头

    2024年04月27日
    浏览(62)
  • 《动手学机器人学》7.2.4姿态之间的互相转换,Python&&C++支持四元数,欧拉角旋转矩阵、轴角

    本系列教程作者:小鱼 公众号:鱼香ROS QQ交流群:139707339 教学视频地址:小鱼的B站 完整文档地址:鱼香ROS官网 版权声明:如非允许禁止转载与商业用途。 上一节小鱼带你一起学习了四种姿态表示方式,这节课我们就利用相关的开源库,来完成姿态的不同表示方式之间的转

    2024年02月02日
    浏览(48)
  • ROS系列——使用python的transforms3d、numpy库实现四元数、旋转矩阵、欧拉角、轴角等的相互转换

    pip3 install transforms3d 四元数模块在transforms3d.quaternions里,直接导入即可使用 2.1.1四元数转旋转矩阵 2.1.2 旋转矩阵转四元数 2.2.1 四元数转轴角 2.2.2 轴角转四元数 四元数模块在transforms3d.euler里,直接导入即可使用 3.1.1 固定轴欧拉角转四元数 3.1.2 四元数转固定轴欧拉角 3.2.1 固定

    2024年02月07日
    浏览(93)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包