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;

 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值