基于 Boost Preprocessor 的 Manifold Toolkit (MTK) 通用流形类构建 —— IKFoM 中实现的源码解读

Title: 基于 Boost Preprocessor 的 Manifold Toolkit (MTK) 通用流形类构建 —— IKFoM 中实现的源码解读

I. 前言

我们之前系列解读了 IKFoM 的数学原理 (博客 I, 博客 II, 博客III, 博客 IV, 博客 V), 也对应用 IKFoM 进行建图的 Fast_LIO 的前后处理设置 进行了说明.

我们本篇对 IKFoM 的基础 —— OpenSLAM 的流形工具箱 (Manifold Toolkit, MTK) 进行部分解读. IKFoM 主要用到 MTK 进行流形构建和流形上的基础数学计算. 这里我们关注 MTK 如何实现各种流形的构建, 尤其复合流形的构建.

这里的难点是 MTK 构建流形时使用了大量 Boost Preprocessor 宏, 第一眼看上去很难理解且无从下手.

我们这里就抽丝剥茧, 逐步升入, 剖析 MTK 源码如何利用 Boost Preprocessor 宏实现通用的流形类/结构体的构建的 (即所谓的元编程 Meta-Programming).

最终我们可以徒手解开 MTK 中通用流形类/结构体构建源码 (…/mtk/build_manifold.hpp 文件) 的实现细节.

---

II. 他山之石

MTK 链接:

https://openslam-org.github.io/MTK

https://github.com/OpenSLAM-org/openslam_MTK

可能 MTK 发布年代久远的缘故更可能我自己菜的缘故, 从 github 拉下来代码后, 编译无法通过. 幸好 IKFoM 包含了已经裁剪和修改好的 MTK 代码.

IKFoM 链接:

https://github.com/hku-mars/IKFoM

我们就以这份代码为基础.

MTK 中利用 Boost 预处理宏实现了复杂了流形的统一构建.

我们首先会参考其他博主的经验, 利用宏扩展技术, 可以获得机器编译的流形类/结构体的构建结果.

网友解读宏定义的链接:

CSDN 博文: 如何解读fast-lio2中的 ‘IKFoM’ 中的宏定义

稀土掘金博文: 如何解读fast-lio2中的 ‘IKFoM’ 中的宏定义

---

III. 宏扩展

一. 宏展开基础知识

宏展开的顺序和基本规则参见博文 C/C++ Preprocessor Metaprogramming - Macro Basics/Rules.

宏展开式注意事项请参见博客文章 C宏展开的几个注意事项 (这篇博文对快速理解基础宏展开规则有帮助).

编译阶段打印宏内容参见博客文章 宏展开顺序 (主要讲了宏展开后的打印输出).

注意, 在写下面小节中宏展开程序时, 稍有不注意可能遇到 error,

"too many arguments in invocation of macro “MACRO_EXPAND_HELPER”

这里涉及参数预扫描 (Argument Prescan) 中的无保护逗号 (Unshielded Commas).

解决方案就是多套一个括号, 参考 “Argument Prescan - Macros used in arguments, whose expansions contain unshielded commas”.

---

二. 宏展开环境准备

我们按照 如何解读fast-lio2中的 ‘IKFoM’ 中的宏定义 中的方法, 先让机器解读一下复杂的宏.

Step 1. 拷贝 IKFoM -> IKFoM_toolkit -> mtk 到某独立处

Step 2. 添加 main.cpp

#include "types/vect.hpp"
#include "types/SOn.hpp"
#include "types/S2.hpp"
#include "startIdx.hpp"
#include "build_manifold.hpp"

typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2; 

// 定义要展开的各个流形
#define manifold_1 MTK_BUILD_MANIFOLD(manifold_1_state, ((vect3, acc)))                            // manifold_1
#define manifold_2 MTK_BUILD_MANIFOLD(manifold_2_state, ((vect3, acc)) ((SO3, rot)) ((S2, grav)) ) // manifold_2

// 展开宏的代码
#define MACRO_EXPAND_HELPER(x) #x                            // 将参数 x 转换为字符串后不再继续扫描展开
#define MACRO_EXPAND(x) "\n\n" #x "\n===>>>\n" MACRO_EXPAND_HELPER((x))  
// #x —— 参数转换为字符串后不再展开
// MACRO_EXPAND_HELPER((x)) —— 先对参数 x 进行 "prescan" 展开后作为参数代入 MACRO_EXPAND_HELPER(x)
// MACRO_EXPAND_HELPER((x)) 为什么两重括号？ 就是为了将展开后的逗号保护起来
// refer to "Argument Prescan - The C Preprocessor"

// 展开宏的代码的显示
#pragma message(MACRO_EXPAND(manifold_1))
#pragma message(MACRO_EXPAND(manifold_2))

int main()
{
    return 0;
}

