opencv c++ 旋转向量到旋转矩阵

在OpenCV（开源计算机视觉库）中，将旋转向量（Rotation Vector）转换为旋转矩阵是一个常见的操作，这对于图像旋转、姿态估计等任务十分有用。旋转向量通常用于欧拉角的一种表示形式，它描述了围绕三个轴（X、Y、Z）的微小旋转。

旋转向量可以由以下公式表示：
\[ \mathbf{v} = (x, y, w) \]
其中，\( x \) 和 \( y \) 分别代表绕x和y轴的旋转分量，而 \( w \) 是这两个旋转角度对应单位长度矢量的z分量，它相当于旋转角度的一半。

要将其转换为旋转矩阵（也称为正交变换矩阵），可以按照下面的步骤操作：

1. 首先，创建一个3x3的零矩阵：

   cv::Mat rotMat(3, 3, CV_64F);

2. 然后，使用以下公式填充矩阵：
- 如果 \( w \neq 0 \)，则旋转矩阵如下：
\[ R = \begin{bmatrix}
1 - 2(x^2 + w^2) & 2(yw - x) \\
2(xw - y) & 2(yw + x) & 1 - 2(x^2 + y^2)
\end{bmatrix}
\]

- 如果 \( w = 0 \) （即绕原点旋转），那么可以直接从 \( x \) 和 \( y \) 计算出旋转矩阵，因为此时旋转仅沿两个坐标轴进行，这被称为旋转向量的特殊形式，可以简化为：
\[ R = \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos(2\theta) & -\sin(2\theta) \\
0 & \sin(2\theta) & \cos(2\theta)
\end{bmatrix}
\]
其中 \( \theta \) 是围绕一个轴的旋转角度，可以根据 \( x \) 或 \( y \) 来计算。

3. 最后，你可以使用`rotMat.at<double>(i, j)`来访问和修改矩阵元素。

double angle = atan2(y, x) * 2; // 如果w=0时，直接用这个值替代
if (w != 0) {
    double sy = std::sin(angle);
    double cx = 1 - 2 * (y*y + w*w);
    double cy = 2 * (xy + w);
    double sz = 2 * (xw - y);

    rotMat.at<double>(0, 0) = cx;
    rotMat.at<double>(0, 1) = cy;
    rotMat.at<double>(0, 2) = sz;
    // ...继续填充其余矩阵元素
} else {
    // 使用简单公式计算绕原点旋转的情况
    rotMat.at<double>(1, 1) = cos(2*angle);
    rotMat.at<double>(1, 2) = sin(2*angle);
    rotMat.at<double>(0, 1) = -sin(2*angle);
    rotMat.at<double>(0, 2) = cos(2*angle); // 等于rotMat.at<double>(1, 0)
}