Catalogue
Kalman Filters on Differentiable Manifolds
IkFoM在Fast-LIO2中的应用
Fast-LIO2中，主要使用了IkFoM作为状态，其中，在use-ikfom.hpp声明了关于各种状态、数据的宏定义
1 |
|
其中，MTK_BUILD_MANIFOLD宏定义是引用自build_manifold.hpp文件，即mtk文件夹内的内容，mtk文件夹原版是来自(OpenSLAM-MTK)[https://github.com/OpenSLAM-org/openslam_MTK]，并且经过了一定的修改和适配。
MTK_BUILD_MANIFOLD干了啥
MTK_BUILD_MANIFOLD是由BOOST宏模板编程技术写的宏。
1 |
|
#ifndef PARSED_BY_DOXYGEN表示接下来的代码，将不是由 DOXYGEN 工具来解析，而是由C++预处理来处理build_manifold.hpp文件使用了大量的Boost库的预处理(PP: preprocessor)模板元库技术，直接看代码比较复杂，所以这里反过来看，即直接对宏展开，然后往回看。
【方法一】使用IDE展开宏定义
以input_ikfom的定义代码块为例：
1 | MTK_BUILD_MANIFOLD(input_ikfom, |
直接使用CLion展开是最快的，把鼠标移到附近，等待解析完成，显示如下：
展开结果如下：
1 | struct input_ikfom { |
【方法二】编写代码展开宏定义并打印
新建test文件夹，添加test_ikfom.cpp文件
1 |
|
其中，__VA_ARGS__是可变参数宏标识符，#define STR(...)里面的(...)表示可以传递多个参数。
需要注意的是：
- #运算符 把参数字符串化，功能就是转为字符串
为什么需要用两个STR宏，是因为：
STR(...)将MTK_BUILD_MANIFOLD(xxxx)这一串字符传递给__VA_ARGS__变量，如果此时就调用#__VA_ARGS__进行打印，那么输出的仍然是MTK_BUILD_MANIFOLD(xxxx)，为了展开，需要将__VA_ARGS__的值（也就是MTK_BUILD_MANIFOLD(xxxx)的值）进行传递，所以有了STR_(...)，此时MTK_BUILD_MANIFOLD(xxxx)的具体值就会传递给STR_(...)的__VA_ARGS__，这个时候再打印，就是完整的宏定义展开了。
运行此CPP，得到输出结果如下：
1 | struct input_ikfom { typedef input_ikfom self; std::vector<std::pair<int, int> > S2_state; std::vector<std::pair<int, int> > SO3_state; std::vector<std::pair<std::pair<int, int>, int> > vect_state; MTK::SubManifold<vect3, 0, 0> acc; MTK::SubManifold<vect3, 0 + vect3::DOF, 0 + vect3::DIM> gyro; enum {DOF = vect3::DOF + 0 + vect3::DOF}; enum {DIM = vect3::DIM+0 + vect3::DIM}; typedef vect3::scalar scalar; input_ikfom ( const vect3& acc = vect3(), const vect3& gyro = vect3() ) : acc(acc), gyro(gyro) {} int getDOF() const { return DOF; } void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { acc.boxplus(MTK::subvector(__vec, &self::acc), __scale); gyro.boxplus(MTK::subvector(__vec, &self::gyro), __scale); } void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { acc.oplus(MTK::subvector_(__vec, &self::acc), __scale); gyro.oplus(MTK::subvector_(__vec, &self::gyro), __scale); } void boxminus(MTK::vectview<scalar,DOF> __res, const input_ikfom& __oth) const { acc.boxminus(MTK::subvector(__res, &self::acc), __oth.acc); gyro.boxminus(MTK::subvector(__res, &self::gyro), __oth.gyro); } friend std::ostream& operator<<(std::ostream& __os, const input_ikfom& __var){ return __os << __var.acc << " " << __var.gyro << " " ; } void build_S2_state(){ if(acc.TYP == 1){S2_state.push_back(std::make_pair(acc.IDX, acc.DIM));} if(gyro.TYP == 1){S2_state.push_back(std::make_pair(gyro.IDX, gyro.DIM));} } void build_vect_state(){ if(acc.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(acc.IDX, acc.DIM), vect3::DOF));} if(gyro.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(gyro.IDX, gyro.DIM), vect3::DOF));} } void build_SO3_state(){ if(acc.TYP == 2){(SO3_state).push_back(std::make_pair(acc.IDX, acc.DIM));} if(gyro.TYP == 2){(SO3_state).push_back(std::make_pair(gyro.IDX, gyro.DIM));} } void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) { if(acc.IDX == idx){acc.S2_hat(res);} if(gyro.IDX == idx){gyro.S2_hat(res);} } void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) { if(acc.IDX == idx){acc.S2_Nx_yy(res);} if(gyro.IDX == idx){gyro.S2_Nx_yy(res);} } void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) { if(acc.IDX == idx){acc.S2_Mx(res, dx);} if(gyro.IDX == idx){gyro.S2_Mx(res, dx);} } friend std::istream& operator>>(std::istream& __is, input_ikfom& __var){ return __is >> __var.acc >> __var.gyro ; } }; |
重新格式化后，得到：
1 | struct input_ikfom { |
两种方法得到的结果，是一致的。
MTK_BUILD_MANIFOLD细读
有了输入、输出，这个时候结合MTK_BUILD_MANIFOLD的代码来分析：
1 |
|
把name = input_ikfom代入上面，得到：
1 |
|
接下来，还有
MTK_SUBVARLIST,MTK_TRANSFORM_COMMA,MTK_TRANSFORM,MTK_CONSTRUCTOR_ARG,MTK_CONSTRUCTOR_COPY,MTK_BOXPLUS,MTK_OSTREAM,MTK_S2_state,MTK_vect_state,MTK_SO3_state,MTK_S2_hat,MTK_S2_Nx_yy,MTK_S2_Mx,MTK_ISTREAM,MTK::vectview
需要展开。
MTK_SUBVARLIST
MTK_SUBVARLIST的定义如下：
1 |
|
输入：
1 | std::vector<std::pair<int, int> > S2_state; |
输出：
1 | std::vector<std::pair<int, int> > S2_state; |
此处用了BOOST_PP_FOR_1，根据附录，BOOST_PP_FOR(state, pred, op, macro) 宏用于执行一定数量的宏展开操作
在这里，
- 循环变量的初始状态state为
1 | ( \ |
- 结束循环的条件pred为
MTK_ENTRIES_TEST - 产生新状态的操作op为
MTK_ENTRIES_NEXT - 具体输出操作macro为
MTK_ENTRIES_OUTPUT
============= 第一轮循环 ============
初始状态
原始输入:
1 | seq = ((vect3, acc)) \ |
输入初始状态：
1 | ( \ |
- BOOST_PP_SEQ_SIZE(seq) = 2
- BOOST_PP_SEQ_HEAD(seq) = (vect3, acc)
- BOOST_PP_SEQ_TAIL(seq) (~) = ((vect3, gyro))
即初始状态为
1 | ( \ |
MTK_ENTRIES_TEST
取state第0个元素，非0则继续循环，为0则结束循环
1 | //! this used to be BOOST_PP_TUPLE_ELEM_4_0: |
MTK_ENTRIES_OUTPUT
1 |
|
初始状态为
1 | ( \ |
调用:
1 | MTK_ENTRIES_OUTPUT_I (s = 2, head = (vect3, acc), seq = ((vect3, gyro)), dof = 0, dim = 0, S2state = S2state, SO3state = SO3state) |
此处，s - 1=2 - 1 > 0，所以会调用MTK_PUT_TYPE宏
1 |
|
输出：
1 | MTK::SubManifold<vect3, 0, 0> acc; |
MTK_ENTRIES_NEXT
1 |
|
初始状态为
1 | ( \ |
调用:
1 | MTK_ENTRIES_NEXT_I(len = 2, head = (vect3, acc), seq = ((vect3, gyro)), 0, 0, S2state, SO3state) |
经过op操作（MTK_ENTRIES_NEXT）之后，得到新的state：
1 | 1, \ |
============= 第二轮循环 ============
当前状态：
1 | 1, \ |
MTK_ENTRIES_TEST
第0个元素为1，继续执行
MTK_ENTRIES_OUTPUT
输出：
1 | MTK::SubManifold<vect3, 0 + vect3::DOF, 0 + vect3::DIM> gyro; |
MTK_ENTRIES_NEXT
op操作，得到下一个状态：
1 | (0,BOOST_PP_SEQ_ELEM_III,BOOST_PP_SEQ_TAIL_I,0+vect3::DOF+vect3::DOF,0+vect3::DIM+vect3::DIM,S2state,SO3state) |
============= 结束循环 ============
最终，MTK_SUBVARLIST得到结果如下：
输出：
1 | // 第一轮循环输出的 |
附录
BOOST_PP_FOR
BOOST_PP_FOR 是 Boost Preprocessor 库中的一个宏，用于实现基于循环的元编程。BOOST_PP_FOR(state, pred, op, macro) 表示泛化的for水平重复结构，它接受四个参数：
- state : 初始状态
- pred : 判断是否继续展开，形如 pred(r, state) 的二元谓词。该宏必须展开为一个位于 0 到 BOOST_PP_LIMIT_MAG 间的整数。当该谓词返回非零时，BOOST_PP_FOR 重复展开 macro
- op : 操作生成新的状态，形如 op(r, state) 的二元操作; 该宏被重复应用于 state, 每次产生一个新的 state, 直至 pred 返回 0.
- macro: 利用state生成做后的输出，形如 macro(r, state) 的二元宏;该宏被 BOOST_PP_FOR 重复调用，直至 pred 返回 0
简单来说，使用BOOST_PP_FOR，大概会展开成这个模式：
1 | macro(r, state) macro(r, op(r, state)) … macro(r, op(r, … op(r, state) … )) |
一个op操作举例如下:
1 |
|
一个macro操作举例如下：
1 |
|
对上面的操作进行组合，形成一个BOOST_PP_FOR操作：
1 | // 当state的首元素不等于state第二元素+1时，返回1，否则返回0 |
BOOST_PP运算
算术运算
1 | BOOST_PP_ADD(x,y) x + y |
整型逻辑运算
1 | BOOST_PP_AND(x,y) x && y |
位逻辑运算（单个位操作的）
1 | BOOST_PP_BITAND(x,y) x && y |
比较运算
1 | BOOST_PP_EQUAL(x,y) x == y ? 1 : 0 |
BOOST_PP序列
序列 (简称为 seq)是一组相邻的带括号的元素。例如, (a)(b)©(d) seq 不能为空。因此，一个 “空的” seq 被认为是一种特殊情况，必须由C++单独处理。 对于序列的处理效率是非常高的，可以认为是随机访问的。
BOOST_PP_SEQ_HEAD(seq) 展开为一个 seq 的第一个元素。 BOOST_PP_SEQ_TAIL(seq) 展开为一个 seq 中除了第一个元素以外的其它元素。(还是一个序列)
例如：
1 |
|
BOOST_PP_IF(cond, t, f)
类似三元运算符 cond ? t : f ;