Eigen位姿表示

瞻邈

已于 2022-08-21 11:39:09 修改

阅读量2.1k

点赞数 2

分类专栏： # C++开源库文章标签：算法线性代数矩阵

于 2020-12-21 19:24:58 首次发布

本文链接：https://blog.csdn.net/xhtchina/article/details/111489117

版权

C++开源库专栏收录该内容

36 篇文章 15 订阅

订阅专栏

Eigen有多种位姿表示方法，下面依次介绍

1. Isometry3d

// 虽然称为3d，实质上是4＊4的矩阵，齐次坐标
Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();

// 按照rotation_matrix进行旋转
Tc1w.rotate(rotation_matrix);

// 把平移向量设成t
Tc1w.pretranslate(t);                                                  

// 获取旋转矩阵
Eigen::Matrix3d rotation = Tc1w.rotation();

// 获取平移向量
Eigen::Vector3d position = Tc1w.translation();

注意：

使用前一定要初始化，否则后面的设置无法生效；

函数pretranslate(p)就是把平移t设置到位姿矩阵之中，即实现的是t = p

函数translate(t)先用把位姿用到到t上，再把得到的结果设置到位姿矩阵之中，即实现的是t = Rp + t

2. 旋转向量AngleAxisd

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

2.1. 旋转向量转旋转矩阵

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

2.2. 旋转向量转欧拉角

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

2.3. 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

3. 四元数Quaterniond

3.1. 构造

# 使用四个数构造，注意四元数的顺序
Eigen::Quaterniond quaternion(w, x, y, z);

# 使用指针构造，注意四元数的顺序，与上面不同
Eigen::Matrix<double, 4, 1> pose_vec;
pose_vec << x, y, z, w;
Eigen::Quaterniond quaternion(pose_vec.data());

# 使用旋转矩阵构造
Eigen::Matrix3d rotation;
Eigen::Quaterniond (rotation);

3.2. 映射

Eigen::Matrix<double, 4, 1> pose_vec;
pose_vec << x, y, z, w;

# 二者共享内存
Eigen::Map<Eigen::Quaterniond> q(q_vec.data());

3.3. 其它函数

# 转旋转矩阵
q.toRotationMatrix();

# 求逆
q.inverse()

# 归一化
q.normalize()

4. 欧拉角

欧拉角转旋转矩阵

Eigen::AngleAxisd roll_matrix(Eigen::AngleAxisd(nav.roll, Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitch_matrix(Eigen::AngleAxisd(nav.pitch, Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yaw_matrix(Eigen::AngleAxisd(nav.yaw, Eigen::Vector3d::UnitZ()));
Eigen::Matrix rotation_matrix = (yaw_matrix * pitch_matrix * roll_matrix).matrix();