《自动驾驶与机器人中的SLAM技术》ch8：基于 IESKF 的紧耦合 LIO 系统

基于 IESKF 的紧耦合 LIO 系统

1 IESKF 的状态变量和运动过程

1.1 对名义状态变量的预测

1.2 对误差状态变量的预测及对协方差矩阵的递推

2 观测方程中的迭代过程

3 高维观测中的等效处理

4 NDT 和卡尔曼滤波的联系

5 紧耦合 LIO 系统的主要流程

5.1 IMU 静止初始化

5.2 ESKF 之运动过程——使用 IMU 预测

5.3 使用 IMU 预测位姿进行运动补偿

5.4 松耦合系统的配准部分

参考

紧耦合系统，就是把点云的残差方程直接作为观测方程，写入观测模型中。这种做法相当于在滤波器或者优化算法内置了一个 ICP 或 NDT。因为 ICP 和 NDT 需要迭代来更新它们的最近邻，所以相应的滤波器也应该使用可以迭代的版本，ESKF 对应的可迭代版本的滤波器即为 IESKF。

紧耦合和松耦合的联系：

	紧耦合LIO	松耦合 LIO
预测	使用IMU读数预测得到先验位姿	同
观测	使用滤波器预测得到的先验位姿（首次）和更新后位姿（后续迭代）计算点云残差	使用点云配准部分迭代优化得到的位姿作为观测值，观测过程本身不迭代
更新	多次迭代，直到更新量dx满足要求每次迭代都会以上一次更新的位姿来重新计算点云残差	一次更新

基于 IESKF 的紧耦合 LIO 系统

基于 IESKF 的紧耦合 LIO 系统的流程图如下所示：

1 IESKF 的状态变量和运动过程

IESKF 的状态变量及运动过程和前文介绍过的 ESKF 的状态变量及运动过程完全相同，包括：① 对名义状态变量的预测 ②对误差状态变量的预测及对协方差矩阵的递推参考《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航和《自动驾驶与机器人中的SLAM技术》ch7：基于 ESKF 的松耦合 LIO 系统即可。

1.1 对名义状态变量的预测

$\begin{aligned} &{p}(t+\Delta t) =p(t)+{v(t)}\Delta t+\frac{1}{2}\left({R(t)}(\tilde{{a}}-{b}_{a}(t))\right)\Delta t^{2}+\frac{1}{2}{g}(t)\Delta t^{2}, \\ &{v}(t+\Delta t) =v(t)+R(t)(\tilde{a}-b_{a}(t))\Delta t+g(t)\Delta t, \\ &{R}(t+\Delta t) =R(t)\mathrm{Exp}\left((\tilde{\omega}-b_{g}(t))\Delta t\right), \\ &{b}_g(t+\Delta t) =b_g(t), \\ &{b}_{a}(t+\Delta t) =b_{a}(t), \\ &{g}(t+\Delta t) =g(t). \end{aligned}$

1.2 对误差状态变量的预测及对协方差矩阵的递推

$F$ 为线性化后的雅可比矩阵，由于 离散时间下误差状态变量的运动方程 已经线性化，所以我们可以直接得到 $F$ 。注意其等号右侧时间下标为 $k-1$ ：

$F=\begin{bmatrix}I&I\Delta t&0&0&0&0\\0&I&-R(\tilde{a}-b_a)^\wedge\Delta t&0&-R\Delta t&I\Delta t\\0&0&\mathrm{Exp}\left(-(\tilde{\omega}-b_g)\Delta t\right)&-I\Delta t&0&0\\0&0&0&I&0&0\\0&0&0&0&I&0\\0&0&0&0&0&I\end{bmatrix},$ $\delta{x}=\begin{bmatrix}\delta{p}\\\delta{v}\\\delta{\theta }\\\delta{b_{g}}\\\delta{b_{a}}\\\delta{g}\end{bmatrix}$

在此基础上执行 对误差状态变量的预测及对协方差矩阵的递推：

$\begin{aligned} &\delta{x}_{\mathrm{k,pred}}={F}_{\mathrm{k}}*{\delta}{x}_{\mathrm{k-1}}={0} \\ &{P}_{\mathrm{k,pred}}={F}_{\mathrm{k}}{P}_{\mathrm{k-1}}{F}_{\mathrm{k}}^{\mathrm{T}}+{Q}_{\mathrm{k}} \end{aligned}$

省略时间下标得：

