二维旋转公式

ros的tf工具包可以很方便的实现任意坐标系之间的坐标转换。但是，如果只是想简单的测试想法，而又不想编写过于庞杂的代码，考虑自己写二维旋转的函数。而与二维旋转问题对偶的另一个问题便是二维坐标系旋转变换。这两个问题的形式基本一样，只是旋转的角度相差一个负号。就是这个容易搞混，所以做个笔记，以备查用。

1. 二维旋转公式（算法）

而（此文只针对二维）旋转则是表示某一坐标点$(x_1, y_1)$某一坐标系下绕原点逆时针（正向）旋转角度$\theta$后得到新的坐标点$(x_2, y_2)$。

推导： 假定$v=(x, y)$,$v'=(x',y')$,如上图有$x=rcos(\phi),y=rsin(\phi),x'=rcos(\phi+\theta),y'=rsin(\phi+\theta)$(注意，上图有几处错误，坐标轴边上的$cos/sin(\theta)$应改为$cos/sin(\phi+\theta$)。展开$x',y'$可得： $x'=rcos(\phi)cos(\theta)-rsin(\phi)sin(\theta)=xcos(\theta)-ysin(\theta)$ $y'=rsin(\phi)cos(\theta)+rcos(\phi)sin(\theta)=xsin(\theta)+ycos(\theta)$

矩阵形式为:$\left[\begin{matrix}x' \\ y'\end{matrix}\right]=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right]\left[\begin{matrix}x \\ y\end{matrix}\right]$

则二维旋转矩阵为：A=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right] \tag{1}

void Rotate2(double x1, double y1, double alpha, double& x2, double& y2)
{
	x2 = x1 * cos(alpha) - y1 * sin(alpha);
	y2 = x1 * sin(alpha) + y1 * cos(alpha);
}

2. 二维坐标系旋转变换

假设有一坐标系$XOY$，经过逆时针(正向)旋转角度$\theta$后，得到新的坐标系$X'O‘Y'$。得到原来坐标系中的坐标$(x,y)$在新坐标系下的坐标值被称为坐标系转换。

$x'=xcos(\theta)+ysin(\theta)=xcos(-\theta)-ysin(-\theta)$ $y'=-xsin(\theta)+ycos(\theta)=xsin(-\theta)+ycos(-\theta)$

所以二维坐标旋转变换矩阵为:B=\left[\begin{matrix} cos(\theta) & sin(\theta) \\ -sin(\theta) & cos(\theta)\end{matrix}\right]=\left[\begin{matrix} cos(-\theta) & -sin(-\theta) \\ sin(-\theta) & cos(-\theta)\end{matrix}\right] \tag{2}

结论

对比公式（1）与（2），可发现，二维旋转与二维坐标旋转形式一致，只是当旋转都为正向（逆时针）时，角度相差一个负号。也即，在同一坐标轴下将某一点$(x,y)$沿原点正向（逆时针）旋转角度$\theta$后得到的新坐标点$(x',y')$，等价于将点$(x,y)$所在的坐标系$XOY$逆向(顺时针)旋转角度$\theta$后，在新的坐标系$X'O'Y'$下，$(x,y)$对应的新坐标点$(x',y')$。拿起纸笔，多摆弄上面两张解释图，就清楚了。