本文的内容可以看结构图,结果和推导过程都有
ps:文章内容是自己写的,但是在Typora上用LaTex编辑好的公式没法在这里用,重新编辑成本太高,就很尴尬,只好用Typora把全文输出为图像,直接发了,可复制的代码部分和超链接会在最后给出
代码段:
import numpy as np
import os
def gen_rot_mtx(angle,isdegree): #生成旋转矩阵,逆时针为正方向
if isdegree:
angle = math.radians(angle)
cos_rad = math.cos(angle)
sin_rad = math.sin(angle)
trans = np.eye(3)
trans[:2,