构建相机预积分残差块

预积分约束定义在类 ProjectionFactor中;

vins_estimator\src\estimator.cpp\void Estimator::optimization()

//O_残差块2: 视觉重投影约束
// 遍历所有特征点
for (auto &it_per_id : f_manager.feature)
{
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    // 进行有效特征点的检查
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
        continue;

    ++feature_index;

    // 第一个观测到这个特征点的帧的idx
    int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

    // 特征点在第一个帧下的归一化相机坐标系
    Vector3d pts_i = it_per_id.feature_per_frame[0].point;

    // 遍历这个特征点所有的KF
    for (auto &it_per_frame : it_per_id.feature_per_frame)
    {
        imu_j++;
        if (imu_i == imu_j) // 自己与自己构不成重投影误差
        {
            continue;
        }
        // 取出另一帧的归一化相机坐标系
        Vector3d pts_j = it_per_frame.point;

        // 带有时间延迟估计的雅可比的计算
        if (ESTIMATE_TD)
        {
            //....略
        }
        else // 不带时间延迟估计的雅可比计算
        {
            // 构造函数就是同一个特征点在不同帧的观测
            ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
            // 约束的变量是该特征的的第一个观测帧以及其他一个观测帧,加上外参和特征点逆深度
            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
        }
        f_m_cnt++;
    }
}

同样地,由于VINS在后端优化中使用的是解析求导, 在这种情况下,需要定义残差的计算方式,又需要定义残差对各个参数块的雅可比。为此,如果您在编译时知道参数和残差的大小,请定义 CostFunctionSizedCostFunction 的子类。另外,需要重载Evaluate 函数,用来计算残差和雅可比。

这些在自定义的CostFuction函数中,对应代码为:vins_estimator\src\factor\projection_factor.h\class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>,具体实现位于:vins_estimator\src\factor\projection_factor.cpp

残差构建

待优化变量

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    //G_ 待优化变量
    // 第 i 帧的位姿
    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]);

    // 第 j 帧的位姿
    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]);

    // 相机和IMU之间的外参
    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];
}

构建残差

  • VINS中的重投影误差

image-20220726151556969

视觉的帧间约束为重投影误差,如上图所示,对于路标点 $l$ ,我们使用相机可以观测到它在第 $i$ 帧(关键帧)中的像素坐标,即在第 $i$ 帧中的 $\overline{\mathcal{P} }^{c_i}_l$(图中红色点),然后根据光流法可以求得其在第 $j$ 帧中的 $\hat{\overline{\mathcal{P} } }^{c_j}_l$(图中红色点);

而根据坐标变换,我们同样可以利用我们所求的外参计算第 $i$ 帧中的点,经过坐标变换,在第 $j$ 帧中的理论值 $\mathcal{P}^{c_j}_l$; 由于收到噪声的影响,两者是不可能重合的,两者之间的差值就是我们需要优化的残差,进而达到优化外参以及3D点等优化变量的目的。

$$ \begin{align*} \mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X})&=[\mathbf{b}_1,\mathbf{b}_2]^T\cdot(\hat{\overline{\mathcal{P} } }^{c_j}_l-\frac{\mathcal{P}^{c_j}_l}{\parallel\mathcal{P}^{c_j}_l \parallel^2})\\ \end{align*}\tag{3} $$

其中:

$$ \begin{align*} \hat{\overline{\mathcal{P} } }^{c_j}_l&=\pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_j}_l\\\hat{v}^{c_j}_l \end{bmatrix}\right)\\ \mathcal{P}^{c_j}_l &=\mathbf{R}^c_b\left\{\mathbf{R}^{b_j}_w\left[\mathbf{R}^{w}_{b_i}\left(\mathbf{R}^{b}_{c}\frac{1}{\lambda_l}\overline{\mathcal{P} }^{c_i}_l+\mathbf{p}^{b}_{c}\right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j}\right]-\mathbf{p}^{b}_{c}\right\}\\ \overline{\mathcal{P} }^{c_i}_l&= \pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_i}_l\\\hat{v}^{c_i}_l \end{bmatrix} \right) \end{align*} $$

详细查看:VINS-Mono-5-后端非线性优化-视觉约束

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