Step 3. 添加 CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(MTK_Macro_Expansion)

ADD_COMPILE_OPTIONS(-std=c++14 )

add_executable(${CMAKE_PROJECT_NAME} main.cpp)

---

三. 典型流形的宏展开结果

执行

mkdir build
cd build
cmake ..
make

终端输出 (在线格式化后)

Manifold_1 的构建结果

manifold_1
===>>>
(struct manifold_1_state {
	typedef manifold_1_state 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;
	std::vector<std::pair<std::pair<int, int>, int> > SEN_state;
	MTK::SubManifold<vect3, 0, 0> acc;
	enum {
		DOF = vect3::DOF + 0
	}
	;
	enum {
		DIM = vect3::DIM+0
	}
	;
	typedef vect3::scalar scalar;
	manifold_1_state ( const vect3& acc = vect3() ) : acc(acc) {
	}
	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);
	}
	void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) {
		acc.oplus(MTK::subvector_(__vec, &self::acc), __scale);
	}
	void boxminus(MTK::vectview<scalar,DOF> __res, const manifold_1_state& __oth) const {
		acc.boxminus(MTK::subvector(__res, &self::acc), __oth.acc);
	}
	friend std::ostream& operator<<(std::ostream& __os, const manifold_1_state& __var) {
		return __os << __var.acc << " " ;
	}
	void build_S2_state() {
		if(acc.TYP == 1) {
			S2_state.push_back(std::make_pair(acc.IDX, acc.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));
		}
	}
	void build_SO3_state() {
		if(acc.TYP == 2) {
			(SO3_state).push_back(std::make_pair(acc.IDX, acc.DIM));
		}
	}
	void build_SEN_state() {
		if(acc.TYP == 4) {
			(SEN_state).push_back(std::make_pair(std::make_pair(acc.IDX, acc.DIM), vect3::DOF));
		}
	}
	void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.hat(vec, res);
		}
	}
	void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.Jacob_right_inv(vec, res);
		}
	}
	void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.Jacob_right(vec, res);
		}
	}
	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {
		if(acc.IDX == idx) {
			acc.S2_hat(res);
		}
	}
	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {
		if(acc.IDX == idx) {
			acc.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);
		}
	}
	friend std::istream& operator>>(std::istream& __is, manifold_1_state& __var) {
		return __is >> __var.acc ;
	}
}
;
)
   23 | #pragma message(MACRO_EXPAND(manifold_1))

Manifold_2 的构建结果

