目的：将IMU数据进行积分，得到当前的P V Q，同时计算相邻帧的预积分增量，以及预积分误差的雅可比矩阵和协方差项。
计算PVQ
回调函数imu_callback：
int main(int argc, char **argv)
{
........
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
......
........
return 0;
}在estimator_node.cpp的main函数中可以看到这个，看看这个回调函数做了什么？
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);
}
}重点是这个预测函数，它输入imu_msg，输出当前的P V Q,并用 pubLatestOdometry发布。
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;//qi
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);//qi+1
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;
}首先是读取imu_msg得到 linear_acceleration{dx, dy, dz}，angular_velocity{rx, ry, rz};接下来是得到P V Q，相邻两帧的P V Q 是存在递推关系的，我们从理论看它是怎么推导得到的：
当前时刻P V Q的连续形式:
代码中采用的是中值法的离散形式
可以看到这与代码一一对应。所以每来一个帧，我们都可以计算其P V Q。
计算相邻帧的预积分增量
processIMU 函数，processIMU输入是{时间差，原始加速度，原始角速度}。
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}首先会进入一个初始化函数IntegrationBase
ntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
: acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
{
noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}初始化各种数据。这边注意关注各种矩阵的维度。
接下来我们关注这两个push_back
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);进入第一个push_back ,输入仍然是{时间差，原始加速度，原始角速度}，frame_count 意为处理第几个图像帧的预积分。
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}这里先把原始数据存进buffer里面，以便后续需要进行重新积分使用，重新积分也是因为初始化后零偏经过优化后的数值会发生改变，此时需要重新积分，初始化完后就不会这样了，会采用一阶的方式进行近似来更新零偏变化对预积分变化的影响，因为初始化的时候零偏变化会比较大，后续的变化量很小所以可以采用一阶的方式。
我们关注这个proagate(...)，这个就是预积分函数，输入与前面一致。
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
...............
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
..........
}这里面需要关注的是midPointIntegration的函数。我们先看一下它的输入，_dt是时间差，aac_0,gry_0是上一帧的加速度和角速度，_acc_1,_gyr_1是当前的加速度和角速度。delta_p, delta_q, delta_v对应P Q V增量，其result_delta对应其更新量。其推导如下：
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}接下来我们需要关注的是F矩阵和V矩阵，它们可以推导预积分误差的雅可比矩阵和协方差项。
其理论推导非常复杂，可以参考《VINS 论文推导及代码解析》
接下来关注第二个push_back
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);存一下tmp_pre_integration，是连续两个图像帧之间的预积分，这个值是用来做旋转外参标定用的，因为初始化的时候会用到全部的图像帧数据，不止是关键帧。这个量用来做初始化用的，后面滑窗优化就用不到这个变量了.
接下来我们关注下面这个中值积分
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;我们可以看出它对应下面的公式：
IMU约束（后端优化）：
首先我们看构建残差的函数，IntegrationBase：：evaluate()
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}构建的残差如下：
348
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
