vins estimator ProjectionFactor (Td) factor

xiaoma_bk

已于 2022-11-03 10:21:31 修改

阅读量812

点赞数

分类专栏： # vins 文章标签：计算机视觉人工智能

于 2022-05-09 19:09:35 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/124636050

版权

vins 专栏收录该内容

15 篇文章 3 订阅

订阅专栏

本文详细介绍了视觉SLAM系统中处理卷帘快门相机时间差和IMU数据同步的方法。首先，通过逆深度参数化转换特征点坐标，并考虑时间差和速度影响。接着，定义了带时间差的重投影误差函数，包括针孔相机和鱼眼相机的误差模型。为了优化，计算了残差对优化变量（如IMU位姿、外参、特征点深度和时间差）的雅可比矩阵。最后，提供了实现这些计算的代码片段，展示了如何在实际系统中应用这些理论。

摘要由CSDN通过智能技术生成

简介

逆深度参数化： $\begin{bmatrix} x \ y \ z \end{bmatrix} = \frac{1}{\lambda}\begin{bmatrix} u \ v \ 1 \end{bmatrix}$
第i 帧中第一次被观测到转化到归一化平面： $\begin{bmatrix} u_{c_{i}}\ v_{c_{i}} \ 1\end{bmatrix}$
转换到第j 帧： $\begin{bmatrix} x_{c_{j}}\ y_{c_{j}} \ z_{c_{j}} \ 1 \end{bmatrix} = T_{bc}^{-1}T_{wb_{j}}^{-1}T_{wb_{i}}T_{bc} \begin{bmatrix} \frac{1}{\lambda} u_{c_{i}} \ \frac{1}{\lambda} v_{c_{i}} \ \frac{1}{\lambda} \ 1\end{bmatrix}$
视觉残差：
- 针孔： $r_{c} = \begin{bmatrix} \frac{x_{c_{j}}}{z_{c_{j}}} - u_{c_{j}} \ \frac{y_{c_{j}}}{z_{c_{j}}} - v_{c_{j}}\end{bmatrix}$
- 鱼眼： $r_{\mathcal{C}}(\hat{z_{l}}^{c_{j}}, \mathcal{X}) = \begin{bmatrix} \overrightarrow{b_{1}} \ \overrightarrow{b_{2}} \end{bmatrix} (\frac{P_{l}^{c_{j}}}{\left | P_{l}^{c_{j}} \right |}-\bar{P_{l}}^{c_{j}})$
  - 因为视觉残差的自由度为2, 因此将视觉残差投影到正切平面上， $b_{1}, b_{2}$ 为正切平面上的任意俩个正交基。
  - 采用施密特正交化得到
求解Jacobian
- 视觉残差对重投影3D点 $f_{c_{j}}$ 求导
- $f_{c_{j}}$ 对各个优化变量的求导

重投影误差

对于同一特征点，i 时刻与 j 时刻的重投影误差
先将 i时刻特征点 camera坐标系通过外参＋i时刻全局位姿得到　特征点的全局位姿
然后将该特征点的全局位姿通过j时刻全局位姿＋外参转换到j时刻camera坐标系
转换后的两个差异之差即为重投影误差。

卷帘快门相机时间差

相加快门

快门是照相机用来控制感光片有效曝光时间的机构，Global Shutter（全局快门）与Rolling Shutter（卷帘快门）对应全局曝光和卷帘曝光模式，这两种曝光模式都是相机常见的曝光模式。
Global Shutter 特点
- 感光元件的所有像素点同时曝光一定时间，进而成像。
- 曝光时间比Rolling Shutter短，曝光整帧后输出像素数据。
- 对于高速运动的部分曝光后体现出模糊的现象。
- 一般使用CCD作为Sensor，对像素点输出的带宽的要求较高，CCD的造价也相对CMOS较高。
Rolling Shutter 特点
- 感光元件的所有像素点逐行轮流曝光一定时间，进而成像。
- 会出现“果冻现象”。
- 曝光时间比Global Shutter长，但是曝光一行输出一行。
- 对于高速运动的部分曝光后体现出弯曲的现象。
- 一般使用CMOS作为Sensor，对像素点输出的带宽的要求较低，CMOS的造价也相对较低。

本部分功能：

对imu-camera 时间戳不完全同步和 Rolling shutter 相机的支持
该部分的功能是优化 IMU 与相机硬件的固定时间差。
认为滑窗内 imu 与相机硬件的时间间隔一样。
输入的变量：
- i 时刻角点（j时刻角点一样，赋值一份）
  - 在归一化平面的坐标 pts_i
  - 角点在归一化平面的速度 velocity_i
  - 处理IMU数据时用到的时间同步误差 td_i
  - 角点在归一化平面行坐标 _row_i = _row_i - ROW / 2
每张图片以中心时间曝光作为其时间。

带Td 的优化

pts_i基于时间差及速度得到 pts_i_td
pts_j基于时间差及速度得到 pts_j_td

残差

优化变量

imu_i 的全局位姿 维度：7
imu_j 的全局位姿 维度：7
外参 维度：7
inv_dep_i 维度：1
时间差变量 para_Td[0] 维度：1

残差计算

