Eigen

1 块操作

块指的是矩阵或数组中的一个矩形区域，块表达式可以用于左值或者右值

Eigen中最常用的块操作是block()方法，共有两个版本：

块大小为（p,q），起始索引为（i,j）

版本1，动态构建块大小表达式：matrix.block.block(i,j,p,q)

版本2，固定大小构建块的表达式：matrix.block<p,q>(i,j)

这两个表达式语义上相同，唯一的区别是如果块的尺寸比较小的话固定尺寸版本的块操作运行更快，但是需要在编译阶段知道大小。

#include <Eigen/Dense>
#include <iostream>
using namespace std;
int main()
{
  Eigen::MatrixXf m(4,4);
  m <<  1, 2, 3, 4,
        5, 6, 7, 8,
        9,10,11,12,
       13,14,15,16;
  cout << "Block in the middle" << endl;
  cout << m.block<2,2>(1,1) << endl << endl;
  for (int i = 1; i <= 3; ++i)
  {
    cout << "Block of size " << i << "x" << i << endl;
    cout << m.block(0,0,i,i) << endl << endl;
  }
}

输出：

Block in the middle
 6  7
10 11

Block of size 1x1
1

Block of size 2x2
1 2
5 6

Block of size 3x3
 1  2  3
 5  6  7
 9 10 11

上述例子中的块操作方法作为表达式的右值，意味着是只读形式的，然而，块操作也可以作为左值使用，意味着你可以给他赋值。下面的例子说明了这一点，当然对于矩阵的操作是一样的。

#include <Eigen/Dense>
#include <iostream>
using namespace std;
using namespace Eigen;
int main()
{
  Array22f m;
  m << 1,2,
       3,4;
  Array44f a = Array44f::Constant(0.6);
  cout << "Here is the array a:" << endl << a << endl << endl;
  a.block<2,2>(1,1) = m;
  cout << "Here is now a with m copied into its central 2x2 block:" << endl << a << endl << endl;
  a.block(0,0,2,3) = a.block(2,1,2,3);
  cout << "Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:" << endl << a << endl << endl;
}

Here is the array a:
0.6 0.6 0.6 0.6
0.6 0.6 0.6 0.6
0.6 0.6 0.6 0.6
0.6 0.6 0.6 0.6

Here is now a with m copied into its central 2x2 block:
0.6 0.6 0.6 0.6
0.6   1   2 0.6
0.6   3   4 0.6
0.6 0.6 0.6 0.6

Here is now a with bottom-right 2x3 block copied into top-left 2x2 block:
  3   4 0.6 0.6
0.6 0.6 0.6 0.6
0.6   3   4 0.6
0.6 0.6 0.6 0.6

2 旋转向量

2.1 初始化旋转向量

旋转角为alpha(顺时针)，旋转轴为(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

2.2 旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

2.3 旋转向量转欧拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

2.4 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;
Quaterniond quaternion;

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

3 旋转矩阵

3.1 初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

3.2 旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

3.3 旋转矩阵转欧拉角(xyz，即RPY)

 Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

3.4 旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

4 欧拉角

4.1 初始化欧拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle(roll,pitch,yaw);

4.2 欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

4.3 欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

4.4 欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

5 四元数

5.1 初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);0

5.2 四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

5.3 四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

5.4 四元数转欧拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);

6 齐次欧式变换

 Isometry3d T=Isometry3d::Identity();
T.rotate(rotation_vector1);
T.pretranslate(t);
cout<<"齐次欧式变换：\n"<<T.matrix()<<endl;