算法4.融合算法

输入：相机前端特征点集合 $S=\left(k{p}_1,k{p}_2,\cdots ,k{p}_n\right)$ ；激光雷达点云簇 $C_{ij}=\left(C_1,C_2,\cdots ,C_k\right)$ ，i ,j代表该物体被相邻两帧标记的序号。

输出：机器人融合位姿 $T^{*}$

1)for each points cluster C in $C_{ij}$

2)judge whether C is dynamic object

3) if C is dynamic object

4) for each key point in S

5) if $k{p}_i$ in C

6) eliminate $k{p}_i$ in S

7) endif

8) endfor

9) endif

10)endfor

11)UpdatePose by new key points S