Question about the function "double2vector( )"

[1216621137]

Thank you for your great work. I have some questions about the function "double2vector( )" in which the state does not compensate offset of the state of the first keyframe, i.e. Ps[0] and Rs[0]. However, in VINS-Fusion, this offset is considered which is equal to fixing the first keyframe. If this modification is ok when the GPS is lost?

GVINS: `void Estimator::double2vector() { for (int i = 0; i <= WINDOW_SIZE; i++) {

    Rs[i] = Quaterniond(para_Pose[i][6], para_Pose[i][3], 
                        para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();

    Ps[i] = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]);

    Vs[i] = Vector3d(para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2]);

    Bas[i] = Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5]);

    Bgs[i] = Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8]);
}

for (int i = 0; i < NUM_OF_CAM; i++)
{
    tic[i] = Vector3d(para_Ex_Pose[i][0], para_Ex_Pose[i][1], para_Ex_Pose[i][2]);
    ric[i] = Quaterniond(para_Ex_Pose[i][6], para_Ex_Pose[i][3],
                         para_Ex_Pose[i][4], para_Ex_Pose[i][5]).normalized().toRotationMatrix();
}

VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < f_manager.getFeatureCount(); i++)
    dep(i) = para_Feature[i][0];
f_manager.setDepth(dep);
if (ESTIMATE_TD)
    td = para_Td[0][0];

if (gnss_ready)
{
    yaw_enu_local = para_yaw_enu_local[0];
    for (uint32_t k = 0; k < 3; ++k)
        anc_ecef(k) = para_anc_ecef[k];
    R_ecef_enu = ecef2rotation(anc_ecef);
}

}`

VINS: void Estimator::double2vector() { Vector3d origin_R0 = Utility::R2ypr(Rs[0]); Vector3d origin_P0 = Ps[0];

if (failure_occur)
{
    origin_R0 = Utility::R2ypr(last_R0);
    origin_P0 = last_P0;
    failure_occur = 0;
}

if(USE_IMU)
{
    Vector3d origin_R00 = Utility::R2ypr(Quaterniond(para_Pose[0][6],
                                                      para_Pose[0][3],
                                                      para_Pose[0][4],
                                                      para_Pose[0][5]).toRotationMatrix());
    double y_diff = origin_R0.x() - origin_R00.x();
    //TODO
    Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
    if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)
    {
        ROS_DEBUG("euler singular point!");
        rot_diff = Rs[0] * Quaterniond(para_Pose[0][6],
                                       para_Pose[0][3],
                                       para_Pose[0][4],
                                       para_Pose[0][5]).toRotationMatrix().transpose();
    }

    // rot_diff.setIdentity();
    // origin_P0.setZero();
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {

        Rs[i] = rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();

        Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],
                                para_Pose[i][1] - para_Pose[0][1],
                                para_Pose[i][2] - para_Pose[0][2]) + origin_P0;

            Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0],
                                        para_SpeedBias[i][1],
                                        para_SpeedBias[i][2]);

            Bas[i] = Vector3d(para_SpeedBias[i][3],
                              para_SpeedBias[i][4],
                              para_SpeedBias[i][5]);

            Bgs[i] = Vector3d(para_SpeedBias[i][6],
                              para_SpeedBias[i][7],
                              para_SpeedBias[i][8]);

    }
}

[ooww0123TW]

I think two codes are conducting the same thing if origin_P0 has same vector3d value with para_Pose[0][0~2]