$\begin{aligned}&\delta x_{\mathrm{pred}}=F\delta\boldsymbol{x},\\&P_{\mathrm{pred}}=FPF^{\top}+Q.\end{aligned}$

书上的内容如下所示：

2 观测方程中的迭代过程

整个示意图如下图所示。我们从 $x_0$ ， $P_0$ 出发，不断迭代观测模型，计算出本次迭代的 $\delta{x}_i$ ，进而得到下一次迭代的 $x_{i+1}$ 和 $P_{i+1}$ （在滤波器未收敛时只需进行切空间投影），最终收敛。

切空间投影：把一个切空间中的高斯分布投影到另一个切空间中。

考虑当前为第 $i$ 次迭代，工作点是 $x_i$ 、 $P_i$ ，希望计算本次的增量 $\delta x_{i}$ ，进而得到下一次迭代的 $x_{i+1}$ 和 $P_{i+1}$ 。

IESKF 的更新过程的表达式如下：

$\begin{aligned} & K_{i}=P_{i}H_{i}^{\top}(H_{i}P_{i}H_{i}^{\top}+V)^{-1}, \\ & \delta x_{i}=K_{i}(z-h(x_{i})). \end{aligned}$

对于其中的 $P_{i}$ ：

如果滤波器没有收敛，则暂不使用卡尔曼公式对 $P_i$ 进行更新，因为下一时刻的 $J_{i+1}$ 可以由 $x_{i+1}$ 算得，所以可以按照那时的 $J_{i+1}$ ，将初始分布的协方差投影过去。公式如下：

$\begin{aligned} & {J}_{\mathrm{i}}=\mathrm{diag}({I}_{3},{I}_{3},{J}_{\theta},{I}_{3},{I}_{3},{I}_{3}) \\ & {J}_{\theta}={I}-{\frac{1}{2}}{\delta}{\theta}_{\mathrm{i}}{}^{\wedge} \\ & \delta\theta_{\mathrm{i}}={Log}({R_{i}}^{\mathrm{T}}{R_{0}}) \\ & {P}_{\mathrm{i}}={J}_{\mathrm{i}}{P}_{\mathrm{k,~pred}}{J}_{\mathrm{i}}^{T} \end{aligned}$

即 $x_{i+1} \rightarrow R_{i+1}\rightarrow \delta\theta_{\mathrm{i+1}} \rightarrow {J}_{\theta+1}\rightarrow J_{i+1}\rightarrow P_{i+1}$ 。

如果滤波器收敛，则 $P_{i+1}$ 应该先按照卡尔曼公式进行更新，然后再使用切空间投影：

$P_{i+1}=(I-K_iH_i)J_iP_{\mathrm{k,pred}}J_i^\top$

$\begin{aligned} & {J}_{i+1}=\mathrm{diag}({I}_{3},{I}_{3},{J}_{\theta+1},{I}_{3},{I}_{3},{I}_{3}) \\ & {J}_{\theta+1}={I}-{\frac{1}{2}}{\delta}{\theta}_{i+1}{}^{\wedge} \\ & {\delta\theta_{i+1}}={Log}({R_{i+1}}^{{T}}{R_{0}}) \\ & {P}_{i+1}={J}_{i+1}{P}_{i+1}{J}_{i+1}^{T} \end{aligned}$

3 高维观测中的等效处理

即使用 SMV 恒等式对卡尔曼增益的公式进行变换，得：

$\begin{aligned} & AB(D+CAB)^{-1}=(A^{-1}+BD^{-1}C)^{-1}BD^{-1}, \\ & K_{i}=(P_{i}^{-1}+H_{i}^{\top}V^{-1}H_{i})^{-1}H_{i}^{\top}V^{-1}. \end{aligned}$

综上，IESKF 的更新过程的表达式变为如下形式：

$\begin{aligned} & K_{i}=(P_{i}^{-1}+H_{i}^{\top}V^{-1}H_{i})^{-1}H_{i}^{\top}V^{-1}, \\ & \delta x_{i}=K_{i}(z-h(x_{i})). \end{aligned}$

滤波器收敛时， $P_{i+1}$ 的卡尔曼更新公式变为：

$P_{i+1}=(I-(P_{i}^{-1}+H_{i}^{\top}V^{-1}H_{i})^{-1}H_{i}^{\top}V^{-1}H_{i})J_{i}P_{\mathrm{k,pred}}J_{i}^{\top},$

