推荐开源项目:简单的SLAM与机器人教程与编程实践-github
我们在做几何变换的时候经常需要把某个坐标系上的所有点都进行一个旋转,这个操作就叫做刚体旋转(所有的点相对位置不变的发生旋转)。下图是一个典型的二维坐标系下刚体旋转。我们把蓝色的坐标系旋转了 θ \theta θ度,新坐标系就是红色的坐标系。我们现在已知一个点相对红色那个坐标系的坐标 ( x r e d , y r e d ) (x_{red},y_{red}) (xred,yred),和已知旋转角度 θ \theta θ,然后我们想求得该点相对于蓝色那个坐标系的坐标 ( x b l u e , y b l u e ) (x_{blue},y_{blue}) (xblue,yblue)。
这个其实很简单我们利用高中学的三角几何就可以轻松解决。从上图可以发现不变量是黑色的那个线段的长度。而这个长度我们是可以根据黑色点相对红色坐标系下的坐标 ( x r e d , y r e d ) (x_{red},y_{red}) (xred,yred)算出来的。黑色线段长度为 r = x r e d 2 + y r e d 2 r=\sqrt {x_{red}^2+y_{red}^2} r=xred2+yred2</