在EIS（电子图像稳定）系统中，实现Horizon
Lock功能的目的是确保相机的水平方向保持稳定，即在世界坐标系中，相机的视图水平线与地平线保持一致。根据题目定义：
世界坐标系：Z轴向上。
IMU坐标系：X轴指向设备左边，Y轴指向内（设备背面），Z轴指向设备上边，视图方向（view
direction）为Y轴负方向。
输入数据：有一系列从IMU坐标系到世界坐标系的四元数数组std::vector<Eigen::Quaterniond> quats。Horizon
Lock的核心是补偿相机的滚转（roll）角度，使得相机的上方向（IMU坐标系的Z轴）在世界坐标系中尽可能与世界Z轴对齐，同时保持视图方向不变。下面是实现的具体步骤：
实现思路
- 理解四元数作用 每个四元数
q表示从IMU坐标系到世界坐标系的旋转。对于IMU坐标系中的向量 \(\mathbf{v}_{\text{imu}}\)，可以通过 \(\mathbf{v}_{\text{world}} = q \cdot
\mathbf{v}_{\text{imu}}\)转换到世界坐标系（使用Eigen库的四元数运算规则）。
- 确定关键方向
视图方向：在IMU坐标系中，视图方向是Y轴负方向，即 \(\begin{pmatrix} 0 \\ -1 \\ 0
\end{pmatrix}\)。在世界坐标系中，视图方向为 \(\mathbf{d} = q \cdot \begin{pmatrix} 0 \\ -1 \\ 0
\end{pmatrix}\)。 上方向：在IMU坐标系中，上方向是Z轴，即 \(\begin{pmatrix} 0 \\ 0 \\ 1
\end{pmatrix}\)。在世界坐标系中，当前上方向为 \(q \cdot \begin{pmatrix} 0 \\ 0 \\ 1
\end{pmatrix}\)。
- Horizon Lock目标 保持视图方向 \(\mathbf{d}\)不变，同时调整旋转，使得相机的上方向在世界坐标系中与世界Z轴
\(\begin{pmatrix} 0 \\ 0 \\ 1
\end{pmatrix}\)的投影对齐（在与 \(\mathbf{d}\)垂直的平面内）。
- 调整方法 通过在IMU坐标系中围绕Y轴（视图方向的轴）施加一个额外的旋转
r，生成新的四元数 \(q_{\text{lock}} = q \cdot
r\)，使得：视图方向保持为 \(\mathbf{d}\)。上方向尽可能与世界Z轴对齐。
具体实现步骤
对于四元数数组中的每个四元数 q，按以下步骤处理： ### 计算视图方向
在IMU坐标系中，视图方向为 \(\begin{pmatrix} 0
\\ -1 \\ 0 \end{pmatrix}\)。使用四元数q将其转换到世界坐标系：
\[\mathbf{d} = q \cdot \begin{pmatrix} 0
\\ -1 \\ 0 \end{pmatrix}\]
这里 \(\mathbf{d}\)是世界坐标系中的视图方向向量。
计算期望的上方向\(\mathbf{up}_{\text{desired}}\)
在实现Horizon
Lock功能（如在电子图像稳定系统中）时，我们需要计算一个期望的上方向\(\mathbf{up}_{\text{desired}}\)，以确保相机的水平方向在世界坐标系中与地平线保持一致，同时不改变相机的视图方向\(\mathbf{d}\)。
我们希望计算的期望上方向\(\mathbf{up}_{\text{desired}}\)满足以下两个条件：
- \(\mathbf{up}_{\text{desired}}\)是世界坐标系中，相机的上方向应该指向的方向，尽可能与世界Z轴\(\begin{pmatrix} 0 \\ 0 \\ 1
\end{pmatrix}\)对齐。
- 为了保持视图方向不变，\(\mathbf{up}_{\text{desired}}\)必须与视图方向\(\mathbf{d}\)垂直。
其中：
- 视图方向\(\mathbf{d}\)是相机在世界坐标系中指向的方向（通常为相机光轴的方向）。
- 世界Z轴\(\mathbf{w} = \begin{pmatrix} 0 \\
0 \\ 1 \end{pmatrix}\)表示世界坐标系的上方向。
为了实现Horizon
Lock，我们需要调整相机的滚动（roll）角度，使得相机的上方向与\(\mathbf{up}_{\text{desired}}\)对齐，同时保持\(\mathbf{d}\)不变。
步骤 1：计算世界Z轴在视图方向\(\mathbf{d}\)上的投影
首先，我们需要将世界Z轴\(\mathbf{w}\)分解为两个分量：
- 一个分量是\(\mathbf{w}\)在\(\mathbf{d}\)方向上的投影（平行于\(\mathbf{d}\)）。
- 另一个分量是\(\mathbf{w}\)在与\(\mathbf{d}\)垂直的平面上的分量（垂直于\(\mathbf{d}\)）。
\(\mathbf{w}\)在\(\mathbf{d}\)上的投影公式为：
\[\text{proj}_{\mathbf{d}} \mathbf{w} =
\left( \mathbf{w} \cdot \mathbf{d} \right) \mathbf{d}\]
其中：
- \(\mathbf{w} \cdot
\mathbf{d}\)是\(\mathbf{w}\)和\(\mathbf{d}\)的点积，计算\(\mathbf{w}\)在\(\mathbf{d}\)方向上的分量大小。
- \(\mathbf{d}\)是单位向量（如果不是，需要先归一化）。
这个投影分量\(\text{proj}_{\mathbf{d}}
\mathbf{w}\)表示\(\mathbf{w}\)中与\(\mathbf{d}\)平行的部分。
步骤 2：计算世界Z轴在与\(\mathbf{d}\)垂直的平面上的分量
为了得到与\(\mathbf{d}\)垂直的分量，我们从\(\mathbf{w}\)中减去其在\(\mathbf{d}\)上的投影：
\[\mathbf{up}_{\text{desired}} =
\mathbf{w} - \text{proj}_{\mathbf{d}} \mathbf{w}\]
代入投影公式：
\[\mathbf{up}_{\text{desired}} =
\mathbf{w} - \left( \mathbf{w} \cdot \mathbf{d} \right)
\mathbf{d}\]
这个\(\mathbf{up}_{\text{desired}}\)就是\(\mathbf{w}\)在与\(\mathbf{d}\)垂直的平面上的分量。
步骤 3：归一化\(\mathbf{up}_{\text{desired}}\)
\[\mathbf{up}_{\text{desired}} =
\frac{\mathbf{up}_{\text{desired}}}{\|\mathbf{up}_{\text{desired}}\|}\]
转换到IMU坐标系
将 \(\mathbf{up}_{\text{desired}}\)从世界坐标系转换回IMU坐标系：
\[\mathbf{up}_{\text{desired}}^{\text{imu}} =
q^{-1} \cdot \mathbf{up}_{\text{desired}}\]
由于 \(\mathbf{up}_{\text{desired}}\)与 \(\mathbf{d}\)垂直，且 \(q^{-1} \cdot \mathbf{d} = \begin{pmatrix} 0 \\ -1
\\ 0 \end{pmatrix}\)，因此 \(\mathbf{up}_{\text{desired}}^{\text{imu}}\)的Y分量为零，即形如
\(\begin{pmatrix} a \\ 0 \\ b
\end{pmatrix}\)。
计算旋转角度
现在，我们需要在IMU坐标系中找到一个旋转角度\(\theta\)，使得IMU坐标系中的当前上方向 \(\mathbf{up}_{\text{current}}^{\text{imu}} =
\begin{pmatrix} 0 \\ 0 \\ 1
\end{pmatrix}\)（即Z轴）通过围绕Y轴旋转\(\theta\)后，与\(\mathbf{up}_{\text{desired}}^{\text{imu}}\)对齐。
由于旋转是围绕Y轴进行的，我们可以将问题简化到XZ平面上的二维旋转问题。以下是详细的推导过程：
- 分析旋转过程：
旋转轴是Y轴（视图方向），因此旋转只影响X和Z分量，Y分量保持不变。
在XZ平面上，当前上方向\(\mathbf{up}_{\text{current}}^{\text{imu}}\)的投影是\(\begin{pmatrix} 0 \\ 1
\end{pmatrix}\)（即Z轴）。
期望上方向\(\mathbf{up}_{\text{desired}}^{\text{imu}}\)的投影是\(\begin{pmatrix} a \\ b
\end{pmatrix}\)。
我们需要找到一个角度\(\theta\)，使得从\(\begin{pmatrix} 0 \\ 1
\end{pmatrix}\)旋转到\(\begin{pmatrix}
a \\ b \end{pmatrix}\)，方向遵循右手定则。
- 使用\(\text{atan2}\)函数计算角度：
在二维平面上（这里是XZ平面），从向量\(\mathbf{v}_1 = \begin{pmatrix} x_1 \\ z_1
\end{pmatrix}\)到向量\(\mathbf{v}_2 =
\begin{pmatrix} x_2 \\ z_2 \end{pmatrix}\)的旋转角度\(\theta\)可以通过以下公式计算：
\[\theta = \text{atan2}(x_2, z_2) -
\text{atan2}(x_1, z_1)\]
角度 \(\theta_1 = \text{atan2}(y_1,
x_1)\) 是 \(\mathbf{v}_1\)相对于X轴正方向的角度。 角度
\(\theta_2 = \text{atan2}(y_2, x_2)\)
是\(\mathbf{v}_2\)相对于X轴正方向的角度。
两个向量之间的有向角度就是从\(\mathbf{v}_1\)到 \(\mathbf{v}_2\)的夹角，可以表示为：\(\text{angle} = \theta_2 - \theta_1\)
在 XZ 平面中，我们希望计算从正 Z
轴到向量的逆时针旋转角度。因此我们使用\(\text{atan2}(x, z)\)。
在本问题中：
\(\mathbf{v}_1 = \begin{pmatrix} 0 \\ 1
\end{pmatrix}\)，因此\(\text{atan2}(x_1, z_1) = \text{atan2}(0, 1) =
0\)。 \(\mathbf{v}_2 = \begin{pmatrix}
a \\ b \end{pmatrix}\)，因此\(\text{atan2}(x_2, z_2) = \text{atan2}(a,
b)\)。
代入公式，旋转角度\(\theta\)为：
\[\theta = \text{atan2}(a, b) - 0 =
\text{atan2}(a, b)\]
其中：
\(a =
\mathbf{up}_{\text{desired}}^{\text{imu}}.x\)（期望上方向的X分量）。
\(b =
\mathbf{up}_{\text{desired}}^{\text{imu}}.z\)（期望上方向的Z分量）。
\(\text{atan2}(a,
b)\)返回的角度\(\theta\)是从正Z轴到向量\(\begin{pmatrix} a \\ b
\end{pmatrix}\)的旋转角度，范围在\([-
\pi, \pi]\)内。
- 旋转方向的解释： 如果\(\theta >
0\)，表示逆时针旋转（从Z轴向X轴正方向）。 如果\(\theta < 0\)，表示顺时针旋转。
构造旋转四元数：
\[r = \text{Eigen::AngleAxisd}(\theta,
\text{Eigen::Vector3d}(0, 1, 0))\]
生成新的四元数
将补偿旋转 \(r\)应用于原始四元数
\(q\)：
\[q_{\text{lock}} = q \cdot r\]
注意：四元数相乘顺序表示先应用 \(r\)（局部旋转），再应用 \(q\)（到世界坐标系）。
代码实现
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
| #include <Eigen/Geometry>
#include <vector>
std::vector<Eigen::Quaterniond> processHorizonLock(const std::vector<Eigen::Quaterniond>& quats) {
std::vector<Eigen::Quaterniond> quats_locked;
quats_locked.reserve(quats.size());
for (const auto& q : quats) {
Eigen::Vector3d view_dir_imu(0, -1, 0);
Eigen::Vector3d d = q * view_dir_imu;
Eigen::Vector3d world_up(0, 0, 1);
Eigen::Vector3d up_desired = world_up - world_up.dot(d) * d;
up_desired.normalize();
Eigen::Vector3d up_desired_imu = q.inverse() * up_desired;
double theta = std::atan2(up_desired_imu.x(), up_desired_imu.z());
Eigen::AngleAxisd r(theta, Eigen::Vector3d(0, 1, 0));
Eigen::Quaterniond r_quat(r);
Eigen::Quaterniond q_lock = q * r_quat;
quats_locked.push_back(q_lock);
}
return quats_locked;
}
|