由旋转矩阵到Rodrigues

由Rodrigues公式有：
$$R=Icos\theta +(1-cos\theta )u{u}^T+u_{\times }sin\theta$$
其中，$Icos\theta +(1-cos\theta )u{u}^T$是对称矩阵，$u_{\times }sin\theta$是反对称矩阵。
所以，
$$R-R^T=2u_{\times }sin\theta$$
因为$R(\hat{n},\theta )=PR(\hat{z},\theta )P^{-1}$,
所以$tr(R)=2cos\theta -1$
结合$\Vert R-R^T\Vert =2sin\theta$可以求解$u、\theta$
参考代码：https://github.com/zjulion/handeyecat

Geo3d rodrigues2(const RotMat& matrix)
{
	Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
	RotMat R = svd.matrixU() * svd.matrixV().transpose();

	double rx = R(2, 1) - R(1, 2);
	double ry = R(0, 2) - R(2, 0);
	double rz = R(1, 0) - R(0, 1);

	double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
	double c = (R.trace() - 1) * 0.5; //c=cos(theta)
	c = c > 1. ? 1. : c < -1. ? -1. : c;

	double theta = acos(c);

	if (s < XEPS)//sin s =0
	{
		double t;

		if (c > 0) //cos s=1
			rx = ry = rz = 0;
		else
		{
			t = (R( 0, 0) + 1)*0.5;
			rx = sqrt(std::max(t, 0.0));
			t = (R(1, 1) + 1)*0.5;
			ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0);
			t = (R(2, 2) + 1)*0.5;
			rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0);

			if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0))//这里不懂
				rz = -rz;
			theta /= sqrt(rx*rx + ry*ry + rz*rz);
			rx *= theta;
			ry *= theta;
			rz *= theta;
		}
	}
	else
	{
		double vth = 1 / (2 * s);
		vth *= theta;
		rx *= vth; ry *= vth; rz *= vth;
	}
	return Eigen::Vector3d(rx, ry, rz).cast<float>();
}

参考源：
https://www2.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.pdf
http://web.cs.iastate.edu/~cs577/handouts/rotation.pdf