《自动驾驶与机器人中的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_0P_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;
}

        参考

        自动驾驶与机器人中的SLAM技术--第八章--紧耦合LIO系统 

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/diannao/66996.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

HTML实战课堂之简单的拜年程序

一、目录&#xff1a; &#xfffc;&#xfffc; 一、目录&#xff1a; 二、祝福 三&#xff1a;代码讲解 &#xff08;1&#xff09;详细解释&#xff1a; 1.HTML部分 2. CSS部分 三、运行效果&#xff08;随机截图&#xff09;&#xff1a; 四、完整代码&#xff1a; 二、祝福…

vue 与 vue-json-viewer 实现 JSON 数据可视化

前言 接口的调试和测试是确保系统稳定性的重要步骤。为了让开发人员和测试人员能够直观地查看接口返回的 JSON 数据&#xff0c;使用合适的工具至关重要。vue-json-viewer 插件为 vue 开发者提供了一个简单而强大的解决方案。本文将详细介绍如何在 vue 项目中使用该插件&#x…

用Pygame Zero 画矩形(空心、实心、多个矩形、多层同心矩形、彩虹条矩形、条纹相间、随机颜色矩形、特殊效果、渐变效果)

用Pygame Zero 画矩形 &#xff08;空心、实心、多个矩形、多层同心矩形、彩虹条矩形、条纹相间、随机颜色矩形、特殊效果、渐变效果&#xff09; 本文目录&#xff1a; 零、时光宝盒 一、绘制空心矩形 二、绘制实心矩形 三、画多个静止矩形 四、绘制多层同心矩形 4.1、…

【Rust自学】11.9. 单元测试

喜欢的话别忘了点赞、收藏加关注哦&#xff0c;对接下来的教程有兴趣的可以关注专栏。谢谢喵&#xff01;(&#xff65;ω&#xff65;) 11.9.1. 测试的分类 Rust把测试分为两类&#xff0c;一个是单元测试&#xff0c;一个是集成测试。 单元测试比较小也比较专注&#xff…

[java基础-集合篇]优先队列PriorityQueue结构与源码解析

优先队列PriorityQueue 优先级队列表示为平衡二进制堆&#xff1a; queue[n] 的两个子级是 queue[2*n1] 和 queue[2*&#xff08;n1&#xff09;]。 注&#xff1a;左子节点index2*parentIndex1,右子节点index2*parentIndex2,源码中计算parent位置时就是这样反过来计算的 优…

回归预测 | MATLAB实RVM-Adaboost相关向量机集成学习多输入单输出回归预测

回归预测 | MATLAB实RVM-Adaboost相关向量机集成学习多输入单输出回归预测 目录 回归预测 | MATLAB实RVM-Adaboost相关向量机集成学习多输入单输出回归预测预测效果基本介绍程序设计参考资料 预测效果 基本介绍 RVM-Adaboost相关向量机集成学习多输入单输出回归预测是一种先进…

Linux(Centos7)安装Mysql/Redis/MinIO

安装Mysql 安装Redis 搜索Redis最先版本所在的在线安装yum库 查看以上两个组件是否是开机自启 安装MinIO 开源的对象存储服务&#xff0c;存储非结构化数据&#xff0c;兼容亚马逊S3协议。 minio --help #查询命令帮助minio --server --help #查询--server帮助minio serve…

MySQL批量修改数据表编码及字符集为utf8mb4

​​​​​​MySQL批量修改数据表编码及字符集为utf8mb4 utf8mb4编码是utf8编码的超集&#xff0c;兼容utf8&#xff0c;并且能存储4字节的表情字符。 采用utf8mb4编码的好处是&#xff1a;存储与获取数据的时候&#xff0c;不用再考虑表情字符的编码与解码问题。 更改数据库…

Edge浏览器网页设置深色模式/暗模式

文章目录 需求分析1. 浏览器中的设置——外观——深色。2. 在Edge浏览器的地址栏如下网址&#xff1a;edge://flags/&#xff0c;直接搜索Dark则有内容弹出&#xff0c;将Default更改为Enable即可设置成功。3. 成果 需求 长期对着电脑屏幕&#xff0c;白色实在太刺眼&#xff…

TypeScript Jest 单元测试 搭建