manifold_2
===>>>
(struct manifold_2_state {
	typedef manifold_2_state 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;
	std::vector<std::pair<std::pair<int, int>, int> > SEN_state;
	MTK::SubManifold<vect3, 0, 0> acc;
	MTK::SubManifold<SO3, 0 + vect3::DOF, 0 + vect3::DIM> rot;
	MTK::SubManifold<S2, 0 + vect3::DOF + SO3::DOF, 0 + vect3::DIM + SO3::DIM> grav;
	enum {
		DOF = S2::DOF + 0 + vect3::DOF + SO3::DOF
	}
	;
	enum {
		DIM = S2::DIM+0 + vect3::DIM + SO3::DIM
	}
	;
	typedef S2::scalar scalar;
	manifold_2_state ( const vect3& acc = vect3(), const SO3& rot = SO3(), const S2& grav = S2() ) : acc(acc), rot(rot), grav(grav) {
	}
	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);
		rot.boxplus(MTK::subvector(__vec, &self::rot), __scale);
		grav.boxplus(MTK::subvector(__vec, &self::grav), __scale);
	}
	void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) {
		acc.oplus(MTK::subvector_(__vec, &self::acc), __scale);
		rot.oplus(MTK::subvector_(__vec, &self::rot), __scale);
		grav.oplus(MTK::subvector_(__vec, &self::grav), __scale);
	}
	void boxminus(MTK::vectview<scalar,DOF> __res, const manifold_2_state& __oth) const {
		acc.boxminus(MTK::subvector(__res, &self::acc), __oth.acc);
		rot.boxminus(MTK::subvector(__res, &self::rot), __oth.rot);
		grav.boxminus(MTK::subvector(__res, &self::grav), __oth.grav);
	}
	friend std::ostream& operator<<(std::ostream& __os, const manifold_2_state& __var) {
		return __os << __var.acc << " " << __var.rot << " " << __var.grav << " " ;
	}
	void build_S2_state() {
		if(acc.TYP == 1) {
			S2_state.push_back(std::make_pair(acc.IDX, acc.DIM));
		}
		if(rot.TYP == 1) {
			S2_state.push_back(std::make_pair(rot.IDX, rot.DIM));
		}
		if(grav.TYP == 1) {
			S2_state.push_back(std::make_pair(grav.IDX, grav.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(rot.TYP == 0) {
			(vect_state).push_back(std::make_pair(std::make_pair(rot.IDX, rot.DIM), SO3::DOF));
		}
		if(grav.TYP == 0) {
			(vect_state).push_back(std::make_pair(std::make_pair(grav.IDX, grav.DIM), S2::DOF));
		}
	}
	void build_SO3_state() {
		if(acc.TYP == 2) {
			(SO3_state).push_back(std::make_pair(acc.IDX, acc.DIM));
		}
		if(rot.TYP == 2) {
			(SO3_state).push_back(std::make_pair(rot.IDX, rot.DIM));
		}
		if(grav.TYP == 2) {
			(SO3_state).push_back(std::make_pair(grav.IDX, grav.DIM));
		}
	}
	void build_SEN_state() {
		if(acc.TYP == 4) {
			(SEN_state).push_back(std::make_pair(std::make_pair(acc.IDX, acc.DIM), vect3::DOF));
		}
		if(rot.TYP == 4) {
			(SEN_state).push_back(std::make_pair(std::make_pair(rot.IDX, rot.DIM), SO3::DOF));
		}
		if(grav.TYP == 4) {
			(SEN_state).push_back(std::make_pair(std::make_pair(grav.IDX, grav.DIM), S2::DOF));
		}
	}
	void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.hat(vec, res);
		}
		if(rot.IDX == idx) {
			rot.hat(vec, res);
		}
		if(grav.IDX == idx) {
			grav.hat(vec, res);
		}
	}
	void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.Jacob_right_inv(vec, res);
		}
		if(rot.IDX == idx) {
			rot.Jacob_right_inv(vec, res);
		}
		if(grav.IDX == idx) {
			grav.Jacob_right_inv(vec, res);
		}
	}
	void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {
		if(acc.IDX == idx) {
			acc.Jacob_right(vec, res);
		}
		if(rot.IDX == idx) {
			rot.Jacob_right(vec, res);
		}
		if(grav.IDX == idx) {
			grav.Jacob_right(vec, res);
		}
	}
	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {
		if(acc.IDX == idx) {
			acc.S2_hat(res);
		}
		if(rot.IDX == idx) {
			rot.S2_hat(res);
		}
		if(grav.IDX == idx) {
			grav.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(rot.IDX == idx) {
			rot.S2_Nx_yy(res);
		}
		if(grav.IDX == idx) {
			grav.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(rot.IDX == idx) {
			rot.S2_Mx(res, dx);
		}
		if(grav.IDX == idx) {
			grav.S2_Mx(res, dx);
		}
	}
	friend std::istream& operator>>(std::istream& __is, manifold_2_state& __var) {
		return __is >> __var.acc >> __var.rot >> __var.grav ;
	}
}
;
)
   24 | #pragma message(MACRO_EXPAND(manifold_2))

---

IV 预处理宏的源码解读

一. Boost 预处理宏的基础知识

上一节是机器自动编译的宏扩展结果, 让我们对结果有了初步了解和预期, 但是里面的原理细节我们完全不知道.

我们最终更希望知其所以然, 需要了解 MTK 如何利用 Boost Preprocessor 库进行元编程 (Meta-Programming), 从而构建各种各样机器人运动相关流形类/结构体的.

Boost Preprocessor 参考链接:

The Boost Library Preprocessor Subset for C/C++

Appendix A - An Introduction to Preprocessor Metaprogramming

---

二. 源代码

基于 Boost 预处理宏的流形构建源代码如下:

/**
 * Construct a manifold.
 * @param name is the class-name of the manifold, 
 * @param entries is the list of sub manifolds 
 * 
 * Entries must be given in a list like this:
 * @code
 * typedef MTK::trafo<MTK::SO3<double> > Pose;
 * typedef MTK::vect<double, 3> Vec3;
 * MTK_BUILD_MANIFOLD(imu_state,
 *    ((Pose, pose))
 *    ((Vec3, vel))
 *    ((Vec3, acc_bias))
 * )
 * @endcode
 * Whitespace is optional, but the double parentheses are necessary.
 * Construction is done entirely in preprocessor.
 * After construction @a name is also a manifold. Its members can be 
 * accessed by names given in @a entries.
 * 
 * @note Variable types are not allowed to have commas, thus types like
 *       @c vect<double, 3> need to be typedef'ed ahead.
 */
