estimator_node.cpp
1、解释predict函数
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;
}
//获取dt并传递时间
double dt = t - latest_time;
latest_time = t;
//获取当前时刻的IMU采样数据
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;//参见vio公式21,上一时刻的加速度、tem_Q等数据
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;//上一时刻的数据
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);//Q的更新就是公式26 //Q为body系到world系之间的变换,此时求出的是此时刻得Q
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;//当前时刻的P当前时刻的加速度
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;//求出此时刻的P
tmp_V = tmp_V + dt * un_acc;//求出此时刻的V
///注意,这里就是关于IMU的普通的运动积分,不涉及与积分
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
本段程序涉及到的公式如下两个图
2、getMeasurements()函数
getMeasurements()
{ //这个函数的作用就是把图像帧和对应的IMU数据们 配对起来,而且IMU数据时间是在图像帧的前面
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
while (true)
{
//边界判断:数据取完了,说明配对完成
if (imu_buf.empty() || feature_buf.empty())
return measurements;
//边界判断:IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早,说明缺乏IMU数据,需要等待IMU数据
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
//ROS_WARN("wait for imu, only should happen at the beginning");
sum_of_wait++;//统计等待的次数
return measurements;
}
//边界判断:IMU第一个数据的时间要大于第一个图像特征数据的时间(说明图像帧有多的)
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
ROS_WARN("throw img, only should happen at the beginning");
feature_buf.pop();
continue;
}
//核心操作:装入视觉帧信息
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
//核心操作:转入IMU信息
std::vector<sensor_msgs::ImuConstPtr> IMUs;
while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
{
IMUs.emplace_back(imu_buf.front());//加入IMUS
imu_buf.pop();
}
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
measurements.emplace_back(IMUs, img_msg);
//注意:把最后一个IMU帧又放回到imu_buf里去了
//原因:最后一帧IMU信息是被相邻2个视觉帧共享的
}
return measurements;
}
getMeasurements()函数,从imu_buf和feature_buf中读入IMU数据和图像特征点数据,并在该函数中对IMU数据和图像特征点数据进行初步的时间戳对齐。
我主要想解释那两个条件,imu保证在2个图像之间的,初步的时间戳对齐需要满足两个条件: 1. IMU缓存队列中队尾数据的时间戳,要晚于图像特征点数据缓存队列中队首数据的时间戳; 2. IMU缓存队列中队首数据的时间戳,要早于图像特征点数据缓存队列中队首数据的时间戳。
满足两个条件,才进行装载数据:
1.当最新的imu帧(imu_buf.back()) 新过 图像帧(feature_buf.front()+ td )的时间戳(保证了图像帧之前已经没有imu数据了)
2.最旧的imu帧(imu_buf.front()) 旧过 图像帧(feature_buf.front()+ td )的时间戳的(保证了图像帧之前一定要有imu帧)
时间戳初步对齐的结果是:一帧图像特征点数据与多帧IMU数据对应,这些IMU数据为:该帧图像与上一帧图像的时间间隔内的所有IMU数据。这里只保证了相邻的feature数据之间有完整的IMU数据。每一组IMU数据和对应的图像特征点数据组成measurement,多组measurement组成measurements,作为getMeasurements()的返回值。
值得一提的是,在获得观测数据的过程中,使用了互斥锁和条件等待的功能。当回调函数imu_callback()或者feature_callback()从话题消息中接收数据时,互斥锁会锁住imu_buf或者feature_buf,此时getMeasurements()就无法从imu_buf、feature_buf中获取观测数据。当imu_callback()或者feature_callback()接收话题数据完成时,条件等待就会唤醒getMeasurements()从imu_buf、feature_buf中读取观测数据,在读取过程中两个回调函数imu_callback()和feature_callback()是无法接收数据的。当getMeasurements()提取完成后,回调函数就可以继续接收话题消息数据。
图解说明比较清晰:
参考:https://blog.csdn.net/huanghaihui_123/article/details/103032782 吧 https://qingsimon.github.io/post/2018-12-10-vins-mono%E4%BA%8Cvins_estimator_node/
3、预积分中的表达式部分----(IntegrationBase类详解中的中值积分函数(midPointIntegration))预积分是vins的精髓,这段代码找了半天才找到。
所使用的公式即为下图中的公式,一一对应即可,注意,代码中的delta_q是IMU的body系下的相邻两帧之间的变换。
更加需要指出:代码中delta_p就是下图中αbibk+1,
代码中的delta_p就是下图中βbibk+1,
代码中的dela_q就是下图中的qbibk+1。
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)
{
/**该函数用于求解IMU的预积分状态递推和更新对应的Jacobian矩阵,
* 函数接口参数分别为:
t时刻的IMU量测:_acc_0, _gyr_0
t时刻的位置、姿态、速度预积分估计值:delta_p, delta_q, delta_v
t+1时刻的IMU量测:_acc_1, _gyr_1
t+1时刻的位置、姿态、速度预积分估计值:result_delta_p, result_delta_q, result_delta_v
t和t+1时刻加速度计和陀螺仪零偏估计值:result_linearized_ba, result_linearized_ba, result_linearized_ba, result_linearized_bg
Jacobian矩阵更新选项:update_jacobian***/
//ROS_INFO("midpoint integration");
//注意这里不涉及世界坐标系,全在body坐标系下。公式见与积分离散形式(vio从零开始写vi0中公式33)
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;
midPointIntegration函数剩下一部分就是预积分的误差递推函数公式。
于是就得到了F和G表达式。
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();
//这部分应该就是预积分误差传递的形式,
}
最重要的是得到了这段代码:
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
//这部分应该就是预积分误差传递的形式,
为什么要这么做递推?见下图!
就是用概率的方法保证信息传递的准确性,毕竟,预积分通俗解释,就是在相邻两个相机的图片之间有很多imu的数据,通过预积分简化运算,但是将这些imu数据合并的过程中就需要以概率的形式给他们加权处理,置信度越高,比例就越大,权重就越大。这是我个人理解。
20200109 于研究室