1.构建ikd-tree

点云中每个点(世界坐标系)构建ikd-tree

// 初始化 ikd-tree
if (ikdtree.Root_Node == nullptr)
{
    if (feats_down_size > 5)
    {
        // 设置 KD 树的降采样参数
        ikdtree.set_downsample_param(filter_size_map_min);

        // 去畸变点云坐标(lidar)转到世界坐标系
        feats_down_world->resize(feats_down_size);
        for (int i = 0; i < feats_down_size; i++)
        {
            pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
        }

        // 使用世界坐标系的点构建ikd-tree
        ikdtree.Build(feats_down_world->points);
    }
    continue;
}

// 获取ikd-tree中的有效节点数，无效点就是被打了deleted标签的点
int featsFromMapNum = ikdtree.validnum();

// 获取ikd-tree中的节点数
kdtree_size_st = ikdtree.size();

2.迭代卡尔曼滤波更新，对应论文公式14、15

同样这部分原代码还是不好理解，参考简化代码的这个函数

void update_iterated_dyn_share_modified(double R, PointCloudXYZI::Ptr &feats_down_body,
										KD_TREE<PointType> &ikdtree, vector<PointVector> &Nearest_Points,
										int maximum_iter, bool extrinsic_est)
{
	normvec->resize(int(feats_down_body->points.size()));

	dyn_share_datastruct dyn_share;
	dyn_share.valid = true;
	dyn_share.converge = true;
	int t = 0;

	// 这里的x_和P_分别是经过正向传播后的状态量和协方差矩阵
	state_ikfom x_propagated = x_; 
	cov P_propagated = P_;

	vectorized_state dx_new = vectorized_state::Zero(); // 24x1的向量

	for (int i = -1; i < maximum_iter; i++) // maximum_iter是卡尔曼滤波的最大迭代次数
	{
		dyn_share.valid = true;

		// 计算雅克比，也就是点面残差的导数 H(代码里是h_x)
		h_share_model(dyn_share, feats_down_body, ikdtree, Nearest_Points, extrinsic_est);

		if (!dyn_share.valid)
		{
			continue;
		}

		vectorized_state dx;
		// 公式(14)中的 x^k - x^ (广义减法)
		dx_new = boxminus(x_, x_propagated); 

		// m x 12 的矩阵
		auto H = dyn_share.h_x;												

		Eigen::Matrix<double, 24, 24> HTH = Matrix<double, 24, 24>::Zero(); // 矩阵 H^T * H
		HTH.block<12, 12>(0, 0) = H.transpose() * H;

		// 论文公式(14) 卡尔曼增益的计算
		auto K_front = (HTH / R + P_.inverse()).inverse();

		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> K;
		K = K_front.block<24, 12>(0, 0) * H.transpose() / R;

		// 矩阵 K * H
		Eigen::Matrix<double, 24, 24> KH = Matrix<double, 24, 24>::Zero(); 
		KH.block<24, 12>(0, 0) = K * H;

		// 公式(14)
		Matrix<double, 24, 1> dx_ = K * dyn_share.h + (KH - Matrix<double, 24, 24>::Identity()) * dx_new; 
		x_ = boxplus(x_, dx_); 

		dyn_share.converge = true;
		for (int j = 0; j < 24; j++)
		{
			if (std::fabs(dx_[j]) > epsi) // 如果dx>epsi 认为没有收敛
			{
				dyn_share.converge = false;
				break;
			}
		}

		if (dyn_share.converge)
			t++;

		if (!t && i == maximum_iter - 2) // 如果迭代了3次还没收敛 强制令成true，h_share_model函数中会重新寻找近邻点
		{
			dyn_share.converge = true;
		}

		if (t > 1 || i == maximum_iter - 1)
		{
			P_ = (Matrix<double, 24, 24>::Identity() - KH) * P_; // 公式(15)
			return;
		}
	}
}

3.迭代卡尔曼滤波更新后，得到了最新的状态量和先验误差的协方差矩阵。

4.发布话题

发布里程计话题：根据最新状态量的旋转R和平移获取数据，构成位姿；

发布路径话题；发布去畸变点云(点是世界坐标系)；发布去畸变点云(点是IMU坐标系)

5.当前点转到世界坐标系，插入ikd-tree  map_incremental()

注：省略了 ikd-tree底层代码的阅读，后续有时间再看。