角点i在归一化平面的row坐标
- pts_i - (td - td_i + TR / ROW * row_i) * velocity_i
- pts_camera_i = pts_i_td / inv_dep_i
通过外参转换到 imu坐标系
- pts_imu_i = qic * pts_camera_i + tic;
通过imu_i的全局位姿，将角点转换到世界坐标系
- pts_w = Qi * pts_imu_i + Pi;
将角点通过 imu_j 转换到 j时刻imu坐标系下位姿
- pts_imu_j = Qj.inverse() * (pts_w - Pj);
转换到 j 时刻相机坐标系下
- pts_camera_j = qic.inverse() * (pts_imu_j - tic);

求解其Jacobian

求解 jacobian时，即残差对优化变量求导
求导分两步，先对 pts_camera_j 求导，再对变量求导。

对 pts_camera_j 求导

残差对 pts_camera_j求导
非单位球面误差时：
$\begin{bmatrix} 1./dep_j & 0 & -{pts\_camera\_j(0) / (dep\_j * dep\_j)} \\ 0 & 1./dep_j & -{pts\_camera\_j(0) / (dep\_j * dep\_j)} \end{bmatrix}}$

由于权重(信息矩阵)因素，故 reduce = sqrt_info * reduce

    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif

对 Pose_i 求导

pts_camera_j 对 Pose_i求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_i的偏导： ${q_{ic}^{-1} Q_j^{-1}}$
pts_camera_j 对 Q_i的偏导： ${q_{ic}^{-1} Q_j^{-1} -Q_i ({pts\_imu\_i})^\wedge}$ （通过右扰动求导）

对 Pose_j 求导

pts_camera_j 对 Pose_j求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_j的偏导： ${q_{ic}^{-1} Q_j^{-1}}$
pts_camera_j 对 Q_j的偏导： ${q_{ic}^{-1} ({pts\_imu\_j})^\wedge}$ （通过右扰动求导）

对外参求导

pts_camera_j 对 Pose_{ic}求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_{ic}的偏导： ${q_{ic}^{-1} (Q_j^{-1}Q_i -I)}$
pts_camera_j 对 Q_{ic}的偏导： ${q_{ic}^{-1}({pts\_imu\_j}-t_{ic})^\wedge + q_{ic}^{-1}Q_j^{-1} Q_iq_{ic}({pts\_camera\_i}-t_{ic})^\wedge}$ （通过右扰动求导）

对特征点深度 inv_dep_i 求导

pts_camera_j 对 inv_dep_i求导，对pts_camera_j偏导乘以其值
${-q_{ic}^{-1}Q_j^{-1} Q_iq_{ic} {pts\_i\_td} / (inv\_dep\_i)^2}$

对时间差求导

对时间差求偏导时， pts_camera_j 和 pts_camera_j都有其变量，故：
pts_camera_j 对 dt 求导： ${ reduce * -q_{ic}^{-1}Q_j^{-1} Q_iq_{ic} {pts\_i\_td} * velocity\_y/ (inv\_dep\_i) }$
pts_camera_j 对 dt 求导： ${sqrt\_info *velocity\_y }$

代码实现

bool ProjectionTdFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];

    double td = parameters[4][0];

    Eigen::Vector3d pts_i_td, pts_j_td;

    pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
    pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;
    Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
#endif

    residual = sqrt_info * residual;

    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;

        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }

        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix<double, 3, 6> jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i);
        }
        if (jacobians[4])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[4]);
            jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0  +
                          sqrt_info * velocity_j.head(2);
        }
    }
    sum_t += tic_toc.toc();

    return true;
}

不带Td 优化

直接使用 pts_i 和 pts_j 求解

残差

优化变量

imu_i 的全局位姿维度：7
imu_j 的全局位姿维度：7
外参维度：7
inv_dep_i 维度：1

残差计算

pts_i 是i时刻归一化相机坐标系下的3D坐标
第i帧相机坐标系下的的逆深度
double inv_dep_i = parameters[3][0];
第i帧相机坐标系下的3D坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
第i帧IMU坐标系下的3D坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
世界坐标系下的3D坐标
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
第j帧imu坐标系下的3D坐标
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
第j帧相机坐标系下的3D坐标
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
残差固件
Eigen::MapEigen::Vector2d residual(residuals);
- 若球面相机
  residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
- 针孔相机
  double dep_j = pts_camera_j.z();
  residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();

求解Jacobian

由上述可知，二者残差几乎一致，因此Jacobian类似

代码实现

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);


    //pts_i 是i时刻归一化相机坐标系下的3D坐标
    //第i帧相机坐标系下的的逆深度
    double inv_dep_i = parameters[3][0];
    //第i帧相机坐标系下的3D坐标
    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    //第i帧IMU坐标系下的3D坐标
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    //世界坐标系下的3D坐标
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    //第j帧imu坐标系下的3D坐标
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    //第j帧相机坐标系下的3D坐标
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);


    // 残差构建
    // 根据不同的相机模型
#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else//针孔相机模型
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual;

    //reduce 表示残差residual对fci（pts_camera_j）的导数，同样根据不同的相机模型
    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else//针孔相机模型
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;


        // 残差项的Jacobian
        // 先求fci对各项的Jacobian，然后用链式法则乘起来
        // 对第i帧的位姿 pbi,qbi      2X7的矩阵 最后一项是0
        if (jacobians[0]) 
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }
        // 对第j帧的位姿 pbj,qbj
        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix<double, 3, 6> jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }
        // 对相机到IMU的外参 pbc,qbc (qic,tic)
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        // 对逆深度 \lambda (inv_dep_i)
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
#if 1
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
        }
    }
    sum_t += tic_toc.toc();

    return true;
}