#define MTK_BUILD_MANIFOLD(name, entries) \
struct name { \
	typedef name 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;\
	std::vector<std::pair<std::pair<int, int>, int> > SEN_state;\
	MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state) \
	name ( \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
		) : \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\
	int getDOF() const { return DOF; } \
	void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
		MTK_TRANSFORM(MTK_BOXPLUS, entries)\
	} \
	void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
		MTK_TRANSFORM(MTK_OPLUS, entries)\
	} \
	void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
		MTK_TRANSFORM(MTK_BOXMINUS, entries)\
	} \
	friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
		return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
	} \
	void build_S2_state(){\
		MTK_TRANSFORM(MTK_S2_state, entries)\
	}\
	void build_vect_state(){\
		MTK_TRANSFORM(MTK_vect_state, entries)\
	}\
	void build_SO3_state(){\
		MTK_TRANSFORM(MTK_SO3_state, entries)\
	}\
	void build_SEN_state(){\
		MTK_TRANSFORM(MTK_SEN_state, entries)\
	}\
	void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
		MTK_TRANSFORM(MTK_HAT, entries)\
	}\
	void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
		MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\
	}\
	void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
		MTK_TRANSFORM(MTK_JACOB_RIGHT, entries)\
	}\
	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
		MTK_TRANSFORM(MTK_S2_hat, entries)\
	}\
	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
		MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
	}\
	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
		MTK_TRANSFORM(MTK_S2_Mx, entries)\
	}\
	friend std::istream& operator>>(std::istream& __is, name& __var){ \
		return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
	} \
};

---

三. 源码解读

1. 流形结构体构建源码的整体结构

在代码注释里面, 原著者已经很清晰的说明了这个构造流形用的预处理代码的使用方法、为什么预定义类型 (也是为了逗号保护) 等, 只是调用的话看到这里也够用了.

出于好奇我们还是想对细节有更多了解, 所以我们继续 … …

我们以如下流形结构体的创建为例进行解读:

MTK_BUILD_MANIFOLD(manifold_2_state, ((vect3, acc)) ((SO3, rot)) ((S2, grav)) )

流形的 name (the class-name of the manifold) $\iff$ manifold_2_state

流形的 entries (the list of sub manifolds) $\iff$ ((vect3, acc)) ((SO3, rot)) ((S2, grav))

(type, id) $\iff$ (vect3, acc)

(type, id) $\iff$ (SO3, rot)

(type, id) $\iff$ (S2, grav)

从上一节的源码中, 初步分析可知, 流形结构体的创建主要围绕三个方面

- 通过 MTK_SUBVARLIST 的子流形构建

MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state)

- 流形结构体的构造函数

name ( \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
		) : \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}

- 流形结构体的成员函数, 如

	void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
		MTK_TRANSFORM(MTK_BOXPLUS, entries)\
	}

下面我们分别解读.

---

2. 通过 MTK_SUBVARLIST 的子流形构建

MTK_SUBVARLIST 关联的各个预定义宏的源代码如下:

#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \
BOOST_PP_FOR_1( \
		( \
				BOOST_PP_SEQ_SIZE(seq), \
				BOOST_PP_SEQ_HEAD(seq), \
				BOOST_PP_SEQ_TAIL(seq) (~), \
				0,\
				0,\
				S2state,\
				SO3state,\
				SENstate ),\
		MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)

#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
	MTK::SubManifold<type, dof, dim> id; 
#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \
	MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
	enum {DOF = type::DOF + dof}; \
	enum {DIM = type::DIM+dim}; \
	typedef type::scalar scalar; 

#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \
	MTK_APPLY_MACRO_ON_TUPLE(~, \
		BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
		( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)) 

#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state

//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a

#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \
		BOOST_PP_DEC(len), \
		BOOST_PP_SEQ_HEAD(seq), \
		BOOST_PP_SEQ_TAIL(seq), \
		dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
		dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
		S2state,\
		SO3state,\
		SENstate )

其中, seq $\iff$ entries $\iff$ ((vect3, acc)) ((SO3, rot)) ((S2, grav))

以上预处理宏的核心围绕 BOOST_PP_FOR_1 展开.

BOOST_PP_FOR_r macro 的定义

​
The BOOST_PP_FOR_r macro represents a reentry into the BOOST_PP_FOR repetition construct.

Usage

​ \qquad BOOST_PP_FOR_ ## r(state, pred, op, macro)

Arguments

r

​ \qquad The next available BOOST_PP_FOR repetition.

state

​ \qquad The initial state.

pred

​ \qquad A binary predicate of the form pred(r, state). This macro must expand to an integer in the range of 0 to BOOST_PP_LIMIT_MAG. BOOST_PP_FOR repeatedly expands macro while this predicate returns non-zero. This macro is called with the next available BOOST_PP_FOR repetition and the current state.

op

​ \qquad A binary operation of the form op(r, state). This operation is expanded by BOOST_PP_FOR with the next available BOOST_PP_FOR repetition and the current state. This macro is repeatedly applied to the state, each time producing a new state, until pred returns 0.

macro

​ \qquad A binary macro of the form macro(r, state). This macro is expanded by BOOST_PP_FOR with the next available BOOST_PP_FOR repetition and the current state. This macro is is repeated by BOOST_PP_FOR until pred returns 0.

