0. 整体流程图
1. 代码目录
代码目录如下，重点和难点是factor部分，是关于IMU部分的，有较多关于IMU预积分公式的推导。
2. process主线程
2.1 条件变量con.wait读取测量值：getMeasurements()
读取buf中IMU和IMG的数据，并进行align，最后的结果是这样：
2. 读取到之后，对IMU数据进行预积分
按照上面2.1中的图示，我们可以得到如下所示的IMU和Img数据，根据时间戳不同，IMU数据可能要相应地进行插值：
对应代码的这部分：
auto img_msg = measurement.second; //measurement.second就是pair<IMUs, img_msg>的second，即img_msg，即features
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();// 最新IMU数据的时间戳
double img_t = img_msg->header.stamp.toSec() + estimator.td;//用优化后的td来补偿Img时间戳
if (t <= img_t)
{
if (current_time < 0) //第一次接受会这样
current_time = t;
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t;
//3轴加计
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
//3轴角速度
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//IMU预积分，通过中值积分得到当前PQV作为优化初值
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
// IMU时间戳晚于Img特征点数据（时间偏移补偿后）的第一帧IMU数据（也是一组measurement中的唯一一帧），对IMU数据进行插值，得到图像帧时间戳对应的IMU数据
// 那如果IMU更早，后面没有IMU数据了为什么不解决呢？
else
{
// 时间戳的对应关系如下图所示：
// current_time t
// * * * * * （IMU数据）
// | （图像特征点数据）
// img_t
double dt_1 = img_t - current_time;
double dt_2 = t - img_t;
current_time = img_t;
ROS_ASSERT(dt_1 >= 0);
ROS_ASSERT(dt_2 >= 0);
ROS_ASSERT(dt_1 + dt_2 > 0);
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
}
}
然后进行IMU预积分：
IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];//总共11维的指针数组，放的是每一帧的预积分类的指针
pre_integrations是一个指针数组，构建出IntegrationBase对象之后就可以开始IMU预积分了。
IntegrationBase是管理预积分的一个类，包含以下函数，主要是：积分函数propogate，调用预积分函数midPointIntegration(代码里面用的是中值积分法，也有欧拉积分的内容)，用估计的babg重新propogate IMU预积分
核心函数是midPointIntegration，里面的公式跟博客3.2节式(34)，式(44)完全相同，有了前面的基础，看这个很容易理解。
核心内容是预积分的离散形式和预积分的误差传递，分别见下图式(34)和(44)：
对于维度的解释，由(44)可得：
(
15
,
1
)
=
(
15
,
15
)
∗
(
15
,
1
)
+
(
15
,
18
)
∗
(
18
,
1
)
(15, 1) = (15, 15)*(15, 1) + (15, 18)*(18, 1)
(15,1)=(15,15)∗(15,1)+(15,18)∗(18,1)
故 jacobian F是(15, 15)，G是(15, 18)。
（Jacobian更新细节后面再来补充）
//IMU预积分(中值积分法)，code中都是离散形式
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");
//见博客3.2节式(34)：https://blog.csdn.net/qq_37746927/article/details/130714626#t11
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);//此时delta_q=qbi_bk
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//w=1/2*((wb_k - bgk)+(wb_k+1 - bgk)) = 1/2*(wb_k + wb_k+1) - bgk
//qbi_bk+1 = qbi_bk x Quaterniond(1, (w * delta_t)/2)
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);//此时delta_q=qbi_bk+1
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;//忽略噪声n，认为k+1时刻的bias和k时刻相等
result_linearized_bg = linearized_bg;
//更新预积分的Jacobian（内容较多,主要是在求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();
}
}
2.1 看起来似乎有两次的IMU预积分的解释
在IntegrationBase::push_back中算一次预积分，然后又在外面再算一次。
if (frame_count != 0)//
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);//调用IntegrationBase中定义的成员函数push_back，保存变量并propagate预积分
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);//保存这两帧IMU之间的时间间隔，用于预积分
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
//IMU预积分(为什么这里要重新再算一遍？push_back里面不是重新算过了吗？为什么不直接把delta_p灯结果拿出直接用？)
// 用IMU数据进行积分，当积完一个measurement中所有IMU数据后，就得到了对应图像帧在世界坐标系中的Ps、Vs、Rs（这里为什么是相对于世界坐标系呢？为什么不把关于world系的抽出来呢？）
// 下面这一部分的积分，在没有成功完成初始化时似乎是没有意义的，因为在没有成功初始化时，对IMU数据来说是没有世界坐标系的
// 当成功完成了初始化后，下面这一部分积分才有用，它可以通过IMU积分得到滑动窗口中最新帧在世界坐标系中的P V R
int j = frame_count;//到后面frame_count一直为window_size即10
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//为什么要有重力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);//mid-point中值法计算a，w在k~k+1时刻内的测量值
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
需要清楚，这两个预积分的范围是不同的，pre_integrations中存的是两个img之间的IMU预积分，
//pre_integrations定义
IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];//总共11维的指针数组，放的是每一帧的预积分类的指针
//IntegrationBase中的预积分量的定义
// delta_p、delta_q和delta_v是标称状态的预积分.假设两帧Img的index分别为ij(i<j)，则
// delta_p表示αbj_bi
// delta_q表示qbj_bi
// delta_v表示βbj_bi
Eigen::Vector3d delta_p;
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_v;
这些量在slidwindow时是会被改变的，
而所谓的第二次计算的Ps，Vs，Rs，Bas，Bgs ，计算的是目前window内的最新帧相对于world系的IMU预积分
Vector3d Ps[(WINDOW_SIZE + 1)]; // 滑动窗口中各帧在世界坐标系下的位置
Vector3d Vs[(WINDOW_SIZE + 1)]; // 滑动窗口中各帧在世界坐标系下的速度
Matrix3d Rs[(WINDOW_SIZE + 1)]; // 滑动窗口中各帧在世界坐标系下的旋转
Vector3d Bas[(WINDOW_SIZE + 1)]; // 滑动窗口中各帧对应的加速度计偏置
Vector3d Bgs[(WINDOW_SIZE + 1)]; // 滑动窗口中各帧对应的陀螺仪偏置
3. 设置重定位帧
relo_buf：订阅pose graph node发布的回环帧数据，存到relo_buf队列中，供重定位使用
关于relo的部分需要搞清楚数据结构，何时pub？
为何要在process中执行estimator.setReloFrame，何时设置reloframe？
4. processImage的KF selection部分
该部分重点是3rd和2nd间视差的计算，有一些判断条件较难理解，进行debug探究。
4.0 frame_count变量
int frame_count;// 最新帧在滑动窗口中的索引（0，1，2，... ，WINDOW_SIZE-1）WINDOW_SIZE=10，等frame_count==10时就该进行marg了，以保证window内只有10帧数据，该变量在window没满之前会自增，达到window_size之后就一直保持为window_size(10)
4.1 addFeatureCheckParallax中的条件判断
如下代码，其中进入compensatedParallax2函数的条件比较难理解，
进入条件是：要有至少3帧，且第3新帧和第2新帧之间要tracking上，保证有共视点来计算视差
这里debug研究一下：
for (auto &it_per_id : feature)
{
// it_per_id.feature_per_frame.size()就是sliding window内该id的feature被tracking的次数，
// 这两个判断条件意思是：要有至少3帧，且第3新帧和第2新帧之间要tracking上，保证有共视点来计算视差 TODO:可以debug看看，Done：已经debug过了，确实是这个意思
// 这里img一定是it_per_id.feature_per_frame.size()>=1，即至少tracking了1次，即使在1st lost了但是在3rd和2nd时分别都tracking上了，那么3rd和2nd之间就有视差
int condition_2 = it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1;
if (it_per_id.start_frame <= frame_count - 2 &&
it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1) //后一个判断条件是什么意思？
{
ROS_DEBUG("condition matched: condition_1: %d, condition_2: %d, frame_count: %d", it_per_id.start_frame, condition_2, frame_count);
// 对于给定id的特征点，计算第2最新帧和第3最新帧之间该特征点的视差（当前帧frame_count是第1最新帧）
//（需要使用IMU数据补偿由于旋转造成的视差）
parallax_sum += compensatedParallax2(it_per_id, frame_count);
parallax_num++;
} else {
ROS_WARN("condition not matched: condition_1: %d, condition_2: %d, frame_count: %d", it_per_id.start_frame, condition_2, frame_count);
}
}
如下所示：
一个最极端的例子：
该feature在3rd时被extract出来，it_per_id.feature_per_frame.size()=1，
在2nd时被tracking上了，it_per_id.feature_per_frame.size()=2，
在1st时lost掉了，it_per_id.feature_per_frame.size()=2，start=0，frame_count=2
也是符合条件的：如下打印的0，1，2就是这种例子，start=0，size()=1+1=2，frame_count=2
而0，0，2则对应的是该feature目前只在[0]帧被extract出来一次，没有被tracking上
2，2，2则代表在3rd刚被extract出来，故在3rd和2nd之间不可能存在共视点，也就没有视差。
调试结果：
4.2 compensatedParallax2()
/**
* 对于给定id的特征点
* 计算第2最新帧和第3最新帧之间该特征点的视差（当前帧frame_count是第1最新帧）
* （需要使用IMU数据补偿由于旋转造成的视差）
*/
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)
{
//check the second last frame is keyframe or not
//parallax betwwen seconde last frame and third last frame
//计算3rd和2nd在it_per_id.feature_per_frame下的index，size是该id的feature已经被tracking的次数，
// 如frame_count=4时，从第0帧开始被tracking，start_frame=0，则3rd和2nd时，it_per_id.feature_per_frame.size()分别为3[index=2],4[index=3]，则他们的index分别为4-2-0=2，4-1-0=3
// start_frame=1时，size()=2[index=1],3[index=2], index分别为4-2-1=1，4-1-1=2，图示见博客4.2节：https://blog.csdn.net/qq_37746927/article/details/134436475
int third_lst_idx = frame_count - 2 - it_per_id.start_frame;
int second_lst_idx = frame_count - 1 - it_per_id.start_frame;
//这里的window size是10
/*ROS_INFO("======here1: frame_count: %d, third_lst_idx: %d, second_lst_idx: %d", frame_count, third_lst_idx, second_lst_idx);*/
const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[third_lst_idx];//third last frame
const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[second_lst_idx];//seconde last frame
// ------------- 3rd 2nd 1st
// other_frame i j frame_count
double ans = 0;
Vector3d p_j = frame_j.point;//2nd
double u_j = p_j(0);
double v_j = p_j(1);
Vector3d p_i = frame_i.point;//3rd
Vector3d p_i_comp;
//int r_i = frame_count - 2;
//int r_j = frame_count - 1;
//p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;//这是将i重投影到j？计算rpj error？搞错了吧
p_i_comp = p_i;
double dep_i = p_i(2);//深度
double u_i = p_i(0) / dep_i;//i归一化
double v_i = p_i(1) / dep_i;
double du = u_i - u_j, dv = v_i - v_j;//计算i,j帧间的视差(为啥j不归一化？)
double dep_i_comp = p_i_comp(2);
double u_i_comp = p_i_comp(0) / dep_i_comp;
double v_i_comp = p_i_comp(1) / dep_i_comp;
double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;
// ROS_INFO("====u_equal: %d, v_equal: %d, u_i: %f, u_i_comp: %f, v_i: %f, v_i_comp: %f", u_i==u_i_comp, v_i==v_i_comp, u_i, u_i_comp, v_i, v_i_comp);
//这俩货是一样的，有啥作用？也没看到IMUN啥补偿啊？
ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));//勾股定理计算视差的欧氏距离
return ans;
}
3rd和2nd的index计算图示
调试结果符合预期：
论文IV.A节中说的使用短时gyro测量的积分来补偿预积分，出现在计算系统关于IMU的残差的Jacobian时，imu_factor.h中：
//使用短时gyro的数据对预积分q进行补偿，对应论文IV.A节
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
...
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
计算出视差之后就可以判断2nd是否为KF，完成了KF selection。
接下来是CalibrationExRotation旋转外参标定部分。
1431
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
