从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() 提取结果四元数的向量部分,得到角速度向量。
这个类大部分都是涉及到了四元数的计算,而四元数的计算我了解不是很多,所以我也只能先写到这里,我要花一段时间去补充四元数的知识点。