Eigen的学习（十）几何学应用

Geometry（几何学）主要就是指的空间变换。学习这一章，可以减少代码编写量。本小节将会介绍一些处理2D、3D旋转、投影和仿射的变换的一些内容，这些内容将会由几何模块（geometry module）提供。

官方文档地址

一、Geometry模块介绍

Geometry模块可以提供以下功能支持：

• 固定大小的齐次变换；
• 平移、缩放及二维和三维的旋转；
• 四元数；
• 叉积 (MatrixBase::cross, MatrixBase::cross3) ；
• 产生正交向量 (MatrixBase::unitOrthogonal)；
• 一些线性组件： 参数化直线和超平面；
• 轴对称包围盒 axis aligned bounding boxes；
• 最小二乘矩阵拟合 least-square transformation fitting；

这里主要对Tranform进行支持，这个类是几何变换的基础，代表N维齐次矩阵。

二、Tranform类

2.1 构造函数

Transform模板定义如下：

template<typename _Scalar, int _Dim, int _Mode, int _Options>
class Eigen::Transform< _Scalar, _Dim, _Mode, _Options >

它代表了N维的齐次矩阵（Homogeneous transformation），单应性（homography）在内部由矩阵表示，可以使用matrix()方法返回。

• _Scalar 标量类型, 也就是系数的类型；
• _Dim 空间维度
• _Mode 变换的类型，它可能是：Affine或者AffineCompact或者Projective、Isometry[2]
• _Options 空间存储顺序，可选

对于机器人的变换算子（齐次矩阵）_Mode应该为Isometry，_Dim=3，对应的完整声明为：Transform T;，和Matrix3f等一样，为了方便声明，有以下别名：

完整声明	别名
typedef Transform	Isometry2f;
typedef Transform	Isometry3f;
typedef Transform	Isometry2d;
typedef Transform	Isometry3d;
typedef Transform	Affine2f;
typedef Transform	Affine3f;
typedef Transform	Affine2d;
typedef Transform	Affine3d;
typedef Transform	AffineCompact2f;
typedef Transform	AffineCompact3f;
typedef Transform	AffineCompact2d;
typedef Transform	AffineCompact3d;
typedef Transform	Projective2f;
typedef Transform	Projective3f;
typedef Transform	Projective2d;
typedef Transform	Projective3d;

这样一来，我们就能够直接使用Isometry3d来表示机器人的变换算子了。

默认构造函数	解释
Transform()	默认不进行初始化
Transform(const EigenBase< OtherDerived > & other)	用满足一定规则的Matrix构造
Transform(const QTransform< _Scalar, _Dim, _Mode, _Options > & other)	QT中的QTransform构造
Transform(const QMatrix & other)	QT中的QMatrix构造
Transform(const Transform< OtherScalarType, Dim, Mode, Options > & other)	拷贝构造（Tranform）类型

注：第二个构造函数是EigenBase，可用于构造和赋值MatrixBase，MatrixBase是所有dense matrices, vectors, and expressions的基类。像是一开始学到的Matrix类就是MatrixBase的子类，因此，你可以直接使用Matrix构造Transform。

MatrixBase有一个反解欧拉角的方法：Matrix< Scalar, 3, 1 > eulerAngles (Index a0, Index a1, Index a2) const）
Index是一个std::ptrdiff_t long型整数，0代表绕x，1代表绕y，2代表z。

2.2 进行变换

Transform之间的变换可以通过运算符*来进行，值得注意的是，运算支持了DiagonalBase、EigenBase和Transform ，这也就是说，我们可以直接与Vector、Matrix进行混合运算。
Tranform类会提供两种不同的几何变换：

• 抽象变换。比如旋转（以轴角或者四元数），平移，缩放。这些变换不是由matrix类来表示的，但是你愿意，你仍可以将它们与matrix类、vector类混合使用。
• 投影或者仿射变换则是matrix类。查看Transform类，确实如此。

请注意，Transform虽非一个Matrix类型，但是仍然可以与Matrix、Vector进行混合使用。与一般的Matrix不同的地方在于，Tranform提供了许多性质用于简化组合和使用。具体的，它能够由其他变换（Transfrom,Translation,RotationBase,DiaonalMatrix）隐式转换成齐次向量后组合而成，这个操作就是运算符*，运算一般就是纯乘法，eigen不可以充分利用这些变换的性质，对其进行优化以提升效率。

二、Transformation 类构造方法

坐标变换是将一个坐标变换到另一个坐标，可以分为平移、旋转、缩放等。

变换类型	典型的初始化方法
二维定角度旋转	Rotation2D rot2(angle_in_radian);
三维定轴、定角旋转	AngleAxis aa(angle_in_radian, Vector3f(ax,ay,az)); 注：轴向量必须标准化，怕出错用，Vector3d::UnitX
三维四元数旋转	Quaternion q; q = AngleAxis(angle_in_radian, axis);
N维缩放	Scaling(sx, sy) Scaling(sx, sy, sz) Scaling(s) Scaling(vecN)
N维平移	Translation(tx, ty) Translation(tx, ty, tz) Translation(s) Translation(vecN)
N维维仿射变换	Transform t = concatenation_of_any_transformations; Transform t =Translation3f(p) * AngleAxisf(a,axis) * Scaling(s);
N维线性变换(纯旋转、缩放等)	Matrix t = concatenation_of_rotations_and_scalings; Matrix t = Rotation2Df(a) * Scaling(s); Matrix t = AngleAxisf(a,axis) * Scaling(s);