Remarks

​ \qquad This macro expands to the sequence:

​ \qquad macro(r, state) macro(r, op(r, state)) … macro(r, op(r, … op(r, state) … ))

这里的参数对应关系为

r $\iff$ 1

state $\iff$ ( BOOST_PP_SEQ_SIZE(seq), BOOST_PP_SEQ_HEAD(seq), BOOST_PP_SEQ_TAIL(seq) (~), 0, 0, S2state, SO3state, SENstate )

pred(r, state) $\iff$ MTK_ENTRIES_TEST(r, state)

op(r, state) $\iff$ MTK_ENTRIES_NEXT(r, state)

macro(r, state) $\iff$ MTK_ENTRIES_OUTPUT(r, state)

BOOST_PP_FOR_r 会重复执行宏扩展, 直到 pred(r, state) 结果为 0 时才结束宏扩展. 我们来看一下整个宏扩展过程.

---

A. 初始步骤的宏扩展

初始步骤的 state:

BOOST_PP_SEQ_SIZE(seq) = 3

BOOST_PP_SEQ_HEAD(seq) = (vect3, acc)

BOOST_PP_SEQ_TAIL(seq) (~) = ((SO3, rot)) ((S2, grav)) (~)

state $\iff$ (3, (vect3, acc), ((SO3, rot)) ((S2, grav)) (~), 0, 0, S2state, SO3state, SENstate) (<<< 这是初始步骤进行宏扩展的状态 State)

参考

The BOOST_PP_SEQ_SIZE macro expands to the size of a seq.

The BOOST_PP_SEQ_HEAD macro expands to the first element in a seq.

The BOOST_PP_SEQ_TAIL macro expands to all but the first element of a seq.

初始步骤的 pred:

#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state

//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a

MTK_ENTRIES_TEST(r, state) $\iff$ MTK_TUPLE_ELEM_4_0 state $\iff$ MTK_TUPLE_ELEM_4_0(3, (vect3, acc), ((SO3, rot)) ((S2, grav)) (~), 0, 0, S2state, SO3state, SENstate) $\iff$ 3 (<<< 非 0, 初始步骤可以进行宏扩展)

初始步骤的 op:

#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \
		BOOST_PP_DEC(len), \
		BOOST_PP_SEQ_HEAD(seq), \
		BOOST_PP_SEQ_TAIL(seq), \
		dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
		dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
		S2state,\
		SO3state,\
		SENstate )

MTK_ENTRIES_NEXT(r, state) $\iff$ MTK_ENTRIES_NEXT_I state $\iff$ MTK_ENTRIES_NEXT_I(3, ((vect3, acc)), ((SO3, rot)) ((S2, grav)) (~), 0, 0, S2state, SO3state, SENstate)

其中

\qquad len $\iff$ 3

\qquad head $\iff$ (vect3, acc)

\qquad seq $\iff$ ((SO3, rot)) ((S2, grav)) (~)

\qquad dof $\iff$ 0

\qquad dim $\iff$ 0

那么

MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate)

$\iff$

(BOOST_PP_DEC(len), BOOST_PP_SEQ_HEAD(seq), BOOST_PP_SEQ_TAIL(seq), dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF, dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM, S2state, SO3state, SENstate)

$\iff$

(2, (SO3, rot), ((S2, grav)) (~), 0+BOOST_PP_TUPLE_ELEM_2_0(vect3, acc)::DOF, 0+BOOST_PP_TUPLE_ELEM_2_0(vect3, acc)::DIM, S2state, SO3state, SENstate)

$\iff$

(2, (SO3, rot), ((S2, grav)) (~), 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate) (<<< 这是输入到下一步骤继续进行宏扩展的新状态)

参考

boost 源码, boost_1_83_0/boost/preprocessor/tuple/elem.hpp

# define BOOST_PP_TUPLE_ELEM_2_0(a, b) a

另外,

The BOOST_PP_DEC macro expands to one less than its argument.

初始步骤的 macro:

#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \
	MTK_APPLY_MACRO_ON_TUPLE(~, \
		BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
		( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)) 

MTK_ENTRIES_OUTPUT(r, state) $\iff$ MTK_ENTRIES_OUTPUT_I state $\iff$ MTK_ENTRIES_OUTPUT_I(3, (vect3, acc), ((SO3, rot)) ((S2, grav)) (~), 0, 0, S2state, SO3state, SENstate)

$\iff$

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple

BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM) $\iff$ BOOST_PP_IF(2, MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM)

$\iff$ MTK_PUT_TYPE (因为判断条件2, 非零)

参考

The BOOST_PP_IF macro chooses between to values based on a logical condition.

故有

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

$\iff$

MTK_PUT_TYPE(BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)

$\iff$ MTK_PUT_TYPE(vect3, acc, 0, 0, S2state, SO3state, SENstate)

