旋转向量 和 旋转矩阵的相互转换的几种方法

最近在做旋转向量转旋转矩阵的，尝试了几种方法，现在总结一下，方便以后使用，下面几种方法求出来的旋转矩阵都是一样的，但是在使用之前需要对旋转向量进行单位化。

%% 给定旋转角度和旋转向量
pi=3.1415926;
qq_1= - pi/6;
p1=[0.8;0.6;0.5];
p2=[1;2;3];
PP=p2-p1;

%% 对旋转向量进行单位化
PP=PP/sqrt( PP(1)^2 + PP(2)^2 + PP(3)^2 );

%% 采用旋量法进行求解旋转矩阵，并且把位置也考虑了进去
w_1=PP;          %每个关节转轴在全局坐标系中的方向
r_1=p2;          %旋量中一点在全局坐标系中的位置,旋转轴上的任意位置都是可以的
c_1=[cross(r_1,w_1);w_1];
R_1 = C_poe(c_1,qq_1);   %

%% 采用罗德里格斯公式，  需要进行单位化，不进行单位化的话，算出来的结果是不一样的
R_2= cos(qq_1)*eye(3) + (1 - cos(qq_1) )* PP *PP' + sin(qq_1) *Anti_matrix(PP);

%% 采用《机器人学：建模、规划、控制》中的旋转变换矩阵， 也是需要进行单位化，不进行单位化的话，算出来的结果是不一样的
R_3=zeros(3,3);
R_3(1,1)=PP(1)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(1,2)=PP(1)*PP(2) *(1 - cos(qq_1)) - PP(3)*sin(qq_1);
R_3(1,3)=PP(1)*PP(3) *(1 - cos(qq_1)) +  PP(2)*sin(qq_1);
R_3(2,1)=PP(1)*PP(2) *(1 - cos(qq_1)) + PP(3)*sin(qq_1);
R_3(2,2)=PP(2)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(2,3)=PP(2)*PP(3) *(1 - cos(qq_1)) - PP(1)*sin(qq_1);
R_3(3,1)=PP(1)*PP(3) *(1 - cos(qq_1)) -  PP(2)*sin(qq_1);
R_3(3,2)=PP(2)*PP(3) *(1 - cos(qq_1)) + PP(1)*sin(qq_1);
R_3(3,3)=PP(3)^2 *(1 - cos(qq_1)) + cos(qq_1);


%% 采用Matlab 中的库进行操作，不需要进行单位化
axang = [PP(1) PP(2) PP(3) qq_1];   %% 不进行单位化也是可以的
R_4= axang2rotm(axang);


%% 指数积公式
function pm = C_poe(vs,theta)
v=vs(1:3);
w=vs(4:6);

if(norm(w)<1e-10)
    pm=[
        eye(3)    ,v(:)*theta
        zeros(1,3),1];
else  
    R = eye(3) + sin(theta)*Anti_matrix(w) + (1-cos(theta))*Anti_matrix(w)*Anti_matrix(w);
    p = (eye(3)-R)*Anti_matrix(w)*v + theta * w' * v * w;

    pm=[
     R          , p 
     zeros(1,3) , 1   ];
end

end

% 求反对称矩阵
function C3 = Anti_matrix(w)

C3=[
     0    -w(3)  w(2)
     w(3)  0    -w(1)
    -w(2)  w(1)  0    ];

end
指数积公式C++版
void Anti_matrix(Vector3d&w, Matrix3d& R)
{
	R << 0, -w(2), w(1),
		w(2), 0, -w(0),
		-w(1), w(0), 0;
}

void POE(VectorXd &vs,double theta,Matrix4d &T )
{
	Vector3d v, w;
	v << vs(0), vs(1), vs(2);
	w << vs(3), vs(4), vs(5);
	Matrix3d r, R;
	Vector3d p;
	Matrix3d unit_matrix = Matrix3d::Identity(); // 单位矩阵

	if ( w.norm() < 1e-10)
	{
		T <<0,0, 0, v(0)*theta,
			0, 0, 0, v(1)*theta,
			0, 0, 0, v(2)*theta,
			0, 0, 0, 1;
	}
	else
	{
		Anti_matrix(w, r);  //反对称矩阵
		R = unit_matrix + sin(theta)*r + (1 - cos(theta))*r*r;
		p = (unit_matrix - R)*r*v + theta * w.transpose() * v * w;
	}

		T << R(0, 0), R(0, 1), R(0, 2), p(0),
		     R(1, 0), R(1, 1), R(1, 2), p(1),
		     R(2, 0), R(2, 1), R(2, 2), p(2),
		           0,       0,       0,    1;

}
int main(int *argc, int ** argv)
{
	Vector3d p1, p2, p3;
	p1 << 0, -1, 0;
	p2 << 0, 0, 1;
	Vector3d w_1 = p2-p1;          //每个关节转轴在全局坐标系中的方向
	w_1 = w_1.normalized();        //对向量进行单位化
	Vector3d r_1 = p2;         //旋量中一点在全局坐标系中的位置
	VectorXd vs(6);
	vs << r_1.cross(w_1)(0), r_1.cross(w_1)(1), r_1.cross(w_1)(2), w_1(0), w_1(1), w_1(2);
	double theta = 3.1417926 / 3;  //旋转角度
	Matrix4d T;
	POE(vs, theta, T);
	cout << T << endl;
	system("pause");
	return 0;
}