EIS系统中Horizon Lock功能实现

Posted on 2025-03-05 In Tech , Vision Views: Disqus:

在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\)（到世界坐标系）。

代码实现

#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) {
        // 步骤1：计算世界坐标系中的视图方向
        Eigen::Vector3d view_dir_imu(0, -1, 0);
        Eigen::Vector3d d = q * view_dir_imu;

        // 步骤2：计算期望的上方向（世界坐标系）
        Eigen::Vector3d world_up(0, 0, 1);
        Eigen::Vector3d up_desired = world_up - world_up.dot(d) * d;

        // 步骤3：归一化
        up_desired.normalize();

        // 步骤4：转换到IMU坐标系
        Eigen::Vector3d up_desired_imu = q.inverse() * up_desired;

        // 步骤5：计算旋转角度θ
        double theta = std::atan2(up_desired_imu.x(), up_desired_imu.z());

        // 步骤6：构造滚转补偿四元数
        Eigen::AngleAxisd r(theta, Eigen::Vector3d(0, 1, 0));
        Eigen::Quaterniond r_quat(r);

        // 步骤7：生成新的四元数
        Eigen::Quaterniond q_lock = q * r_quat;

        quats_locked.push_back(q_lock);
    }

    return quats_locked;
}