下面介绍一个更加方便的表达方式。设一中间变量 $\mathrm{Temp_{i}}$ ，其计算公式如下所示：

$\mathrm{Temp_{i}}=({P_{i}}^{-1}+{H_{i}}^{\mathrm{T}}{V^{-1}H_{i}})^{-1}$

则 IESKF 的更新过程的表达式变为如下形式：

$\begin{aligned} & {K_{i}}=({P_{i}}^{-1}+{H_{i}}^{\mathrm{T}}{V^{-1}}{H_{i}})^{-1}{H_{i}}^{\mathrm{T}}{V^{-1}} =\mathrm{Temp_{i}}^{*}{H_{i}}^{\mathrm{T}}{V}^{-1} \\ & \delta{x}_{\mathrm{i}}={K}_{\mathrm{i}}({z}-{h}({x}_{\mathrm{i}}))={K}_{\mathrm{i}}*{r}_{\mathrm{i}} =\mathrm{Temp_{i}}^{*}{H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}} \end{aligned}$

滤波器收敛时， $P_{i+1}$ 的卡尔曼更新公式变为如下形式：

${P}_{i+1}=({I}-\mathrm{Temp}_{\mathrm{i}}{H}_{\mathrm{i}}^{\mathrm{T}}{V}^{-1}{H}_{\mathrm{i}})*{P}_{\mathrm{i}}$

4 NDT 和卡尔曼滤波的联系

先给出结论：紧耦合 LIO 系统看成带 IMU 预测的高维 NDT 或 ICP，并且这些预测分布还会被推导至下一时刻。

式(7.15)左侧矩阵求逆之后得到 $[\sum_i(J_i^\top\Sigma_i^{-1}J_i)]^{-1}$ ，就和式(8.11)中没有预测的卡尔曼增益 $K_{k}=(H_{k}^{\top}V^{-1}H_{k})^{-1}H_{k}^{\top}V^{-1}$ 一致了。只是通常的卡尔曼增益写成了矩阵形式，而 ICP 或 NDT 写成了求和形式。为了方便后文介绍 NDT LIO，我们来推导将 NDT 误差写入卡尔曼增益的形式。并且，在实验部分，我们也会参考这里的推导方式，使用求和形式的卡尔曼增益。

没有预测的卡尔曼增益公式：当没有预测时，相当于忽略了预测误差协方差 $P_k$ ，直接对观测误差进行加权修正，因此去掉 $P_{k}^{-1}$ ，公式变为 $K_{k}=(H_{k}^{\top}V^{-1}H_{k})^{-1}H_{k}^{\top}V^{-1}$ 。

注意：这里点云中的第 $j$ 个点 $p_j$ 经过 IESKF 的预测位姿 $T_i$ （ $R_i,p_i$ ）的转换后，会落在目标点云中的某一个体素内，假设这个体素的正态分布参数为 $\mu_k,\Sigma_k$ 。此时，该点的残差 $r_j$ 为转换后的点的坐标和体素中的正态分布参数中的均值之差，即 $r_j = T_i p_j -\mu_k$ 。这个点产生的平方误差为 $e_j$ ，即 $e_j=r_j^\top\Sigma_k^{-1}r_j$ 。即：

$\begin{aligned} & \end{aligned}$ $\begin{aligned} &r_j = T_i p_j -\mu_k \\& e_j=r_j^\top\Sigma_k^{-1}r_j \end{aligned}$

推导出以上关系后，在当前第 $i$ 次迭代的过程中，我们可以向增量 NDT 里程计传入 IESKF 的预测位姿 $R_i,p_i$ ，在 NDT 内部计算点云残差 ${H_{i}}^{\mathrm{T}}{V^{-1}H_{i}}$ （ $\sum_jJ_j^\top\Sigma_k^{-1}J_j$ ）和 ${H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}}$ （ $\sum_jJ_j^\top\Sigma_k^{-1}r_j$ ），计算完成后将这两个表示点云残差的值传递到 IESKF 中，结合预测协方差矩阵 $P_i$ 计算得到当前迭代过程的增量 $\delta x_{i}$ ，最后将增量代入名义状态变量 $x_{i+1}=x_i +\delta x_{i}$ ，进而得到下一次迭代的 $x_{i+1}$ 和 $P_{i+1}$ 。

IESKF 的更新过程的流程图如下所示：

5 紧耦合 LIO 系统的主要流程

5.1 IMU 静止初始化