//G_ 构建残差
// 地图在第 i 帧相机坐标系下的坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
// 转成第 i 帧 imu坐标系
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
// 转成世界坐标系
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
// 转到第 j 帧imu坐标系
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
// 转成第 j 帧相机坐标系
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> residual(residuals);


double dep_j = pts_camera_j.z(); // 第 j 帧下的逆深度
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); // 重投影误差
residual = sqrt_info * residual; // 误差乘以信息矩阵

代码中定义的#define UNIT_SPHERE_ERROR 球面模型的宏,作者并没有使用球面模型,而是使用普通的归一化相机平面的建模方式,因此省略了该形式下的代码。

另外,对于视觉重投影的协方差,作者认为都是一致的,它的协方差sqrt_info定义在如下位置:

vins_estimator\src\estimator.cpp\void Estimator::setParameter()

void Estimator::setParameter()
{
 for (int i = 0; i < NUM_OF_CAM; i++)
 {
     tic[i] = TIC[i];
     ric[i] = RIC[i];
 }
 f_manager.setRic(ric);
 // 这里可以看到虚拟相机的用法
 // 不带时延的视觉重投影的协方差
 ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
 // 带时延的视觉重投影的协方差
 ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
 td = TD;
}

定义雅可比的计算

由于雅可比的计算在代码中使用的是普通的归一化相机坐标模型,即参差定义为:

因此,其雅可比的计算,作者采用的是链式求导,分两步求取雅可比,这样做是为了简化计算:

$$ \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{X} } =\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l }* \frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} } $$

a.求取 $\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l }$

$$ \begin{align*} \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l } =\begin{bmatrix} \frac{1}{z_{c_j} }&0&-\frac{x}{z_{c_j}^2 }\\ 0&\frac{1}{z_{c_j} }&-\frac{y}{z_{c_j}^2 } \end{bmatrix}_{2\times3} \end{align*} $$

对应代码:

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

if (jacobians)
{
    // 转成Eigen
    Eigen::Matrix3d Ri = Qi.toRotationMatrix();
    Eigen::Matrix3d Rj = Qj.toRotationMatrix();
    Eigen::Matrix3d ric = qic.toRotationMatrix();
    Eigen::Matrix<double, 2, 3> reduce(2, 3);
    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);
    reduce = sqrt_info * reduce;
...
    // 以下为雅可比的定义与计算
}

b.求取 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} }$

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} } &= \begin{bmatrix} \mathbf{J}[0]&\mathbf{J}[1]\\ \mathbf{J}[2]&\mathbf{J}[3] \end{bmatrix}\\ &=\begin{bmatrix} \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} } \right]& \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} } \right]\\ \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c } \right]& \frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l } \end{bmatrix} \end{align*} $$

  • $\mathbf{J}[0]$

$$ \mathbf{J}[0]^{3\times7}= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} } \right] = \begin{bmatrix} \mathbf{R}^c_b\mathbf{R}^{b_j}_w & -\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \left(\mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l}+\mathbf{p}^{b}_{c} \right)^{\land} \end{bmatrix} $$

对应代码:

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

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();
}
  • $\mathbf{J}[1]$

    $$ \mathbf{J}[1]^{3\times7}= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} } \right] = \begin{bmatrix} -\mathbf{R}^c_b\mathbf{R}^{b_j}_w & \mathbf{R}^c_b \left\{ \mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_j}_l}{\lambda_l}+\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] \right\}^{\land} \end{bmatrix} $$

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

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();
}
  • $\mathbf{J}[2]$

$$ \begin{align*} \mathbf{J}[2]^{3\times7}&= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c } \right]\\ &=\begin{bmatrix} \mathbf{R}^c_b(\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}-I_{3\times3}) \\ -\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\left(\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left(\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left\{ \mathbf{R}^c_b \left[ \mathbf{R}^{b_j}_w \left( \mathbf{R}^w_{b_i}\mathbf{p}^{b}_{c}+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right)-\mathbf{p}^{b}_{c} \right] \right\}^{\land} \end{bmatrix}^T \end{align*} $$

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

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();
}
  • $\mathbf{J}[3]$

$$ \mathbf{J}[3]^{3\times1}= \frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l } =-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l^2} $$

vins_estimator\src\factor\projection_factor.cpp\bool ProjectionFactor::Evaluate(...)

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
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/85.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 27 日
赏杯咖啡