$\iff$ MTK::SubManifold<vect3, 0, 0> acc; (<<< 这是初始步骤的宏扩展结果)

#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
	MTK::SubManifold<type, dof, dim> id; 

参考

boost 源码, boost_1_83_0/boost/preprocessor/tuple/rem.hpp

# define BOOST_PP_TUPLE_REM_2(e0, e1) e0, e1

---

B. 第二步骤的宏扩展

第二步骤的 state:

state $\iff$ (2, (SO3, rot), ((S2, grav)) (~), 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate) (<<< 这是第二步骤进行宏扩展的状态 State)

第二步骤的 pred:

MTK_ENTRIES_TEST(r, state) $\iff$ MTK_TUPLE_ELEM_4_0 state $\iff$ MTK_TUPLE_ELEM_4_0(2, (SO3, rot), ((S2, grav)) (~), 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate) $\iff$ 2 (<<< 非 0, 第二步骤可以进行宏扩展)

第二步骤的 op:

MTK_ENTRIES_NEXT(r, state) $\iff$ MTK_ENTRIES_NEXT_I state

$\iff$

MTK_ENTRIES_NEXT_I(2, (SO3, rot), ((S2, grav)) (~), 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate)

其中

\qquad len $\iff$ 2

\qquad head $\iff$ (SO3, rot)

\qquad seq $\iff$ ((S2, grav)) (~)

\qquad dof $\iff$ 0+vect3::DOF

\qquad dim $\iff$ 0+vect3::DIM

那么

MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate)

$\iff$

( BOOST_PP_DEC(len), BOOST_PP_SEQ_HEAD(seq), BOOST_PP_SEQ_TAIL(seq), dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF, dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM, S2state, SO3state, SENstate )

$\iff$

( 1, (S2, grav), (~), 0+vect3::DOF + BOOST_PP_TUPLE_ELEM_2_0(SO3, rot)::DOF, 0+vect3::DIM + BOOST_PP_TUPLE_ELEM_2_0(SO3, rot)::DIM, S2state, SO3state, SENstate )

$\iff$

( 1, (S2, grav), (~), 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM, S2state, SO3state, SENstate ) (<<< 这是输入到再下一步骤继续进行宏扩展的新状态)

第二步骤的 macro:

MTK_ENTRIES_OUTPUT(r, state) $\iff$ MTK_ENTRIES_OUTPUT_I state $\iff$ MTK_ENTRIES_OUTPUT_I(2, (SO3, rot), ((S2, grav)) (~), 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate)

$\iff$

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

此处 s $\iff$ 2, 故 BOOST_PP_DEC(s) $\iff$ 1 (非零)

所以有 BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM) $\iff$ MTK_PUT_TYPE

进一步有

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

$\iff$

MTK_PUT_TYPE( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)

$\iff$ MTK_PUT_TYPE( SO3, rot, 0+vect3::DOF, 0+vect3::DIM, S2state, SO3state, SENstate)

其中

\qquad type $\iff$ SO3

\qquad id $\iff$ rot

\qquad dof $\iff$ 0+vect3::DOF

\qquad dim $\iff$ 0+vect3::DIM

所以

MTK::SubManifold<type, dof, dim> id;

$\iff$ MTK::SubManifold<SO3, 0+vect3::DOF, 0+vect3::DIM> rot; (<<< 这是第二步骤的宏扩展结果)

---

C. 第三步骤的宏扩展

第三步骤的 state:

state $\iff$ ( 1, (S2, grav), (~), 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM, S2state, SO3state, SENstate ) (<<< 这是第三步骤进行宏扩展的状态 State)

第三步骤的 pred:

MTK_ENTRIES_TEST(r, state) $\iff$ MTK_TUPLE_ELEM_4_0 state $\iff$ MTK_TUPLE_ELEM_4_0( 1, (S2, grav), (~), 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM, S2state, SO3state, SENstate ) $\iff$ 1 (<<< 非 0, 第三步骤可以进行宏扩展)

第三步骤的 op:

MTK_ENTRIES_NEXT(r, state) $\iff$ MTK_ENTRIES_NEXT_I state

$\iff$

MTK_ENTRIES_NEXT_I(1, (S2, grav), (~), 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM, S2state, SO3state, SENstate)

$\iff$ (0, ~, , 0+vect3::DOF + SO3::DOF + S2::DOF, 0+vect3::DIM + SO3::DIM + S2::DIM, S2state, SO3state, SENstate) (<<< 这是输入到再再下一步骤继续进行宏扩展的新状态)

第三步骤的 macro:

MTK_ENTRIES_OUTPUT(r, state) $\iff$ MTK_ENTRIES_OUTPUT_I state

$\iff$ MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate)

$\iff$

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