紧耦合 LioIEKF 类持有一个 IncNdt3d（增量 NDT，与松耦合不同）对象，一个 ESKF 对象，一个 MessageSync 对象处理同步之后的点云和 IMU。该类处理流程非常简单：当 MeasureGroup 到达后，在 IMU 未初始化时，使用第 3 章的静止初始化来估计 IMU 零偏。初始化完毕后，先使用 IMU 数据进行预测，再用预测数据对点云去畸变，最后对去畸变的点云做配准。

void LioIEKF::ProcessMeasurements(const MeasureGroup &meas) {LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();measures_ = meas;if (imu_need_init_) {// 初始化IMU系统TryInitIMU();return;}// 利用IMU数据进行状态预测Predict();// 对点云去畸变Undistort();// 配准Align();
}

IMU 静止初始化结果如下：

I0113 20:08:47.763998 403914 lio_iekf.cc:44] call meas, imu: 10, lidar pts: 3601
I0113 20:08:47.764031 403914 static_imu_init.cc:86] mean acce: -0.00215149 00.00016898 000.0978879
I0113 20:08:47.764093 403914 static_imu_init.cc:109] IMU 初始化成功，初始化时间= 9.99018, bg = -0.00259592 00.00176906 0.000707638, ba = 000.213411 -0.0167615 00-9.70973, gyro sq = 5.96793e-05 4.42613e-05 3.58264e-05, acce sq = 9.71749e-07 1.85436e-06 2.14871e-07, grav = 000.215562 -0.0169305 00-9.80762, norm: 9.81
I0113 20:08:47.764106 403914 static_imu_init.cc:113] mean gyro: -0.00259592 00.00176906 0.000707638 acce: 000.213411 -0.0167615 00-9.70973
imu try init true time:1547714610.30704498
I0113 20:08:47.764122 403914 lio_iekf.cc:149] IMU初始化成功

5.2 ESKF 之运动过程——使用 IMU 预测

IMU 的静止初始化与《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航中介绍的大体一致。当 MeasureGroup 到达后，在 IMU 未初始化时，调用 StaticIMUInit::AddIMU() 函数进行 IMU的静止初始化。当 IMU 初始化成功时，在当前 MeasureGroup 中完成 ESKF 中 Q, V, b_g, b_a, g_w, P 的初始化。

void LioIEKF::TryInitIMU() {for (auto imu : measures_.imu_) {imu_init_.AddIMU(*imu);}if (imu_init_.InitSuccess()) {// 读取初始零偏，设置ESKFsad::IESKFD::Options options;// 噪声由初始化器估计options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);ieskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());imu_need_init_ = false;LOG(INFO) << "IMU初始化成功";}
}

注意：这里有一个小地方和松耦合 LIO 不同，即协方差矩阵 P 的初始化，更加细节一些。

ESKF 协方差矩阵初始化

    void ESKF::SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,const VecT& gravity = VecT(0, 0, -9.8)) {BuildNoise(options);options_ = options;bg_ = init_bg;ba_ = init_ba;g_ = gravity;cov_ = Mat18T::Identity() * 1e-4; // P}

IESKF 协方差矩阵初始化（在 $R$ 上进行了额外处理）

    /// 设置初始条件void IESKF::SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,const VecT& gravity = VecT(0, 0, -9.8)) {BuildNoise(options);options_ = options;bg_ = init_bg;ba_ = init_ba;g_ = gravity;cov_ = 1e-4 * Mat18T::Identity();// 设置 R 部分的协方差矩阵cov_.template block<3, 3>(6, 6) = 0.1 * math::kDEG2RAD * Mat3T::Identity();}

5.3 使用 IMU 预测位姿进行运动补偿

和《自动驾驶与机器人中的SLAM技术》ch7：基于 ESKF 的松耦合 LIO 系统中一模一样，不在介绍。

5.4 松耦合系统的配准部分

得到去畸变的点云后，将其作为 source 部分传递给增量 NDT 类 IncNdt3d ，然后开始滤波器的更新过程。在滤波器更新过程的第 $i$ 次迭代过程中，首先调用IncNdt3d::ComputeResidualAndJacobians() 计算函数在 NDT 内部使用滤波器预测得到的先验位姿（首次）和更新后位姿（后续迭代）的计算点云残差 ${H_{i}}^{\mathrm{T}}{V^{-1}H_{i}}$ 和 ${H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}}$ （和松耦合中不同，没有使用增量 NDT 中的 IncNdt3d::AlignNdt() 配准函数迭代优化位姿）。然后将这两个表示点云残差的值传递到 IESKF 中，结合预测协方差矩阵 $P_i$ 计算得到当前迭代过程的增量 $\delta x_{i}$ ，最后将增量代入名义状态变量 $x_{i+1}=x_i +\delta x_{i}$ ，进而得到下一次迭代的 $x_{i+1}$ 和 $P_{i+1}$ 直到滤波器收敛。滤波器收敛后再根据卡尔曼公式计算得到后验位姿作为当前雷达 scan 的位姿。最后根据当前雷达 scan 的位姿判断 scan 是否为关键帧，若为关键帧则添加到 local map中。在这个过程中滤波器部分和 NDT 部分是耦合的，是将点云残差写入到了滤波器的观测过程中。

