matlab子程序,rodrigues变换,旋转矩阵与旋转向量的相互转换