Multiple IMU predict calls on each IMU reading

[TanPinDa]

Tracing the code of estimator_node.cpp you will see that:

1. The predict function changes global variables. Consider the snippet bellow

   
   double latest_time;
   Eigen::Vector3d tmp_P;
   Eigen::Quaterniond tmp_Q;
   Eigen::Vector3d tmp_V;
   Eigen::Vector3d tmp_Ba;
   Eigen::Vector3d tmp_Bg;
   Eigen::Vector3d acc_0;
   Eigen::Vector3d gyr_0;
   bool init_feature = 0;
   bool init_imu = 1;
   double last_imu_t = 0;

void predict(const sensor_msgs::ImuConstPtr &imu_msg) { double t = imu_msg->header.stamp.toSec(); if (init_imu) { latest_time = t; init_imu = 0; return; } double dt = t - latest_time; latest_time = t;

double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};

double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};

Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;

Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQuat(un_gyr * dt);

Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;

Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;

acc_0 = linear_acceleration;
gyr_0 = angular_velocity;

}

We can consider `gyro_0` as *previous_gyro*. Therefore, this function being called in the proper sequence is very crucial.

However,

This function is called from two places:

Firstly from this callback, that also adds the message to a buffer. Take note the values are **NOT REMOVED** from the buffer even after they are passed into the `predict` function.

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) { if (imu_msg->header.stamp.toSec() <= last_imu_t) { ROS_WARN("imu message in disorder!"); return; } last_imu_t = imu_msg->header.stamp.toSec();

m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one();

last_imu_t = imu_msg->header.stamp.toSec();

{
    std::lock_guard<std::mutex> lg(m_state);
    predict(imu_msg);
    std_msgs::Header header = imu_msg->header;
    header.frame_id = "world";
    if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
        pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
}

}

It is in this other function called update that the buffer is cleared and used on the function `predict` again.

void update() { TicToc t_predict; latest_time = current_time; tmp_P = estimator.Ps[WINDOW_SIZE]; tmp_Q = estimator.Rs[WINDOW_SIZE]; tmp_V = estimator.Vs[WINDOW_SIZE]; tmp_Ba = estimator.Bas[WINDOW_SIZE]; tmp_Bg = estimator.Bgs[WINDOW_SIZE]; acc_0 = estimator.acc_0; gyr_0 = estimator.gyr_0;

queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
    predict(tmp_imu_buf.front());

}

[TanPinDa]

I have edited the code to print the time stamps of the imu value when the predict function is called. I also create a second copy of the predict function, just so that it prints something different for the call at imu_callback and update

The results are as shown below

[WARN] [1716718323.496013200]: CALLING predict from ROS calback 2.93 [ WARN] [1716718323.496034931]: CALLING predict from ROS calback 2.935 [ WARN] [1716718323.496055721]: CALLING predict from ROS calback 2.94 [ WARN] [1716718323.496076861]: CALLING predict from ROS calback 2.945 [ WARN] [1716718323.540002460]: UPDATE CALL [ WARN] [1716718323.540046010]: CALLING predict from update calback 2.905 [ WARN] [1716718323.540059515]: CALLING predict from update calback 2.91 [ WARN] [1716718323.540073314]: CALLING predict from update calback 2.915 [ WARN] [1716718323.540086171]: CALLING predict from update calback 2.92 [ WARN] [1716718323.540099840]: CALLING predict from update calback 2.925 [ WARN] [1716718323.540111740]: CALLING predict from update calback 2.93 [ WARN] [1716718323.540127922]: CALLING predict from update calback 2.935 [ WARN] [1716718323.540137040]: CALLING predict from update calback 2.94 [ WARN] [1716718323.540151216]: CALLING predict from update calback 2.945 [ WARN] [1716718323.546263171]: CALLING predict from ROS calback 2.95 [ WARN] [1716718323.546300981]: CALLING predict from ROS calback 2.955 [ WARN] [1716718323.546316322]: CALLING predict from ROS calback 2.96 [ WARN] [1716718323.546570893]: CALLING predict from ROS calback 2.965 [ WARN] [1716718323.546591625]: CALLING predict from ROS calback 2.97 [ WARN] [1716718323.546604155]: CALLING predict from ROS calback 2.975 [ WARN] [1716718323.546616013]: CALLING predict from ROS calback 2.98 [ WARN] [1716718323.546627410]: CALLING predict from ROS calback 2.985 [ WARN] [1716718323.546881632]: CALLING predict from ROS calback 2.99 [ WARN] [1716718323.546950417]: CALLING predict from ROS calback 2.995 [ WARN] [1716718323.592976135]: CALLING predict from ROS calback 3 [ WARN] [1716718323.593188587]: CALLING predict from ROS calback 3.005 [ WARN] [1716718323.593420445]: CALLING predict from ROS calback 3.01 [ WARN] [1716718323.593651611]: CALLING predict from ROS calback 3.015 [ WARN] [1716718323.593801552]: CALLING predict from ROS calback 3.02 [ WARN] [1716718323.593862374]: CALLING predict from ROS calback 3.025 [ WARN] [1716718323.593909296]: CALLING predict from ROS calback 3.03 [ WARN] [1716718323.593948120]: CALLING predict from ROS calback 3.035 [ WARN] [1716718323.593987888]: CALLING predict from ROS calback 3.04 [ WARN] [1716718323.594025925]: CALLING predict from ROS calback 3.045 [ WARN] [1716718323.638935134]: UPDATE CALL [ WARN] [1716718323.638971282]: CALLING predict from update calback 3.005 [ WARN] [1716718323.638982037]: CALLING predict from update calback 3.01 [ WARN] [1716718323.638992694]: CALLING predict from update calback 3.015 [ WARN] [1716718323.639003753]: CALLING predict from update calback 3.02 [ WARN] [1716718323.639012922]: CALLING predict from update calback 3.025 [ WARN] [1716718323.639022486]: CALLING predict from update calback 3.03 [ WARN] [1716718323.639031717]: CALLING predict from update calback 3.035 [ WARN] [1716718323.639041449]: CALLING predict from update calback 3.04 [ WARN] [1716718323.639051292]: CALLING predict from update calback 3.045 [ WARN] [1716718323.645855944]: CALLING predict from ROS calback 3.05 [ WARN] [1716718323.645896127]: CALLING predict from ROS calback 3.055 [ WARN] [1716718323.645924358]: CALLING predict from ROS calback 3.06 [ WARN] [1716718323.645942656]: CALLING predict from ROS calback 3.065 [ WARN] [1716718323.645959485]: CALLING predict from ROS calback 3.07 [ WARN] [1716718323.646059507]: CALLING predict from ROS calback 3.075 [ WARN] [1716718323.646099741]: CALLING predict from ROS calback 3.08

This is clearly an issue. I will push a branch so the edited code can be used and palyed around with https://github.com/TanPinDa/VINS-Mono/tree/14-multiple-imu-predict-calls-on-each-imu-reading