以AngleAxis为例，构造函数有：

• 空默认构造
• 轴角两个参数
• QuaternionBase
• MatrixBase
• AngleAxis

赋值构造函数有：

• MatrixBase
• QuaternionBase

这就是不同四元数、矩阵之间构造时相互转换的基础。

Matrix3d m=Matrix3d::Identity(3,3);
Quaterniond q(m); 					//Rotation Matrix to Quaternion
AngleAxisd d;						//Default construction for AngleAxisd
d.fromRotationMatrix(m);			//Rotation Matrix to AngleAxis
auto dq=d*q;						//Quaternion=AngleAxis*Quaternion 
auto qd=q*d;						//Quaternion=Quaternion*AngleAxis
d=q;								//AngleAxis=Quaternion
q=d;								//Quaternion=AngleAxis
std::cout<<d.matrix()<<std::endl;	//AngleAxis to Rotation Matrix 3x3
std::cout<<q.matrix()<<std::endl;	//Quaternion to Rotation Matrix 3x3

Quaternion也有对应的构造函数：点击此查看

三、跨越变换类型的通用API

在特定范围内，Eigen的几何模块允许您编写处理任何类型变换表示的通用算法：

算法	形式
变换串联	gen1 * gen2;
对一个向量应用变换	vec2 = gen1 * vec1;
变换的逆	gen2 = gen1.inverse();
球面插补	(Rotation2D and Quaternion only)

算法是针对所有抽象类型的而不是特定类型的。

四、仿射变换（Affine transformations）

通用的仿射变换由Transform类表示，其内部内部为$(Dim+1{)}^2$矩阵。在Eigen中，我们选择不区分点和向量，这样所有点实际上都由原点的位移向量表示（$p\equiv P-0$ ). 考虑到这一点，应用变换时，实际点和向量会有所区别。

变换	代码
点的变换	VectorNf p1, p2; p2 = t * p1;
向量的变换	VectorNf vec1, vec2; vec2 = t.linear() * vec1;
Apply a general transformation to a normal vector	VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix *n1).normalized(); (See subject 5.27 of this faq for the explanations)
Apply a transformation with pure rotation to a normal vector (no scaling, no shear)	n2 = t.linear() * n1;
OpenGL compatibility 3D	glLoadMatrixf(t.data());
OpenGL compatibility 2D	Affine3f aux(Affine3f::Identity()); aux.linear().topLeftCorner<2,2>() = t.linear(); aux.translation().start<2>() = t.translation(); glLoadMatrixf(aux.data());

部件获取：

解释	代码
读写内部矩阵	t.matrix() = matN1xN1; // N1 means N+1 mat N1xN1 = t.matrix();
读写系数（元素）	t(i,j) = scalar; <=> t.matrix()(i,j) = scalar; scalar = t(i,j); <=> scalar = t.matrix()(i,j);
移动部分	t.translation() = vecN; vecN = t.translation();
线性部分	t.linear() = matNxN; matNxN = t.linear();
取出旋转矩阵	matNxN = t.rotation();

变换上应用另一个变换：

名称	procedural API	equivalent natural API
Translation	t.translate(Vector_(tx,ty,…)); t.pretranslate(Vector_(tx,ty,…));	t *= Translation_(tx,ty,…); t = Translation_(tx,ty,…) * t;
Rotation In 2D and for the procedural API, any_rotation can also be an angle in radian	t.rotate(any_rotation); t.prerotate(any_rotation);	t *= any_rotation; t = any_rotation * t;
Scaling	t.scale(Vector_(sx,sy,…)); t.scale(s); t.prescale(Vector_(sx,sy,…)); t.prescale(s);	t *= Scaling(sx,sy,…); t *= Scaling(s); t = Scaling(sx,sy,…) * t; t = Scaling(s) * t;
Shear transformation ( 2D only ! )	t.shear(sx,sy); t.shear(sx,sy);

注：pre代表在作用对象的前面，即左乘；不带pre的则为右乘，*=为右乘。

等价形式代码，注意顺序：

t.pretranslate(..).rotate(..).translate(..).scale(..);
t = Translation_(..) * t * RotationType(..) * Translation_(..) * Scaling(..);

---

补充阅读
【1】什么是仿射变换。https://blog.csdn.net/u011681952/article/details/98942207

【2】
_Mode 可以是：

• Affine: the transformation is stored as a $(Dim+1{)}^2$ matrix, where the last row is assumed to be [0 … 0 1]. 假设行齐次矩阵
• AffineCompact: the transformation is stored as a $(Dim)\times (Dim+1)$ matrix. 紧凑版齐次矩阵
• Projective: the transformation is stored as a $(Dim+1{)}^2$matrix without any assumption. 无假设行齐次矩阵
• isometry: Transformation is an isometry. 等距变换，也就是平移+旋转。目前接触到的是这个。

_Option:
_Options A combination of either RowMajor or ColMajor, and of either AutoAlign or DontAlign. The former controls storage order, and defaults to column-major. The latter controls alignment, which is required for vectorization. It defaults to aligning matrices except for fixed sizes that aren’t a multiple of the packet size.

关于仿射变换、射影变换和等距变换的区别。在Slam中几种变换的理解一文中可以知道这些变换的具体的含义。机器人运动学中选iosmetry表示同一个向量再各个坐标系下的长度和角度都没有发生变化，仅仅只是发生了旋转和平移的运动。

【3】http://eigen.tuxfamily.org/dox-devel/group__TutorialGeometry.html