function q = rv2q(rv)  % 等效旋转矢量转换为变换四元数
    nm2 = rv'*rv;  % 旋转矢量的模方
    if nm2<1.0e-8  % 如果模方很小，则可用泰勒展开前几项求三角函数
        q0 = 1-nm2*(1/8-nm2/384); 
        s = 1/2-nm2*(1/48-nm2/3840);
    else
        nm = sqrt(nm2);
        q0 = cos(nm/2); 
        s = sin(nm/2)/nm;
    end
    q = [q0; s*rv];