NPM TypeScript 项目搭建 创建目录 mkdir mockprojectcd mockproject初始化NPM项目 npm init -y安装TypeScript npm i -D typescript使用VSCode 打开项目 创建TS配置文件tsconfig.json {"compilerOptions": {"target": "es5","module&…

基于FPGA的多功能数字钟设计

基于FPGA的多功能数字钟设计 前言基础知识按键数码管 系统概述按键使用说明模块描述模块设计button_debouncebutton_controllerclock_controllerdigital_tube 整体资源用量测试视频 前言 本工程主要是数码管、按键、LED的应用开发。 注&#xff1a;本工程所有IP均使用源码开发…

利用开源AI智能名片2+1链动模式S2B2C商城小程序拓展社交电商的深度实践探索

摘要&#xff1a;在数字化浪潮席卷全球的今天&#xff0c;社交电商作为一种新兴的商业模式&#xff0c;正以前所未有的速度改变着消费者的购物习惯与商家的营销策略。本文深入探讨了开源AI智能名片21链动模式S2B2C商城小程序在社交电商领域的应用&#xff0c;通过分析其核心机制…

【优选算法篇】:模拟算法的力量--解决复杂问题的新视角

✨感谢您阅读本篇文章&#xff0c;文章内容是个人学习笔记的整理&#xff0c;如果哪里有误的话还请您指正噢✨ ✨ 个人主页&#xff1a;余辉zmh–CSDN博客 ✨ 文章所属专栏&#xff1a;优选算法篇–CSDN博客 文章目录 一.模拟算法二.例题1.替换所有的问号2.提莫攻击3.外观数列4…

2_CSS3 背景 --[CSS3 进阶之路]

CSS3 中的背景属性提供了许多强大的功能来增强网页设计&#xff0c;包括但不限于多背景图像、渐变、背景大小控制等。以下是一些关键的 CSS3 背景属性及其用法示例。 1. 多重背景图像 CSS3 允许你为一个元素设置多个背景图像。这些图像按照它们在 background-image 属性中定义…

蓝桥杯备考:数据结构之栈 和 stack

目录 栈的概念以及栈的实现 STL 的stack 栈和stack的算法题 栈的模板题 栈的算法题之有效的括号 验证栈序列 后缀表达式 括号匹配 栈的概念以及栈的实现 栈是一种只允许在一端进行插入和删除的线性表 空栈&#xff1a;没有任何元素 入栈&#xff1a;插入元素消息 出…

C++中的条件变量(condition_variable)详解:小白版

在编程中&#xff0c;我们经常需要处理多个任务&#xff0c;这些任务可能需要同时运行&#xff0c;也可能需要按照一定的顺序运行。这就涉及到了线程的概念。线程就像是一个小程序&#xff0c;它可以在程序中独立运行&#xff0c;而且可以和其他线程并行执行。 但是&#xff0…

【python】OpenCV—Local Translation Warps

文章目录 1、功能描述2、原理分析3、代码实现4、效果展示5、完整代码6、参考 1、功能描述 利用液化效果实现瘦脸美颜 交互式的液化效果原理来自 Gustafsson A. Interactive image warping[D]. , 1993. 2、原理分析 上面描述很清晰了&#xff0c;鼠标初始在 C&#xff0c;也即…

STM32标准库学习笔记(十)SPI

前言 学习永无止境&#xff01;本篇是嵌入式开发之片上外设SPI&#xff0c;了解基本硬件原理以及通信协议。 注&#xff1a;本文章为学习笔记&#xff0c;部分图片与文字来源于网络/江协科技课程/手册&#xff0c;如侵权请联系&#xff01;谢谢&#xff01; 一、SPI通信概述 1.…

从github上,下载的android项目,从0-1进行编译运行-踩坑精力,如何进行部署

因为国内的网络原因&#xff0c;一直在anroidstudio开发的问题上&#xff0c;是个每个开发者都会踩坑 一直以为是自己的原因&#xff0c;其实很多都是国内网络的原因&#xff0c;今天就从一个开发者的视角 把从github上一个陌生的项目&#xff0c;如何通过本地就行运行的 首先…

计算机网络 (40)域名系统DNS

前言 计算机网络域名系统DNS&#xff08;Domain Name System&#xff09;是互联网的基础技术之一&#xff0c;它负责将人类可读的域名转换为计算机用来通信的数字IP地址。 一、基本概念 DNS的主要目的是将域名解析或翻译为IP地址&#xff0c;使得用户可以通过简单易记的域名来访…