Multiple IMU predict calls on each IMU reading #14
Description
Tracing the code of estimator_node.cpp you will see that:
- The
predictfunction 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());
}