VNS-Fusion代码阅读（三）

最新推荐文章于 2024-01-29 22:21:21 发布

changshen_xu

最新推荐文章于 2024-01-29 22:21:21 发布

阅读量987

点赞数 1

分类专栏： ▶ ROS 文章标签： VINS-Fusion

本文链接：https://blog.csdn.net/CSDN_XCS/article/details/90317184

版权

▶ ROS 专栏收录该内容

19 篇文章 6 订阅

订阅专栏

该部分对应解析：三、后端非线性优化 >> 3.3 IMU约束 >> 3)Jacobian （即11页）
IMUFactor是一个类，其定义在imu_factor.h中，且继承自ceres::SizedCostFunction<15, 7, 9, 7, 9>
其中，将基类（即ceres::SizedCostFunction）中定义的虚函数Evaluate进行了覆盖（override），（即自己又写了与基类中，成员函数名相同，形参等完全相同的成员函数）。
总体来说，这个类的框架很简单，只有一个数据成员，一个成员函数，和一个构造函数。如下：

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
{
  public:
    IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration) { }
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
    }
    IntegrationBase* pre_integration;

看具体实现细节：

Evaluate的功能？
参数：两个二级指针parameters和jacobian，一个一级指针residuals

我们看到，Pi, Qi, Vi, Bai, Bgi和Pj, Qj, Vj, Baj, Bgj从parameters中读取得到。
然后，使用他们作为参数去调用之前介绍的IntegrationBase类中的evaluate()成员函数，得到残差residual（这个变量类型为Eigen::Map，暂时还不太清晰，要知道pre_integration->evaluate的返回值是一个Eigen::Matrix类型）
残差residual与信息矩阵sqrt_info相乘的含义，见备注1。
接下来，开始计算Jacobian
$J[0]^{15\times 7}=[\frac{\partial r_B}{\partial p^{w}_{b_k}}, \frac{\partial r_B}{\partial q^{w}_{b_k}}]=$
其中各个矩阵块： $-R^{b_k}_{w}$
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
$[R^{b_k}_w(p^w_{b_{k+1}}-p^w_{b_{k}}-v^w_{b_{k}}\Delta t_k+\frac{1}{2}g^w\Delta t^2_k)]^\wedge$
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
$-\mathcal{L}[{q^w_{b_{k+1}}}^{-1}\otimes {q^w_{b_{k}}}]\mathcal{R} \begin{bmatrix} b_k\\ \gamma_{b_{k+1}} \end{bmatrix}$ 代码里Utility::是什么？
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
$[R^{b_k}_w(v^w_{b_{k+1}}-v^w_{b_{k}}+g^w\Delta t_k)]^\wedge$
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
注意，此处同样有一个处理jacobian_pose_i = sqrt_info * jacobian_pose_i;

$J[1]^{15\times 9}=[\frac{\partial r_B}{\partial v^{w}_{b_k}}, \frac{\partial r_B}{\partial b_{a_k}}, \frac{\partial r_B}{\partial b_{w_k}}]=$
类似，此处暂时不再介绍（且后两项偏导已经计算过，直接拿来用）

$J[2]^{15\times 7}=[\frac{\partial r_B}{\partial p^{w}_{b_{k+1}}}, \frac{\partial r_B}{\partial q^{w}_{b_{k+1}}}]=$

$J[1]^{15\times 9}=[\frac{\partial r_B}{\partial v^{w}_{b_{k+1}}}, \frac{\partial r_B}{\partial b_{a_{k+1}}}, \frac{\partial r_B}{\partial b_{w_{k+1}}}]=$

OK，至此IMU的约束告一段落。

【注1】：在解析中的解释是：真正的优化项其实是Mahalanobis距离 $d=r^TP^{-1}r$ ，P是协方差；同时又因为Ceres只接受最小二乘优化，也就是 $min(e^Te)$ 。因此，将 $P^{-1}$ 做了LLT分解（即 $LL^T=P^{-1}$ ），则有 $d=r^TLL^Tr=(L^Tr)^T(L^Tr)$ ，并将 $r'=L^Tr$ 取做新的优化误差，就可以使用Ceres求解了。

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {

        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 Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

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

        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
        residual = sqrt_info * residual;

        if (jacobians)
        {
            double sum_dt = pre_integration->sum_dt;
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);

            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
                //std::cout << pre_integration->jacobian << std::endl;
///                ROS_BREAK();
            }

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

                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
                jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                    //std::cout << sqrt_info << std::endl;
                    //ROS_BREAK();
                }
            }
            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();
                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;

#if 0
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
                //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif

                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

                //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }
            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_j = sqrt_info * jacobian_pose_j;

                //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
            }
            if (jacobians[3])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();

                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

                //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
            }
        }

        return true;
    }

changshen_xu

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
4
评论
VNS-Fusion代码阅读（三）

该部分对应解析：三、后端非线性优化 >> 3.3 IMU约束 >> 3)Jacobian （即11页）IMUFactor是一个类，其定义在imu_factor.h中，且继承自ceres::SizedCostFunction<15, 7, 9, 7, 9>其中，将基类（即ceres::SizedCostFunction）中定义的虚函数Evaluate进行了覆盖...
复制链接

扫一扫