IncNdt3d::AlignNdt() 配准函数：将 IESKF 的预测的先验位姿 $R_i,p_i$ 作为初始值，在 NDT 内部进行配准操作，迭代得到优化后位姿信息。

配准函数中迭代遍历当前雷达扫描 scan 中的点，计算每个点的平方误差 $e_j$ 和雅可比矩阵 $J_j$ ，根据 $\sum_j(J_j^\top\Sigma_k^{-1}J_j)\Delta x=-\sum_jJ_j^\top\Sigma_k^{-1}e_j$ 计算得到 $\Delta x$ 从而迭代更新位姿信息。

ncNdt3d::ComputeResidualAndJacobians() 计算函数：在当前第 $i$ 次迭代的过程中，根据 IESKF 的预测的先验位姿 $R_i,p_i$ ，在 NDT 内部计算 ${H_{i}}^{\mathrm{T}}{V^{-1}H_{i}}$ （ $\sum_jJ_j^\top\Sigma_k^{-1}J_j$ ）和 ${H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}}$ （ $\sum_jJ_j^\top\Sigma_k^{-1}r_j$ ）。

计算函数不迭代，遍历当前雷达扫描 scan 中的点，计算每个点的平方误差 $e_j$ 和雅可比矩阵 $J_j$ ，根据 ${H_{i}}^{\mathrm{T}}{V^{-1}H_{i}}=\sum_jJ_j^\top\Sigma_k^{-1}J_j$ 和 ${H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}}=\sum_jJ_j^\top\Sigma_k^{-1}r_j$ 在 NDT 内部计算 ${H_{i}}^{\mathrm{T}}{V^{-1}H_{i}}$ 和 ${H_{i}}^{\mathrm{T}}{V}^{-1}{r_{i}}$ 。

由于 NDT 点数要明显多于预测方程，这可能导致估计结果向 NDT 倾斜，我们给这里的信息矩阵 $\Sigma^{-1}$ 添加一个乘积因子（取 0.01），降低其权重，让更新部分更加平衡一些。

bool IncNdt3d::AlignNdt(SE3& init_pose) {LOG(INFO) << "aligning with inc ndt, pts: " << source_->size() << ", grids: " << grids_.size();assert(grids_.empty() == false);SE3 pose = init_pose;// 对点的索引，预先生成int num_residual_per_point = 1;if (options_.nearby_type_ == NearbyType::NEARBY6) {num_residual_per_point = 7;}std::vector<int> index(source_->points.size());for (int i = 0; i < index.size(); ++i) {index[i] = i;}// 我们来写一些并发代码int total_size = index.size() * num_residual_per_point;for (int iter = 0; iter < options_.max_iteration_; ++iter) {std::vector<bool> effect_pts(total_size, false);std::vector<Eigen::Matrix<double, 3, 6>> jacobians(total_size);std::vector<Vec3d> errors(total_size);std::vector<Mat3d> infos(total_size);// gauss-newton 迭代// 最近邻，可以并发std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {auto q = ToVec3d(source_->points[idx]);Vec3d qs = pose * q;  // 转换之后的q, map 坐标系下的点// 计算qs所在的栅格以及它的最近邻栅格Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));for (int i = 0; i < nearby_grids_.size(); ++i) {Vec3i real_key = key + nearby_grids_[i];// 和 local map 产生联系auto it = grids_.find(real_key);int real_idx = idx * num_residual_per_point + i;/// 这里要检查高斯分布是否已经估计if (it != grids_.end() && it->second->second.ndt_estimated_) { // 找到了并且高斯分布是否已经估计auto& v = it->second->second;  // voxel，即 VoxelData 结构Vec3d e = qs - v.mu_; // 残差项// check chi2 thdouble res = e.transpose() * v.info_ * e; // 平方误差项if (std::isnan(res) || res > options_.res_outlier_th_) {effect_pts[real_idx] = false;continue;}// P259, (式 7.16)// build residualEigen::Matrix<double, 3, 6> J;J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q);J.block<3, 3>(0, 3) = Mat3d::Identity();jacobians[real_idx] = J;errors[real_idx] = e;infos[real_idx] = v.info_; // VoxelData 中的协方差矩阵之逆effect_pts[real_idx] = true;} else {effect_pts[real_idx] = false;}}});// 累加Hessian和error,计算dxdouble total_res = 0;int effective_num = 0;Mat6d H = Mat6d::Zero();Vec6d err = Vec6d::Zero();for (int idx = 0; idx < effect_pts.size(); ++idx) {if (!effect_pts[idx]) {continue;}total_res += errors[idx].transpose() * infos[idx] * errors[idx];effective_num++;H += jacobians[idx].transpose() * infos[idx] * jacobians[idx];err += -jacobians[idx].transpose() * infos[idx] * errors[idx];}if (effective_num < options_.min_effective_pts_) {LOG(WARNING) << "effective num too small: " << effective_num;init_pose = pose;return false;}Vec6d dx = H.inverse() * err;pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); // 右乘更新pose.translation() += dx.tail<3>();// 更新LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num<< ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm()<< ", dx: " << dx.transpose();if (dx.norm() < options_.eps_) {LOG(INFO) << "converged, dx = " << dx.transpose();break;}}init_pose = pose;return true;
}

