hku-mars / r2live

R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping package
GNU General Public License v2.0
730 stars 194 forks source link

can not understand the formula ”diff_vins_lio_q“ #51

Closed chenxianbo closed 3 years ago

chenxianbo commented 3 years ago
 diff_vins_lio_q = eigen_q(estimator.Rs[WINDOW_SIZE].transpose() * state_aft_integration.rot_end);
 diff_vins_lio_t = state_aft_integration.pos_end - estimator.Ps[WINDOW_SIZE];
int Estimator::refine_vio_system(eigen_q q_extrinsic, vec_3 t_extrinsic)
{
    Eigen::Matrix<double, 3, 3> R_mat_e = q_extrinsic.toRotationMatrix();
    for (int win_idx = 0; win_idx <= WINDOW_SIZE; win_idx++)
    {
        Rs[win_idx] = Rs[win_idx] * R_mat_e;
        Ps[win_idx] = Ps[win_idx] + t_extrinsic;
        Vs[win_idx] = R_mat_e * Vs[win_idx];
    }

    for (int win_idx = 0; win_idx <= WINDOW_SIZE; win_idx++)
    {
        pre_integrations[win_idx]->delta_p = R_mat_e * pre_integrations[win_idx]->delta_p;
    }
}

what is the formula for diff_vins_lio_q?I can not undestand it‘s meaning

And I found vins often reboot when run function “refine_vio_system” with my dataset, it shows big angle for fail

stale[bot] commented 3 years ago

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.