此处
\qquad s $\iff$ 1
\qquad BOOST_PP_DEC(s) $\iff$ 0

故

BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM)

$\iff$ BOOST_PP_IF(0, MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM)

$\iff$ MTK_PUT_TYPE_AND_ENUM

所以

MTK_APPLY_MACRO_ON_TUPLE(~, BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))

$\iff$ MTK_PUT_TYPE_AND_ENUM( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)

此处

\qquad head $\iff$ (S2, grav)

\qquad dof $\iff$ 0+vect3::DOF + SO3::DOF

\qquad dim $\iff$ 0+vect3::DIM + SO3::DIM

故有

$\iff$ MTK_PUT_TYPE_AND_ENUM( S2, grav, 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM, S2state, SO3state, SENstate)

#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \
	MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
	enum {DOF = type::DOF + dof}; \
	enum {DIM = type::DIM+dim}; \
	typedef type::scalar scalar; 

故有

$\iff$

MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate)

enum {DOF = type::DOF + dof};

enum {DIM = type::DIM+dim};

typedef type::scalar scalar;

$\iff$

MTK::SubManifold<type, dof, dim> id;

enum {DOF = type::DOF + dof};

enum {DIM = type::DIM+dim};

typedef type::scalar scalar;

$\iff$

MTK::SubManifold<S2, 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM> grav;

enum {DOF = S2::DOF + 0+vect3::DOF + SO3::DOF};

enum {DIM = S2::DIM + 0+vect3::DIM + SO3::DIM};

typedef S2::scalar scalar; (<<< 这是第三步骤的宏扩展结果)

---

D. 第四步骤的宏扩展

第四步骤的 state:

state $\iff$ (0, ~, , 0+vect3::DOF + SO3::DOF + S2::DOF, 0+vect3::DIM + SO3::DIM + S2::DIM, S2state, SO3state, SENstate) (<<< 这是第四步骤进行宏扩展的状态 State)

第四步骤的 pred:

MTK_ENTRIES_TEST(r, state) $\iff$ MTK_TUPLE_ELEM_4_0 state $\iff$ MTK_TUPLE_ELEM_4_0(0, ~, , 0+vect3::DOF + SO3::DOF + S2::DOF, 0+vect3::DIM + SO3::DIM + S2::DIM, S2state, SO3state, SENstate) $\iff$ 0 (<<< 为 0, 故 BOOST_PP_FOR_1 宏扩展结束)

---

E. MTK_SUBVARLIST 的最终输出

// 初始步骤宏扩展结果
MTK::SubManifold<vect3, 0, 0> acc; 

// 第二步骤宏扩展结果
MTK::SubManifold<SO3, 0+vect3::DOF, 0+vect3::DIM> rot;

// 第三步骤宏扩展结果
MTK::SubManifold<S2, 0+vect3::DOF + SO3::DOF, 0+vect3::DIM + SO3::DIM> grav; 
enum {DOF = S2::DOF + 0+vect3::DOF + SO3::DOF}; 
enum {DIM = S2::DIM + 0+vect3::DIM + SO3::DIM}; 
typedef S2::scalar scalar;

以上徒手扩展的结果和机器编译的 manifold_2_state 的对应部分相同. 可以看出来这一部分宏扩展后, 实现了复合流形内各子流形成员变量的声明.

---

3. 流形结构体的构造函数

我们对紧接着的流形结构体的构造函数进行解读

	name ( \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
		) : \
		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}

关联的其他预处理宏有

#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))

参考

The BOOST_PP_SEQ_ENUM macro enumerates the elements in a seq.

This macro expands to a comma-separated list of the elements in seq. For example, BOOST_PP_SEQ_ENUM((x)(y)(z)) expands to…

x, y, z

---

The BOOST_PP_SEQ_TRANSFORM_S macro transforms each element in a seq according to a supplied transformation. It reenters BOOST_PP_SEQ_FOLD_LEFT with maximum efficiency.

This macro expands op for each element in seq. It builds a new seq out of the results of each call. If, for example, seq is (a)(b)(c), this macro expands to…

(op(d, data, a))(op(d, data, b))(op(d, data, c))

---

A. MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries)

MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries)

$\iff$ BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_CONSTRUCTOR_ARG, entries))

其中

BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_CONSTRUCTOR_ARG, entries)

此处

\qquad op $\iff$ MTK_APPLY_MACRO_ON_TUPLE

\qquad data $\iff$ MTK_CONSTRUCTOR_ARG

\qquad seq $\iff$ ((vect3, acc)) ((SO3, rot)) ((S2, grav))

故有

$\iff$

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_ARG, (vect3, acc) ))

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_ARG, (SO3, rot) ))

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_ARG, (S2, grav) ))

$\iff$

(MTK_CONSTRUCTOR_ARG(vect3, acc) )

(MTK_CONSTRUCTOR_ARG(SO3, rot) )

