Robotics Library项目代码分析（6）从Math库开始

从Math库开始

简介

之前我们看了math库下的metrics文件夹，现在让我们开始逐步看math下的其他文件，我们先从Real.h头文件开始看，因为这个文件被很多文件引用，所以我们先从这个开始看。

Real.h

这里的代码比较短，首先就是定义了一个double类型的别名——Real，后面我们会看到这个Real会出现很多次。然后就是两个函数，一个cbrt和一个sign，这两个函数没有什么好说的，一个开立方函数，以及一个sign函数。

namespace rl
{
	/**
	 * Common mathematical functions.
	 */
	namespace math
	{
		typedef double Real;
		
		template<typename T>
		inline T cbrt(const T& arg)
		{
			if (arg < 0)
			{
				return -::std::pow(-arg, static_cast<T>(1.0 / 3.0));
			}
			else
			{
				return ::std::pow(arg, static_cast<T>(1.0 / 3.0));
			}
		}
		
		template<typename T>
		inline T sign(const T& arg)
		{
			if (arg > 0)
			{
				return 1;
			}
			else if (arg < 0)
			{
				return -1;
			}
			else
			{
				return 0;
			}
		}
	}
}

因为这些结构都默认包含在rl下的math空间下，所以如果文章标题带有math库，我就后面不写这些内容，以免重复。

Function.h

这个头文件引用了上面的Real.h头文件，我们看一下其内容的定义。

template<typename T>
		class Function
		{
		public:
			Function() :
				x0(0),
				x1(1)
			{
			}
			
			virtual ~Function()
			{
			}
			
			virtual Function* clone() const = 0;
			
			Real duration() const
			{
				return this->upper() - this->lower();
			}
			
			Real& lower()
			{
				return this->x0;
			}
			
			const Real& lower() const
			{
				return this->x0;
			}
			
			Real& upper()
			{
				return this->x1;
			}
			
			const Real& upper() const
			{
				return this->x1;
			}
			
			/**
			 * Evaluates the function or a derivative thereof for a given value x.
			 * 
			 * Some functions are only defined in the interval [lower(), upper()],
			 * and fail to evaluate outside of 
			 * [lower() - FUNCTION_BOUNDARY, upper() + FUNCTION_BOUNDARY].
			 * In Debug mode, this is signaled by failing asserts.
			 * In Release mode, the function is evaluated if algebraically possible,
			 * or will return an empty ArrayX otherwise. 
			 * Some functions are not indefinitely often differentiable, 
			 * and will return a NaN array for all higher orders.
			 * 
			 * @param[in] x Input value of the function or derivative
			 * @param[in] derivative Order of the derivative to be evaluated
			 */
			virtual T operator()(const Real& x, const ::std::size_t& derivative = 0) const = 0;
			
		protected:
			Real x0;
			
			Real x1;
			
		private:
			
		};

这里传入的模板参数T基本没有怎么用到，唯一有用的地方就是后面那句virtual T operator()(const Real& x, const::std::size_t& derivative = 0) const = 0;这里定义了应该抽象虚函数，用来构建重载，后面基础这个类的类必须实现这个函数。其他的都是使用Real包装的函数，而由刚刚上面的那个Real.h头文件，我们可以知道，Real是double类型。这个Function主要是定义了两个Real类型的成员变量，x0和x1，x0是lower，x1是upper，并且提供了duraiton函数，用来计算两个Real类型的成员变量x0和x1的差值。

Circular.h

这个头文件引用了上面的Function.h头文件，我们看一下其内容的定义。

template <typename T>
class Circular : public Function<T> {
 public:
 protected:
 private:
};

这个文件非常简短，只是继承了Function类，并且没有做任何事情，这里是公有继承，意味Ciruclar可以访问Function的公有内容和保护内容，这个派生类是留给CiruclarVector2和CiruclarVector3继承的，这两个类我们先不看，因为，他们这两个又用到了别的头文件，而且挺多杂乱的，让我们继续先看别的。

Matrix.h