void IncNdt3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) {assert(grids_.empty() == false);SE3 pose = input_pose;// 大部分流程和前面的 AlignNdt()函数 是一样的，只是会把z, H, R三者抛出去而非自己处理int num_residual_per_point = 1;if (options_.nearby_type_ == NearbyType::NEARBY6) {num_residual_per_point = 7;}std::vector<int> index(source_->points.size());for (int i = 0; i < index.size(); ++i) {index[i] = i;}int total_size = index.size() * num_residual_per_point;std::vector<bool> effect_pts(total_size, false);std::vector<Eigen::Matrix<double, 3, 18>> jacobians(total_size);std::vector<Vec3d> errors(total_size);std::vector<Mat3d> infos(total_size);// gauss-newton 迭代// 最近邻，可以并发std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {auto q = ToVec3d(source_->points[idx]);Vec3d qs = pose * q;  // 转换之后的q// 计算qs所在的栅格以及它的最近邻栅格Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));for (int i = 0; i < nearby_grids_.size(); ++i) {Vec3i real_key = key + nearby_grids_[i];auto it = grids_.find(real_key);int real_idx = idx * num_residual_per_point + i;/// 这里要检查高斯分布是否已经估计if (it != grids_.end() && it->second->second.ndt_estimated_) {auto& v = it->second->second;  // voxel，即 VoxelData 结构Vec3d e = qs - v.mu_; // 残差项// check chi2 thdouble res = e.transpose() * v.info_ * e; // 平方误差项if (std::isnan(res) || res > options_.res_outlier_th_) {effect_pts[real_idx] = false;continue;}// build residualEigen::Matrix<double, 3, 18> J;J.setZero();J.block<3, 3>(0, 0) = Mat3d::Identity();                   // 对pJ.block<3, 3>(0, 6) = -pose.so3().matrix() * SO3::hat(q);  // 对Rjacobians[real_idx] = J;errors[real_idx] = e;infos[real_idx] = v.info_; // VoxelData 中的协方差矩阵之逆effect_pts[real_idx] = true;} else {effect_pts[real_idx] = false;}}});// 累加Hessian和error,计算dxdouble total_res = 0;int effective_num = 0;HTVH.setZero();HTVr.setZero();// 乘积因子const double info_ratio = 0.01;  // 每个点反馈的info因子for (int idx = 0; idx < effect_pts.size(); ++idx) {if (!effect_pts[idx]) {continue;}total_res += errors[idx].transpose() * infos[idx] * errors[idx];effective_num++;// p314 (式8.18) （矩阵维度为18 * 18）HTVH += jacobians[idx].transpose() * infos[idx] * jacobians[idx] * info_ratio;// p314 (式8.20) （矩阵维度为18 * 1）HTVr += -jacobians[idx].transpose() * infos[idx] * errors[idx] * info_ratio;}LOG(INFO) << "effective: " << effective_num;
}