(MTK_CONSTRUCTOR_ARG(S2, grav) )

#define MTK_CONSTRUCTOR_ARG(  type, id) const type& id = type()

$\iff$

(const vect3& acc = vect3()) (const SO3& rot = SO3()) (const S2& grav = S2())

进一步

MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries)

$\iff$ BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))

$\iff$ BOOST_PP_SEQ_ENUM( (const vect3& acc = vect3()) (const SO3& rot = SO3()) (const S2& grav = S2()) )

$\iff$ const vect3& acc = vect3(), const SO3& rot = SO3(), const S2& grav = S2()

---

B. MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries)

MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) $\iff$ BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_CONSTRUCTOR_COPY, entries))

其中

BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_CONSTRUCTOR_COPY, entries)

$\iff$

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_COPY, (vect3, acc) ))

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_COPY, (SO3, rot) ))

(MTK_APPLY_MACRO_ON_TUPLE(1, MTK_CONSTRUCTOR_COPY, (S2, grav) ))

#define MTK_CONSTRUCTOR_COPY( type, id) id(id)

$\iff$ ( acc(acc) ) ( rot(rot) ) ( grav(grav) )

进一步

MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries)

$\iff$ BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_CONSTRUCTOR_COPY, entries))

$\iff$ BOOST_PP_SEQ_ENUM( ( acc(acc) ) ( rot(rot) ) ( grav(grav) ) )

$\iff$ acc(acc), rot(rot), grav(grav)

---

C. 构造函数

name ( MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) ) : MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}

$\iff$

manifold_2_state( const vect3& acc = vect3(), const SO3& rot = SO3(), const S2& grav = S2() ): acc(acc), rot(rot), grav(grav) {}

(<<< 这就是构造函数宏扩展后的结果)

以上徒手扩展的结果和机器编译的 manifold_2_state 的对应部分相同. 实现了流形结构体的构造函数.

---

4. 流形结构体的成员函数

	void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
		MTK_TRANSFORM(MTK_BOXPLUS, entries)\
	} 

涉及到的预定义宏有

#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)

参考

The BOOST_PP_SEQ_FOR_EACH_R macro repeats a macro for each element in a seq. It reenters BOOST_PP_FOR with maximum efficiency.

Usage: BOOST_PP_SEQ_FOR_EACH_R(r, macro, data, seq)

This macro is a repetition construct. If seq is (a)(b)(c), it expands to the sequence:

macro(r, data, a) macro(r, data, b) macro(r, data, c)

MTK_TRANSFORM(MTK_BOXPLUS, entries) $\iff$ BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_BOXPLUS, entries)

其中

\qquad macro $\iff$ MTK_APPLY_MACRO_ON_TUPLE

\qquad data $\iff$ MTK_BOXPLUS

\qquad seq $\iff$ entries $\iff$ ((vect3, acc)) ((SO3, rot)) ((S2, grav))

故进一步有

BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, MTK_BOXPLUS, entries)

$\iff$

MTK_APPLY_MACRO_ON_TUPLE(1, MTK_BOXPLUS, (vect3, acc) )

MTK_APPLY_MACRO_ON_TUPLE(1, MTK_BOXPLUS, (SO3, rot) )

MTK_APPLY_MACRO_ON_TUPLE(1, MTK_BOXPLUS, (S2, grav) )

$\iff$ MTK_BOXPLUS(vect3, acc) MTK_BOXPLUS(SO3, rot) MTK_BOXPLUS(S2, grav)

#define MTK_BOXPLUS(          type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);

MTK_BOXPLUS(vect3, acc) MTK_BOXPLUS(SO3, rot) MTK_BOXPLUS(S2, grav)

$\iff$

acc.boxplus(MTK::subvector(__vec, &self::acc), __scale);

rot.boxplus(MTK::subvector(__vec, &self::rot), __scale);

grav.boxplus(MTK::subvector(__vec, &self::grav), __scale);

最后, 我们得到

void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { MTK_TRANSFORM(MTK_BOXPLUS, entries) }

$\iff$

void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) {

acc.boxplus(MTK::subvector(__vec, &self::acc), __scale);

rot.boxplus(MTK::subvector(__vec, &self::rot), __scale);

grav.boxplus(MTK::subvector(__vec, &self::grav), __scale);

}

以上徒手扩展的结果和机器编译的 manifold_2_state 的对应部分相同. 实现了流形结构体的成员函数 boxplus.

其他成员函数也可以参照此处处理, 进行徒手预处理宏的展开.

---

V. 总结

我们拨开了隐藏在 Boost Preprocessor 后面的流形工具包 MTK 通用流形类/结构体构建方法下的迷雾.

对 MTK 源文件 build_manifold.hpp 解读完毕.

(如有问题请指出, 谢谢！)

---