这里定义了三个宏，这三个宏是Eigen，比如EIGEN_MATRIXBASE_PLUGIN，然后我们在 MatrixBase 类中添加自定义的成员函数或数据成员，编译时候Eigen3就会在 MatrixBase 自动添加相应的内容，不用修改源码。所以我们还得去看一下Eigen的MatrixBaseAddons添加了什么内容。

#define EIGEN_MATRIXBASE_PLUGIN <rl/math/MatrixBaseAddons.h>
#define EIGEN_QUATERNIONBASE_PLUGIN <rl/math/QuaternionBaseAddons.h>
#define EIGEN_TRANSFORM_PLUGIN <rl/math/TransformAddons.h>

MatrixBaseAddons.h

所幸这里的内容也不多，就是给MatrixBase添加了几个方法，分别是cross3、cross33，voigt6，voigt33，这几个函数都只能用于特定规模的矩阵。所以通过了EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)这个静态断言来判断矩阵是否合规。

Matrix<Scalar, 3, 1>
cross3() const
{
	EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
	Matrix<Scalar, 3, 1> res;
	res.x() = (this->derived()(2, 1) - this->derived()(1, 2)) / Scalar(2);
	res.y() = (this->derived()(0, 2) - this->derived()(2, 0)) / Scalar(2);
	res.z() = (this->derived()(1, 0) - this->derived()(0, 1)) / Scalar(2);
	return res;
}

Matrix<Scalar, 3, 3>
cross33() const
{
	EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3)
	Matrix<Scalar, 3, 3> res;
	res(0, 0) = 0;
	res(0, 1) = -this->derived().z();
	res(0, 2) = this->derived().y();
	res(1, 0) = this->derived().z();
	res(1, 1) = 0;
	res(1, 2) = -this->derived().x();
	res(2, 0) = -this->derived().y();
	res(2, 1) = this->derived().x();
	res(2, 2) = 0;
	return res;
}

Matrix<Scalar, 6, 1>
voigt6() const
{
	EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
	Matrix<Scalar, 6, 1> res;
	res(0) = this->derived()(0, 0);
	res(1) = this->derived()(1, 1);
	res(2) = this->derived()(2, 2);
	res(3) = (this->derived()(1, 2) + this->derived()(2, 1)) / Scalar(2);
	res(4) = (this->derived()(0, 2) + this->derived()(2, 0)) / Scalar(2);
	res(5) = (this->derived()(0, 1) + this->derived()(1, 0)) / Scalar(2);
	return res;
}

Matrix<Scalar, 3, 3>
voigt33() const
{
	EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 6)
	Matrix<Scalar, 3, 3> res;
	res(0, 0) = this->derived()(0);
	res(0, 1) = this->derived()(5);
	res(0, 2) = this->derived()(4);
	res(1, 0) = this->derived()(5);
	res(1, 1) = this->derived()(1);
	res(1, 2) = this->derived()(3);
	res(2, 0) = this->derived()(4);
	res(2, 1) = this->derived()(3);
	res(2, 2) = this->derived()(2);
	return res;
}

QuaternionBaseAddons.h

上来就晦涩难懂，这里因为没有注释和文档，所以比较难以理解，也同时因为这些代码其实是对于Eigen3的QuaternionBase的补充，如果比较熟悉和了解Eigen以及一些机器人运动学那可能就是比较了然。

template<typename OtherDerived>
Vector3
angularVelocity(const QuaternionBase<OtherDerived>& qd) const
{
	return ((qd * this->derived().conjugate()) * Scalar(2)).vec();
}

于是我们通过浏览Eigen相应的文档可以了解得出，这个QuaternionBase是一个关于四元数的基类，this->derived().conjugate() 计算当前四元数的共轭，而 qd * this->derived().conjugate() 计算两个四元数的乘积，之后乘以标量2，(qd * this->derived().conjugate()) * Scalar(2)，最后的 vec() 提取结果四元数的向量部分，得到角速度向量。

这个类大部分都是涉及到了四元数的计算，而四元数的计算我了解不是很多，所以我也只能先写到这里，我要花一段时间去补充四元数的知识点。