功能:旋转矩阵R转旋转向量

格式:SLAM_RToQ(R)

R:一个3*3的旋转矩阵

说明: 
1、本函数不检测R的旋转性质，只检查其维度
2、函数执行完毕返回一个4*1的矩阵变量，其分别对应单位四元数[c, m * s , n * s , l * s],其中 c = cos(theta/2), s = sin(theta/2), [m,n,l]为单位旋转轴向量, theta 为绕单位旋转轴向量扰动的角度

例子:
v = 1:3;
r = {SLAM_VToR<矩阵运算\SLAM_VToR>}(v);
q = SLAM_RToQ(r)//回车得到如下结果
q =
[ 0.29555112749297
  0.25532186004526
  0.51064372009052
  0.76596558013579 ]