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 类构造方法

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

变换类型 典型的初始化方法 二维定角度旋转 Rotation2Drot2(angle_in_radian); 三维定轴、定角旋转 AngleAxisaa(angle_in_radian, Vector3f(ax,ay,az)); 注：轴向量必须标准化，怕出错用，Vector3d::UnitX 三维四元数旋转 Quaternionq; 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类表示，其内部内部为 ( D i m + 1 ) 2 (Dim+1)^2 (Dim+1)2矩阵。在Eigen中，我们选择不区分点和向量，这样所有点实际上都由原点的位移向量表示（ p ≡ P − 0 p≡P−0 p≡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() = 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 ( D i m + 1 ) 2 (Dim+1)^2 (Dim+1)2 matrix, where the last row is assumed to be [0 … 0 1]. 假设行齐次矩阵
• AffineCompact: the transformation is stored as a ( D i m ) × ( D i m + 1 ) (Dim) \times (Dim+1) (Dim)×(Dim+1) matrix. 紧凑版齐次矩阵
• Projective: the transformation is stored as a ( D i m + 1 ) 2 (Dim+1)^2 (Dim+1)2matrix 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