预积分的公式推导已经完成了，接下来将一起阅读一下预积分对应的代码，这部分的代码实际比较简单，后面的初始化才会比较难。
开始节点在 estimator_node.cpp 中
main函数代码如下：
描述： sub_imu是直接订阅 IMU 的原始数据
sub_image订阅的是光流估计节点返回的光流特征点结果，这个是在 feature_tracker_node.cpp 这个节点里面运行，比较简单就不带读了
这个函数会一直循环运行 process函数
int main(int argc, char **argv)
{
// 初始化ROS节点，其中argc是命令行参数的数量，argv是命令行参数的值，
// "vins_estimator" 是节点的名称。
ros::init(argc, argv, "vins_estimator");
// 创建一个ROS节点的句柄（Handle），通常用于与ROS通信。
// 参数 "~" 会使节点句柄查找与节点名称相关的参数。
ros::NodeHandle n("~");
// 设置ROS控制台日志的级别为Info，这会影响日志的输出详细程度。
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
// 读取ROS参数
readParameters(n);
estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
// 注册一些publisher
registerPub(n);
// 接受imu消息
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
// 接受前端视觉光流结果
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
// 接受前端重启命令
ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
// 回环检测的fast relocalization响应
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
// 核心处理线程
std::thread measurement_process{process};
ros::spin();
return 0;
}
进入 process 函数
描述：
这里通过条件变量来控制互斥锁的具体解释看这个链接 [c++11]多线程编程(六)——条件变量(Condition Variable)
总的来作用就是防止 process() 一直不停得循环在等待数据的到来，因为数据没到根据
measurements = getMeasurements()).size() != 0;
这句代码就会不停的循环运行进行判断，也会不停地获取锁和解锁，在还没来数据时这样的操作是很占用CPU内存的，通过这个锁的条件变量的话就会等数据生产那边放入数据了，然后通知这个wait()函数，把它唤醒，此时再执行getMeasurements()函数。
在第一次执行的时候wait会获取锁，然后执行getMeasurements()函数，返回false的话就会unlock这个锁然后进入休眠状态，等待con条件变量进行唤醒，唤醒后会再执行getMeasurements()函数，为true的话就会往下执行代码
唤醒操作在imu_callback线程和feature_callback线程中进行
- 这里的con 是一个条件变量（std::condition_variable）的实例
- wait 方法是等待条件变量的成员函数，它接受一个锁（std::unique_lock）和一个条件（lambda 函数）。线程会在此等待，直到条件为真。
- lk 是一个互斥锁（std::mutex）的实例，它用于保护共享资源，确保在等待条件满足时，其他线程不会同时访问这些资源。
- [&] 表示 lambda 函数捕获当前作用域的所有变量，这里主要是捕获了 measurements 这个向量。
- getMeasurements() 是一个函数，用于匹配 IMU 和 image 数据
顺便讲一下这里的 Lambda 表达式，这里的 [&] 表示以引用的方式捕获实现函数内部使用的变量，这里没有设置参数列表( )，一般的lamba表达式为 [ ] ( ) { } ，[=]为等值捕获，即获取数值，[&] 为引用捕获，捕获的意思就是函数实现内部直接使用变量即可，他会自动捕获的，不用我们像声明函数一样弄个传参这些，当成函数来使用就例外，( )是参数列表，是为了当这个lamba函数赋值给一个变量的时候，这个变量可以当作一个函数来使用来传入参数，{ } 是具体函数的内容。在这里不需要参数列表所以就没有设置 ( )，并且是以引用的方式捕获 measurements 变量
void process()
{
while (true) // 这个线程是会一直循环下去
{
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
lk.unlock(); // 数据buffer的锁解锁，回调可以继续塞数据了
然后现在进入getMeasurements()函数
描述： 总的功能就是需要确保 image 前要有的 IMU 数据
第一个判断的是 image 前面没有还没有 IMU 数据
第二个判断是 IMU 数据来晚了，前面有几个 image 没有对应的 IMU数据要扔掉没IMU的image
第三个判断就是 image 间有 IMU 数据 ，就把IMU数据存起来，然后pop掉最老的那一个数据，但是image后一刻的IMU数据也要存着，因为需要插值
然后把 IMU 数据和 image 数据捆绑起来放进measurements中
// 获得匹配好的图像imu组
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
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 *******
// image *****
// 这就是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 ****
// image ******
// 这种只能扔掉一些image帧
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;
}
// 此时就保证了图像前一定有imu数据
sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
feature_buf.pop();
// 一般第一帧不会严格对齐，但是后面就都会对齐，当然第一帧也不会用到
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());
imu_buf.pop();
}
// 保留图像时间戳后一个imu数据，但不会从buffer中扔掉
// imu * *
// image *
IMUs.emplace_back(imu_buf.front());
if (IMUs.empty())
ROS_WARN("no imu between two image");
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
退出getMeasurements()函数，返回上一级
遍历measurements，就是遍历匹配的数据
描述： 直接从 imu_msg取出原始数据的加速度和角速度，放入 processIMU 里面进行预积分
m_estimator.lock(); // 进行后端求解，不能和复位重启冲突
// 给予范围的for循环，这里就是遍历每组image imu组合
for (auto &measurement : measurements)
{
auto img_msg = measurement.second;
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
// 遍历imu
for (auto &imu_msg : measurement.first)
{
double t = imu_msg->header.stamp.toSec();
double img_t = img_msg->header.stamp.toSec() + estimator.td;
if (t <= img_t) //再判断一次确保时间正确
{
if (current_time < 0)
current_time = t;
double dt = t - current_time;
ROS_ASSERT(dt >= 0);
current_time = t; // current_time 严格意义上是上一时刻的时间
dx = imu_msg->linear_acceleration.x;
dy = imu_msg->linear_acceleration.y;
dz = imu_msg->linear_acceleration.z;
rx = imu_msg->angular_velocity.x;
ry = imu_msg->angular_velocity.y;
rz = imu_msg->angular_velocity.z;
// 时间差和imu数据送进去
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);
}
进入 processIMU 函数
/**
* @brief 对imu数据进行处理，包括更新预积分量，和提供优化状态量的初始值
*
* @param[in] dt
* @param[in] linear_acceleration
* @param[in] angular_velocity
*/
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;
}
// 滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了
// 由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
// 所以只有大于0才处理 frame_count++ 在processImage中进行
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;
}
我们先进入
new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
这个类，看它里面的初始化，这个类是一个预积分的类
描述： 里面的 0 就是第 k 时刻，初始化的雅可比矩阵 J 为单位阵，因为IMU数据没有来临前他们两两间没有联系
协方差矩阵初始化为0，因为没有数据，所以置信度也是 0
这里的雅可比 J 意思是 第 k 时刻的状态量对自己的偏导 记为
σ
x
k
σ
x
\frac{σx_{k}}{σx}
σxσxk ，这里的
x
x
x 意思为
x
k
x_{k}
xk 自身，初始时刻什么数据都没有的时候自己对自己的偏导肯定是单位矩阵
这里的噪声矩阵为
18
×
18
18×18
18×18 的矩阵，第6节中有讲为什么是18，由于使用中值积分，噪声的变量为
[
n
a
k
,
n
a
k
+
1
,
n
w
k
,
n
w
k
+
1
,
b
a
,
b
w
]
[n_{ak},n_{ak+1},n_{wk},n_{wk+1},b_{a},b_{w}]
[nak,nak+1,nwk,nwk+1,ba,bw] ，每个量都是3自由度
class IntegrationBase
{
public:
IntegrationBase() = delete;
IntegrationBase(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},
// 这个雅可比是预积分量对自身的雅可比，在CSDN第7节中有讲述它的作用，这个主要作用是3个PVQ量对ba,bw的雅可比
// IMU数据来临前这个矩阵是单位矩阵，因为两两没有联系
// 协方差矩阵在没有数据来临前是0，数据没有的话置信度肯定也是0
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()}
{
// 这个是噪声的协方差矩阵，为18*18的矩阵，因为噪声的矩阵为18*1∈[nak,nak+1,nwk,nwk+1,ba,bw]，总共6个量
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();
}
返回上一级回到 processIMU 函数
讲解这句代码
描述： 这里是传入时间差
d
t
dt
dt ，原始的加速度值
a
a
a 和原始的角速度值
w
w
w，这个push_back是自己构造的，frame_count这个是图像帧的id，意为处理第几个图像帧的预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
进入 push_back (…)
描述： 这里先把原始数据存进buffer里面，以便后续需要进行重新积分使用，重新积分也是因为初始化后零偏经过优化后的数值会发生改变，此时需要重新积分，初始化完后就不会这样了，会采用一阶的方式进行近似来更新零偏变化对预积分变化的影响，因为初始化的时候零偏变化会比较大，后续的变化量很小所以可以采用一阶的方式。
里面的 propagate() 就是预积分的函数
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
// 相关时间差和传感器数据保留，方便后续repropagate
// 这个重新传播一般发生在VIO初始化中，其实是零偏经过优化后直接进行重新预积分，后面跟踪才是采用一阶近似的方式
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
进入propagate函数
描述： 传入的 IMU 数据是最新的数据，即第k+1时刻的数据，然后传入中值积分操作函数 midPointIntegration(…)，严格意义上这里的
k
+
1
k+1
k+1 应该为
i
+
1
i+1
i+1
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 这个就是中值积分，主要的预积分操作就在这里面
// 0 是对应k时刻 1是K+1时刻，这里的pvq都是k时刻的，ba,bg是零偏，零偏根据预积分建模，其在两帧间积分间是保持不变的
// result是 k+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);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize(); // 缩放为单位四元数，保证其模长为1
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
进入中值积分函数 midPointIntegration(…)
描述： 更新第
i
i
i 时刻的加速度值
a
i
a_{i}
ai，这里把加速度转到第 k 坐标系下，根据中值积分更新
i
+
1
i+1
i+1 时刻的角速度值
w
w
w ，然后更新
i
+
1
i+1
i+1 时刻的旋转量
q
i
+
1
q_{i+1}
qi+1 ，这里的更新公式为
q
k
,
i
+
1
=
q
k
,
i
⊕
q
i
,
i
+
1
q_{k,i+1}=q_{k,i}⊕q_{i,i+1}
qk,i+1=qk,i⊕qi,i+1 这里的
q
i
,
i
+
1
q_{i,i+1}
qi,i+1 定义为
[
1
,
w
Δ
t
2
]
[1,\frac{wΔt}{2}]
[1,2wΔt] ，具体推导看第一节的旋转的定义，这里的 k 表示关键帧的第 k 帧，
i
i
i 表示两帧间的预积分量的数据时间
根据刚刚算出来的
q
k
,
i
+
1
q_{k,i+1}
qk,i+1 ，把
i
+
1
i+1
i+1 时刻的加速度值变换到
k
k
k 坐标系下，这样就和第
i
i
i 下的加速度值统一坐标系了
然后根据中值积分计算
i
+
1
i+1
i+1 时刻的加速度值，来当作
i
i
i 到
i
+
1
i+1
i+1 的积分量
然后更新平移和速度
平移更新公式
p
=
p
+
v
+
1
2
a
t
2
p=p+v+\frac{1}{2}at^{2}
p=p+v+21at2
速度更新公式
v
=
v
+
a
t
v=v+at
v=v+at
这样就完成了
i
i
i 到
i
+
1
i+1
i+1 的
p
v
q
p v q
pvq 的更新量，反复计算就可以获得两个图像帧间的预积分，然后拿这个预积分来作为约束
但是拿这个量来进行约束还需要一个值，那就是这个预积分量的置信度，也就是协方差矩阵
下面一大串的
F
、
V
F、V
F、V 矩阵就是用来计算协方差矩阵和状态量对零偏的雅可比的
这个
F
、
V
F、V
F、V 是由离散时间下的误差传递推导来的，具体见第6节推导，更新公式为
Δ
x
k
+
1
=
F
⋅
Δ
x
k
+
G
⋅
n
k
Δx_{k+1}=F·Δx_{k}+G·n_{k}
Δxk+1=F⋅Δxk+G⋅nk
稍微讲一下怎么来的： 首先预积分本来是积分 ∫ ∫ ∫ 的形式，通过积分可以获得第 k k k 时刻到 k + 1 k+1 k+1 时刻的变化，在第5节（VINS-Mono-IMU预积分 （五：连续时间预积分误差状态传递））中先对状态量 [ p v q b a b w ] [p \ v \ q \ b_{a} \ b_{w}] [p v q ba bw] 进行对时间的求导，获得其根据时间的微量（导数，雅可比J），然后由于实际的 IMU 数据是离散的，所以无法进行求导，也无法进行积分的操作，但是现在需要获得第 k k k 时刻到 k + 1 k+1 k+1 时刻的变化，根据导数的定义 f ( x + Δ x ) = f ( x ) + J ( x ) ⋅ Δ x f(x+Δx)=f(x)+J(x)·Δx f(x+Δx)=f(x)+J(x)⋅Δx，我们之前通过连续积分求的雅可比 J ( x ) J(x) J(x) 就有作用了，就是代入进去，然后在这里仍然是对时间的求导，所以可以写成 f ( t + Δ t ) = f ( t ) + J ( t ) ⋅ Δ t f(t+Δt)=f(t)+J(t)·Δt f(t+Δt)=f(t)+J(t)⋅Δt ，其实指的就是 第 k k k 时刻到 k + 1 k+1 k+1 时刻的状态量变化，然后根据这个定义进行推导（具体见第6节VINS-Mono-IMU预积分 （六：离散时间预积分误差传递）），然后就获得了离散时间下的更新公式 Δ x k + 1 = F ⋅ Δ x k + G ⋅ n k Δx_{k+1}=F·Δx_{k}+G·n_{k} Δxk+1=F⋅Δxk+G⋅nk 这里的 F F F 的实际意义其实是 k + 1 k+1 k+1 状态量对 k k k 时刻状态量的导数，可以写成 F = σ x k + 1 σ x k F=\frac{σx_{k+1}}{σx_{k}} F=σxkσxk+1
更新协方差的公式没什么好讲的，公式为
P
k
+
1
=
F
⋅
P
k
⋅
F
T
+
G
⋅
V
k
⋅
G
T
P_{k+1}=F·P_{k}·F^{T}+G·V_{k}·G^{T}
Pk+1=F⋅Pk⋅FT+G⋅Vk⋅GT，
P
k
P_{k}
Pk 为K时刻的状态量协方差
初始时刻的协方差矩阵
p
k
p_{k}
pk 为0，因为没有数据来，所以置信度肯定也是0，
V
k
V_{k}
Vk 是噪声的协方差
更新雅可比
J
J
J ，这个要稍微理解一下，更新后的雅可比 J 可以定义为
σ
x
k
+
1
σ
x
\frac{σx_{k+1}}{σx}
σxσxk+1 这里的
x
x
x 指的是对自身的导数
初始值的雅可比为
σ
x
k
σ
x
\frac{σx_{k}}{σx}
σxσxk ，其含义也是第
k
k
k 时刻的状态对自己的导数（初始值为单位矩阵，因为没有数据，所以自己对自己求导就是单位矩阵），然后现在要从将雅可比从
k
k
k 时刻传递到
k
+
1
k+1
k+1 时刻，然后
F
=
σ
x
k
+
1
σ
x
k
F=\frac{σx_{k+1}}{σx_{k}}
F=σxkσxk+1 ，则有
σ
x
k
+
1
σ
x
=
σ
x
k
+
1
σ
x
k
⋅
σ
x
k
σ
x
\frac{σx_{k+1}}{σx}=\frac{σx_{k+1}}{σx_{k}}·\frac{σx_{k}}{σx}
σxσxk+1=σxkσxk+1⋅σxσxk ，这样就获得了
k
+
1
k+1
k+1 时刻的状态量对自己的雅可比矩阵，其实这个矩阵的作为也只是为了获得
p
,
v
,
q
p,v,q
p,v,q 各自对
b
a
,
b
w
b_{a},b_{w}
ba,bw 的雅可比矩阵。
这个雅可比的作为是为了在零偏 b a , b w b_{a},b_{w} ba,bw 在经过优化数值发生变化后不需要再重新进行预积分，因为实际上 k + 1 k+1 k+1 时刻的预积分是用 k k k 时刻的零偏 b b b 来进行的，当经过优化后 k + 1 k+1 k+1 时刻的零偏 b b b 就会发生变化，理论上 k + 1 k+1 k+1 时刻的预积分应该是用 k + 1 k+1 k+1 时刻的零偏 b b b 来进行才对，由于是优化后才获得，但是重新进行预积分又非常耗时，所以采用 f ( x + Δ b ) = f ( x ) + J b x ⋅ Δ b f(x+Δb)=f(x)+J^{x}_{b}·Δb f(x+Δb)=f(x)+Jbx⋅Δb 这样的方式来进行近似更新，从这个公式可以看出里面就必须要对零偏的雅可比矩阵，但是在VIO初始化后还是会重新预积分的，只是初始化完成后才会用这个近似公式。零偏具体建模推导见VINS-Mono-IMU预积分 （七：预积分零偏建模方式）
上一时刻的零偏直接赋值给下一时刻的零偏，因为根据建模方式，在预积分这段时间内零偏是不会发生变化的，所以直接赋值就可以了
预积分公式复杂其实就是为了获取当前时刻状态量的协方差矩阵（置信度）和 状态量对零偏的雅可比
这里的协方差其实每个IMU数据的协方差都会累乘在一起，预积分到最终时刻
k
+
1
k+1
k+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)
{
//ROS_INFO("midpoint integration");
// 首先中值积分更新状态量
// 计算k时刻在bk系下加速度的值，这个k是图像间的k，delta_q是t到k时刻的变换，这个t是imu系下的时间
// 第k时刻的pvq都认为是0点
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); // Rki*ai ,i是imu时间下的坐标
//陀螺仪只用来算帧间旋转，所以不用坐标系转换，用陀螺仪的均值代表两帧陀螺仪间的值，这样就不用积分了，这就是中值积分
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // 四元数虚部的推导后期得再看一下视频理解一下怎么来的
// 这里是获得i+1下Rk i+1的的旋转变换
// Rki+1=Rki*Rii+1 后面的四元数构造与CSDN第一节的推导一致[1,wt/2] ，因为角度趋向于0,cos theta =1
// 公式那样推导也是为了和角速度产生联系，这样就能用角速度构造四元数了
// dt 是第i 与第i+1的时间差，就是IMU间的时间差
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
// 现在有了i+1下的旋转值，那么就可以求得i+1下转换到bk系下的加速度值
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba); // 上面先算当前状态下的位姿变换，然后再操作加速度
// 现在两个速度都是同一个坐标系bk下了，然后求一下均值，来代表这段时间的加速度值
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); // 采用中值积分去做
// 然后更新i+1时刻下的位移p和速度v
// p = p + v+(a*t*t)/2
// v = v + at
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; // i 时刻下的imu加速度
Vector3d a_1_x = _acc_1 - linearized_ba; // i+1 时刻
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;
// xi+1=F*xi+G*n 这个公式
// 这个F矩阵是i+1 对 i 的状态量的雅可比
// 与公式一一对应
// 用误差卡尔曼模型最终就是为了推导出这个协方差矩阵
// 对连续时间求导其实就是为了求出 k 到 k+1的变化微量，然后推导出离散下 k 到 k+1 的状态量更新公式，重点是这个更新公式，同时这个公式里面的F的含义就是 k+1的状态量对k时刻的导数
// 然后根据这个含义就可以推导出对k+1时刻对自己的雅可比，这个雅可比也是为了获得对bias的雅可比
// 总的来说这个F矩阵就是为了计算协方差（预积分的置信度）的更新，和对自身雅可比的更新（这个是为了更新零偏对预积分量的影响）
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;
// 这里与公式推导的全部相差一个负号，但是噪声的正负是一样的意思，因为噪声本来就是0均值的分布
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;
// 这里获得pvq对零偏的雅可比，推导根据第7节可得
// 按照这么看的话这个雅可比是自身k+1对k时刻的求导，分母的x的含义一直都是自己的意思，但是F是k+1对k时刻的求导，经过相乘可得k+1时刻对自己的求导
jacobian = F * jacobian; // 从k状态对自己的雅可比传递到k+1状态对自己的雅可比，这个是状态对bias的雅可比，因为每次优化ba都会变化，这样做是为了避免重新预积分，属于零偏建模部分
// covariance 初始值是单位矩阵 ，noise 是用配置文件的值设置的
// 这个协方差是预积分量的置信度
covariance = F * covariance * F.transpose() + V * noise * V.transpose(); // 这个是协方差传递公式
}
}
预积分部分讲完啦，接下来讲图像数据的处理
回到自己创建的push_back（）函数的前面，即processIMU()中，这里重新复制一遍过来
这个processIMU（）里面处理的是每一帧的IMU的数据，两个图像帧之间是会有很多IMU数据的
描述：在自定义的push_back中做好预积分后，存一下tmp_pre_integration，是连续两个图像帧之间的预积分，这个值是用来做旋转外参标定用的，因为初始化的时候会用到全部的图像帧数据，不止是关键帧。
然后processIMU（）最下面又会有个中值积分，注意，这里的积分量是变换到世界系下面去的，目的是给非线性优化一个可靠的初值，通过
R
s
[
j
]
Rs[j]
Rs[j] 进行变换到世界系下，这里的Rs和刚刚预积分里面的 delta_q 是不同的，delta_q 是变换到上一帧的坐标系下
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;
}
// 滑窗中保留11帧，frame_count表示现在处理第几帧，一般处理到第11帧时就保持不变了
// 由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
// 到后面会一直指向最后的帧
if (!pre_integrations[frame_count])
{
// 初始的时候才要创建
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
// 所以只有大于0才处理 frame_count++ 在processImage中进行
// 其实就是帧为0的预积分量不会用到，就是在这里使得第一帧的预积分不被使用，因为第一帧前面的预积分量会很长，也够不成两帧间约束
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);
// 又是一个中值积分，更新滑窗中状态量，本质是给非线性优化提供可信的初始值
// 这里的作用主要是更新滑窗里面的状态值，把IMU的积分当作一个可靠的初值
int j = frame_count; // Rs[j] 暂时猜测是上一帧的位姿变换，这样做积分就会比较准确
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g; // 这里的 Rs 和预积分里面的delta_q不一样，这里是乘等于，是把当前量变到世界坐标系下，预积分是变到上一帧的坐标系中
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;
}
退出 processIMU 函数，返回上一级，剩下的部分在下一节中讲解
else // 这就是针对最后一个imu数据，需要做一个简单的线性插值
{
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);
}
}
// set relocalization frame
// 回环相关部分
sensor_msgs::PointCloudConstPtr relo_msg = NULL;
while (!relo_buf.empty()) // 取出最新的回环帧
{
relo_msg = relo_buf.front();
relo_buf.pop();
}
if (relo_msg != NULL) // 有效回环信息
{
vector<Vector3d> match_points;
double frame_stamp = relo_msg->header.stamp.toSec(); // 回环的当前帧时间戳
for (unsigned int i = 0; i < relo_msg->points.size(); i++)
{
Vector3d u_v_id;
u_v_id.x() = relo_msg->points[i].x; // 回环帧的归一化坐标和地图点idx
u_v_id.y() = relo_msg->points[i].y;
u_v_id.z() = relo_msg->points[i].z;
match_points.push_back(u_v_id);
}
// 回环帧的位姿
Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
Matrix3d relo_r = relo_q.toRotationMatrix();
int frame_index;
frame_index = relo_msg->channels[0].values[7];
estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
}
ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());
TicToc t_s;
// 特征点id->特征点信息
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
int v = img_msg->channels[0].values[i] + 0.5;
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x; // 去畸变后归一化像素坐标
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i]; // 特征点像素坐标
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i]; // 特征点速度
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1); // 检查是不是归一化
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);
// 一些打印以及topic的发送
double whole_t = t_s.toc();
printStatistics(estimator, whole_t);
std_msgs::Header header = img_msg->header;
header.frame_id = "world";
pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
pubRelocalization(estimator);
//ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
}
m_estimator.unlock();
m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
update();
m_state.unlock();
m_buf.unlock();
}
}
2060
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
