旋转矩阵的雅可比矩阵怎么求，C++代码怎么写

旋转矩阵的雅可比矩阵可以通过求取对应的旋转向量的旋转矩阵的导数来得到。假设旋转向量为 $\boldsymbol{\theta} = (\theta_1, \theta_2, \theta_3)$，则旋转矩阵为：

$$
\mathbf{R}(\boldsymbol{\theta}) = \begin{pmatrix}
c_2 c_3 & -c_2 s_3 & s_2 \\
c_1 s_3 + c_3 s_1 s_2 & c_1 c_3 - s_1 s_2 s_3 & -c_2 s_1 \\
s_1 s_3 - c_1 c_3 s_2 & c_3 s_1 + c_1 s_2 s_3 & c_1 c_2
\end{pmatrix}
$$

其中 $c_i = \cos(\theta_i)$，$s_i = \sin(\theta_i)$。

对于旋转向量的雅可比矩阵，可以通过以下公式计算：

$$
\frac{\partial \mathbf{R}}{\partial \boldsymbol{\theta}} = \begin{pmatrix}
\frac{\partial \mathbf{r}_1}{\partial \boldsymbol{\theta}} \\
\frac{\partial \mathbf{r}_2}{\partial \boldsymbol{\theta}} \\
\frac{\partial \mathbf{r}_3}{\partial \boldsymbol{\theta}} \\
\end{pmatrix}
$$

其中 $\mathbf{r}_i$ 表示旋转矩阵的第 $i$ 行，$\frac{\partial \mathbf{r}_i}{\partial \boldsymbol{\theta}}$ 可以通过以下公式计算：

$$
\frac{\partial \mathbf{r}_i}{\partial \boldsymbol{\theta}} = \begin{pmatrix}
-\sin \theta_i \cdot r_{i2} - \cos \theta_i \cdot r_{i3} \\
\cos \theta_i \cdot r_{i2} - \sin \theta_i \cdot r_{i3} \\
r_{i1}
\end{pmatrix}
$$

具体的 C++ 代码实现如下：

#include <Eigen/Dense>
using namespace Eigen;

// 计算旋转矩阵的雅可比矩阵
Matrix<double, 9, 3> rotation_jacobian(const Vector3d& theta)
{
    double c1 = cos(theta(0)), c2 = cos(theta(1)), c3 = cos(theta(2));
    double s1 = sin(theta(0)), s2 = sin(theta(1)), s3 = sin(theta(2));
    double r11 = c2 * c3, r12 = -c2 * s3, r13 = s2;
    double r21 = c1 * s3 + c3 * s1 * s2, r22 = c1 * c3 - s1 * s2 * s3, r23 = -c2 * s1;
    double r31 = s1 * s3 - c1 * c3 * s2, r32 = c3 * s1 + c1 * s2 * s3, r33 = c1 * c2;
    Matrix<double, 9, 3> J;
    // 计算第一行的雅可比矩阵
    J(0, 0) = -s1 * r21 - c1 * r31;
    J(1, 0) = -s1 * r22 - c1 * r32;
    J(2, 0) = -s1 * r23 - c1 * r33;
    // 计算第二行的雅可比矩阵
    J(3, 1) = -s2 * r11 - c2 * r31;
    J(4, 1) = -s2 * r12 - c2 * r32;
    J(5, 1) = -s2 * r13 - c2 * r33;
    // 计算第三行的雅可比矩阵
    J(6, 2) = -s3 * r11 - c3 * r21;
    J(7, 2) = -s3 * r12 - c3 * r22;
    J(8, 2) = -s3 * r13 - c3 * r23;
    return J;
}