基于Eigen的位姿转换

已于 2023-10-29 20:40:03 修改
阅读量1.3k 收藏 9
点赞数 15
分类专栏： 3D视觉 文章标签： 机器人 自动驾驶 视觉检测 计算机视觉 c++
于 2023-10-13 12:31:55 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_72734364/article/details/133808089
版权

目录

一、概述

二、公式讲解

三、Eigen完整代码

四、新的测试案例

4.1 案例1

4.2 案例2

一、概述

         位姿中姿态的表示形式有很多种，比如：旋转矩阵、四元数、欧拉角、旋转向量等等。这里基于Eigen实现四种数学形式的相互转换功能。本文利用Eigen实现上述四种形式的相互转换。我这里给出一个SE3（4*4）(先平移、再旋转)的构建方法：

Eigen::Isometry3f T1 = Eigen::Isometry3f::Identity(); // 这一句千万别掉
// <1> 初始化R
Eigen::Matrix3f R;
// 按照 ZYX 顺序旋转
R = Eigen::AngleAxisf(3.14159 / 4, Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());
// <2> 初始化t
Eigen::Vector3f t(0.0, 0.0, 4.0);
// <3> 构建T = (R|t)
T1.rotate(R);
T1.pretranslate(t); // 这一句别搞错
//T1.translate(t);
std::cout << "T1 = " <<T1.matrix() <<std::endl;

打印结果：

T1 =         1         0         0         0
            0  0.707107 -0.707106         0
            0  0.707106  0.707107         4
            0         0         0         1

二、公式讲解

以下是在本地word中笔记的截图：

三、Eigen完整代码

Pose.h(类Pose声明)

#pragma once
#ifndef POSE_H
#define POSE_H

#include<Eigen/Core>
#include<Eigen/Geometry>

// namespace Geometry

class Pose
{
public:
    Pose();

    Pose& operator= (const Pose& pose);

    // construct from rotation
    Pose(const Eigen::Matrix3d& rotation);

    // construct from quaternion
    Pose(const Eigen::Quaterniond& quaternion);

    // construct from angle axisd
    Pose(const Eigen::AngleAxisd& angle_axis);

    // construct from euler angle
    Pose(const Eigen::Vector3d& euler_angle);

    ~Pose();

    // return rotation
    Eigen::Matrix3d rotation() const;

    // return quaterniond
    Eigen::Quaterniond quaternion() const;

    // return angle axisd
    Eigen::AngleAxisd angle_axis() const;

    // return euler angle
    Eigen::Vector3d euler_angle() const;

private:

    Eigen::Matrix3d rotation_;       // 旋转矩阵

    Eigen::Quaterniond quaternion_;  // 四元数

    Eigen::AngleAxisd angle_axis_;  // 角轴

    Eigen::Vector3d euler_angle_;    // 欧拉角 roll(X轴)  pitch(Y轴) yaw(Z轴)

};

// 姿态组合
Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2);

// 求逆
Eigen::Isometry3d inverse(const Eigen::Isometry3d& T);


#endif // !POSE_H

Pose.cpp(类Pose的实现)

#include "Pose.h"


Pose::Pose()
{}


Pose& Pose::operator= (const Pose& pose)
{
    this->rotation_    = pose.rotation();
    this->quaternion_  = pose.quaternion();
    this->angle_axis_ = pose.angle_axis();
    this->euler_angle_ = pose.euler_angle();
    return *this;
}

//
Pose::Pose(const Eigen::Matrix3d& rotation) :
    rotation_(rotation),
    quaternion_(Eigen::Quaterniond(rotation_)),
    angle_axis_(Eigen::AngleAxisd(rotation_)),
    euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}

Pose::Pose(const Eigen::Quaterniond& quaternion)
{
    quaternion.normalized();

    this->rotation_    = quaternion.toRotationMatrix();
    this->quaternion_  = Eigen::Quaterniond(rotation_);
    this->angle_axis_ = Eigen::AngleAxisd(rotation_);
    this->euler_angle_ = rotation_.eulerAngles(0, 1, 2);
}


Pose::Pose(const Eigen::AngleAxisd& angle_axis) :
    rotation_(angle_axis),
    quaternion_(Eigen::Quaterniond(rotation_)),
    angle_axis_(Eigen::AngleAxisd(rotation_)),
    euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}


Pose::Pose(const Eigen::Vector3d& euler_angle) :
    rotation_(Eigen::AngleAxisd(euler_angle.x(), Eigen::Vector3d::UnitX()) * // note: ZYX
                Eigen::AngleAxisd(euler_angle.y(), Eigen::Vector3d::UnitY()) *
              Eigen::AngleAxisd(euler_angle.z(), Eigen::Vector3d::UnitZ())),
    quaternion_(Eigen::Quaterniond(rotation_)),
    angle_axis_(Eigen::AngleAxisd(rotation_)),
    euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}


Pose::~Pose()
{}


Eigen::Matrix3d Pose::rotation() const
{
    return this->rotation_;
}


Eigen::Quaterniond Pose::quaternion() const
{
    return this->quaternion_;
}


Eigen::AngleAxisd Pose::angle_axis() const
{
    return this->angle_axis_;
}


Eigen::Vector3d Pose::euler_angle() const
{
    return this->euler_angle_;
}


Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2)
{
    return T1 * T2;
}

Eigen::Isometry3d inverse(const Eigen::Isometry3d& T)
{
    return T.inverse();
}

test_pose.cpp

#include<iostream>
using namespace std;

#include"Pose.h"
const double M_PI = 3.1415926535;

// 对于同一个姿态，从不同的数学形式(旋转矩阵、四元数、欧拉角、角轴)构造类Pose
// 依次得到 pose1 pose2 pose3 pose4
void testClassPose(const Eigen::Matrix3d& R1)
{

    Pose pose1(R1);
    cout << "旋转矩阵 = " << endl; cout << pose1.rotation() << endl;
    cout << "欧拉角 = " << endl;   cout << pose1.euler_angle().transpose()*(180 / M_PI) << endl;
    cout << "四元数 = " << endl;   cout << pose1.quaternion().coeffs().transpose() << endl;
    cout << "角轴 = " << endl;
    cout << pose1.angle_axis().angle()* (180 / M_PI) <<" " << pose1.angle_axis().axis().transpose() <<endl;
    cout << "-----------------------------" << endl;

    Pose pose2(pose1.euler_angle());
    cout << "旋转矩阵 = " << endl; cout << pose2.rotation() << endl;
    cout << "欧拉角 = " << endl;   cout << pose2.euler_angle().transpose()*(180 / M_PI) << endl;
    cout << "四元数 = " << endl;   cout << pose2.quaternion().coeffs().transpose() << endl;
    cout << "角轴 = " << endl;
    cout << pose2.angle_axis().angle()* (180 / M_PI) << " " << pose2.angle_axis().axis().transpose() << endl;
    cout << "-----------------------------" << endl;


    Pose pose3(pose1.angle_axis());
    cout << "旋转矩阵 = " << endl; cout << pose3.rotation() << endl;
    cout << "欧拉角 = " << endl;   cout << pose3.euler_angle().transpose()*(180 / M_PI) << endl;
    cout << "四元数 = " << endl;   cout << pose3.quaternion().coeffs().transpose() << endl;
    cout << "角轴 = " << endl;
    cout << pose3.angle_axis().angle()* (180 / M_PI) << " " << pose3.angle_axis().axis().transpose() << endl;
    cout << "-----------------------------" << endl;


    Pose pose4 = pose3;
    cout << "旋转矩阵 = " << endl; cout << pose4.rotation() << endl;
    cout << "欧拉角 = " << endl;   cout << pose4.euler_angle().transpose()*(180 / M_PI) << endl;
    cout << "四元数 = " << endl;   cout << pose4.quaternion().coeffs().transpose() << endl;
    cout << "角轴 = " << endl;
    cout << pose4.angle_axis().angle()* (180 / M_PI) << " " << pose4.angle_axis().axis().transpose() << endl;
    cout << "-----------------------------" << endl;


}

// 测试求逆、compose等
void testTheOthers(Eigen::Matrix3d R1, Eigen::Vector3d t1,
                   Eigen::Matrix3d R2, Eigen::Vector3d t2)
{
    // 初始化T1
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.prerotate(R1); T1.pretranslate(t1);
    cout << "T1" << endl; cout << T1.matrix() << endl;

    // 初始化T2
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.prerotate(R2); T2.pretranslate(t2);
    cout << "T2" << endl; cout << T2.matrix() << endl;

    // 求逆
    Eigen::Isometry3d T1_inverse = inverse(T1);
    cout << "T1_inverse = " << endl; cout << T1_inverse.matrix() << endl;

    // compose
    Eigen::Isometry3d T12 = compose(T1, T2);
    cout << "T12 = " << endl; cout <<  T12.matrix() << endl;


    /*cout << "Rotation = " << endl;
    cout << T1.rotation() * T2.rotation() << endl;

    cout << "Translation = " << endl;
    cout << T1.rotation() * T2.translation() + T1.translation() << endl;*/

}

int main()
{
    Eigen::Matrix3d R1; //R1
    R1 = Eigen::AngleAxisd((30.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*
         Eigen::AngleAxisd((25.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*
         Eigen::AngleAxisd((27.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d t1(1.2, 0.234, 2.3);//t1

    Eigen::Matrix3d R2; //R2
    R2 = Eigen::AngleAxisd((23.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*
         Eigen::AngleAxisd((33.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*
         Eigen::AngleAxisd((89.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());
    Eigen::Vector3d t2(0.1, 0.4, 0.1); //t2

    // <1> test Class Pose
    testClassPose(R1);

    // <2> test halcon's api
    testTheOthers(R1, t1, R2, t2);


    return 1;
}

可以看到，同一个姿态，不同的表达形式，他们之间相互转换之后结果数值一致。

四、新的测试案例

4.1 案例1

        写在最后，一个T为4×4的变换矩阵，如果旋转分量是欧式正交群，那个这个T为：欧式变换；否者为：仿射变换。如果一个旋转矩阵，不是SO3，那么可以将其转为四元数，接着归一化，再转为旋转矩阵，这样结果就属于SO3。同理，如果一个四元数，最好对齐进行归一化处理，再转为其他形式。例如：构造函数Pose(const Eigen::Quaterniond& quaternion); 其实现中比其他构造函数多了归一化这一步骤；即：quaternion.normalized();

       请看测试案例test3()，我们在第4或第5行进行四元数归一化，那么24行打印出来的矩阵就是单位阵。（不归一化，则不是单位阵）

void test3()
{
    Eigen::Quaterniond q = { 0.1,0.35,0.2,0.3 };
    //q.normalize();
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();

    cout << "R = " << endl;
    cout << R << endl;
    cout << endl;

    Eigen::Matrix3d R_transpose = R.transpose();
    cout << "R.transpose = " << endl;
    cout << R_transpose << endl;
    cout << endl;

    Eigen::Matrix3d R_inverse = R.inverse();

    cout << "R.inverse = " << endl;
    cout << R_inverse << endl << endl;
    cout << endl;

    Eigen::Matrix3d ret = R * R.transpose();
    cout << "ret = " << endl;
    cout << ret << endl << endl;
    cout << endl;
}

运行结果：

4.2 案例2

        现在，如果给定一个旋转矩阵（如果你是随机测试，请你参考博客开始部分公式，每个元素是有取值范围的，给出的数字不要太离谱）；如下测试案例test4。第8行在底层有四元数归一化操作，所以你看下面效果图，R * R.tranpose() 近似为一个单位矩阵。

void test4()
{
    Eigen::Matrix3d R;
    R << 0.74, 0.08, 0.25,
        0.2, 0.575, 0.05,
        0.17, 0.19, 0.675;
    Pose p1(R);
    Pose p2(p1.quaternion()); // Pose中，针对从四元数构造，默认有归一化功能
    R = p2.rotation();
    cout << "R = " << endl;
    cout << R << endl;
    cout << endl;

    cout << "R.inverse = " << endl;
    cout << R.inverse() << endl;
    cout << endl;

    cout << "R.transpose = " << endl;
    cout << R.transpose() << endl;
    cout << endl;

    cout << "R * R.transpose() = " << endl;
    cout << R * R.transpose() << endl;
    cout << endl;
}

运行结果：

        其中，R表示旋转，用旋转向量来描述（欧拉角有周期性和方向锁的问题，四元数有单位向量的约束，旋转矩阵冗余度太高且有各个基需要是单位正交的约束）；t表示平移，用平移向量来描述。R和t均采用无约束的向量进行描述，于是也均可以通过网络的学习来得到。因为R和t共有六个自由度，因此姿态估计又称为6D姿态估计。

注：旋转向量的方向代表旋转轴，模长代表旋转角的大小，旋转方向为逆时针。

        Eigen对于 translate 与 pretranslate的区别？（先平移、再旋转和 先旋转、再平移，有何区别？）可以看看这个：https://zhuanlan.zhihu.com/p/165020637

黄家驹beyond
关注
专栏目录
eigen的使用及常用的位姿变换实现（附C++代码）
qq_35635374的博客
07-02 188
常用于坐标转换及线性函数求解【它仅仅是一个工具，懂得线性代数的运算原理和坐标转换的原理是基础】–一般用eigen定义的矩阵类型，也对应的矩阵加减乘除、数量积向量积，分块、取矩阵元素等运算。
点云从入门到精通技术详解100篇-基于点云位姿估计的运行管理系统设计与实现
最新发布
getusushu的博客
10-18 249
我国聚变堆的建设水平大幅提升，同时为推进深空探索、高场核磁共振等研究提供一流水平的技术支持和功能完善的实验平台[2]。减轻人类工作负担的机器人慢慢出现在人类的视野中，发展至今体。在新时代下，人们对能源的需求越来越大，对机器人智能化的程度要求越来。越高，机器人在很多方面的感知已经远远超过了人类。达到的地方进行探索甚至是操作，就需要用到机器视觉来进行观察定位了。获取放在对其他国家的期望上，那中国将会在国际交流中处处受制于人，因此我。研究开发核聚变能，然而核聚变能源的开发是一个高投入、高风险、大规模、长。
Eigen中的变换矩阵
热门推荐
TODAY WAS A GOOD DAY
04-22 1万+
Eigen是一个矩阵计算的开源库，PCL中用了Eigen作为第三方做矩阵运算。这里借以介绍一下矩阵变换。 矩阵变换（affine transformation）包括旋转（rotation），平移（translation）和尺度（scale）变换。用矩阵表示就是这样的： 那么在三维点云里要表示变换矩阵，只需要旋转和平移：  Eigen::AngleAxisf ini
使用Eigen进行位姿计算
RQren的专栏
04-13 5047
使用Eigen进行位姿计算1. 概述2. 使用方法 1. 概述 在组合导航和slam等领域中，位姿计算是基础内容，使用Eigen来完成这一过程会大大减小工作量。姿态表示方法包括旋转向量、四元数、旋转矩阵、欧拉角，在使用Eigen之前，需要理清它们之间的关系。关于他们之间关系的基础介绍，网上已经有很多了，在此列出几篇博客，供参考： https://blog.csdn.net/yang__jing/a...
三维空间的位姿描述和齐次变换
ailianda的博客
06-10 1605
一、三维空间的位姿描述和齐次变换 首先规定一个坐标系，相对于该坐标系，点的位置可以用3维列向量表示;刚体的方位可用3×3的旋转矩阵来表示。而4×4的齐次变换矩阵则可将刚体位置和姿态（位姿）的描述统一起来，它具有以下优点： (1) 它可描述刚体的位姿，描述坐标系的相对位姿（描述）。 (2) 它可表示点从一个坐标系的描述转换到另一坐标系的描述（映射）。 (3)它可表示刚体运动前、后位姿描述的变换（算子）。 Eigen::Vector3d EulerAngles_OC(-1.57, 0, -1.5
Eigen 中四元数、欧拉角、旋转矩阵、旋转向量
ClaireQi的博客
12-19 1536
一、旋转向量 1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z) Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z)) 1.1 旋转向量转旋转矩阵 Eigen::Matrix3d rotation_matrix; rotation_matrix=rotation_vector.matrix(); Eigen::Matrix...
C++计算位姿之间的相对变换矩阵
11-05
输入是两帧的位姿放入qw,qc,tw,tc中，记住qw，qc里的第一个参数是TUM GroundTruth里位姿的最后一项，输出是两帧的变换矩阵T，下载就能用。
激光SLAM7-基于已知位姿的构图算法
lsxpu的专栏
02-22 568
1. 通过覆盖栅格建图算法进行栅格地图的构建 code //start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容） GridIndex hitPtIndex = ConvertWorld2GridIndex(world_x, world_y); std::vector<GridIndex> freeIndexs = TraceLine(robotIndex.x, robotIndex.y, hitPtIndex.x, hitPtIndex.y); for (con
【创建相机位姿并显示】
小秋 AI SLAM 入门实战教程
06-24 709
在计算机视觉和图形学中，对于摄像机（或一般的3D视图）模型，通常使用一个坐标系来表示其位置和方向。这个坐标系通常是一个右手坐标系或左手坐标系，包括x轴、y轴和z轴。在这些系统中，每个轴都有特定的含义。x轴代表图像的宽度方向。y轴代表图像的高度方向。z轴（深度轴）代表摄像机的视线方向，即从摄像机位置出发向摄像机正在观察的方向延伸。这行代码定义了相机z轴的方向，表示相机的视线方向。这是一种常见的选择，其设定为直向前，即在相机坐标系中，向物体观察的方向为正z方向。
ORB_SLAM2位姿发布所需的文件
04-22
- `Publisher.cpp`：位姿发布的实现，可能包含将相机位姿转换为特定格式，如ROS（Robot Operating System）的tf（Transformer）或自定义的消息结构，并通过某种通信机制（如TCP/IP或ZeroMQ）发送出去。 ORB_SLAM2...
激光slam基础入门笔记2——位姿表示与变换矩阵
m0_67976789的博客
01-10 907
二维平面内机器人位姿及变换矩阵介绍
Eigen中几种表示三维位姿的方式以及相互转换
努力努力努力
08-14 2450
Eigen中几种表示三维位姿的方式以及相互转换
通过Eigen库实现机械手位姿转换为Isometry3f类型
AngryBird_Vision的博客
04-13 340
结果如下，跟halcon中的算子pose_to_hom_mat3d的输出结果相同。初步接触这方面，根据自己的理解先简单实现效果。熟练掌握和使用后再进行优化。因为百度搜索时，大部分答主描述的比较复杂，这里记录一下如何直接使用。
Eigen中三维位姿表示方式以及相互转换
xiaojinger_123的博客
08-11 1963
Eigen中三维位姿表示方式以及相互转换
slam十四讲 03 Eigen实践之不同坐标系下的坐标转换
一千零一夜的博客
06-20 1280
相机1下的坐标转换到相机2下的坐标
Eigen四元数和欧拉角相互转换以及增量转换为四元数
asahi_331的博客
08-17 2910
Eigen四元数和欧拉角相互转换以及增量转换为四元数 因为loam的优化是基于欧拉角的，这不符合我下一步的操作目的，所以我决定将欧拉角的部分都替换为四元数。但是目前来看只替换优化部分效果很差，所以我决定将整个代码做一个修改。 以下，将用到过的知识记录一下。 // 将欧拉角转换为四元数 Eigen::AngleAxisf roll(transformTobeMapped[0], Eigen::Vector3f::UnitX()); Eigen::AngleAxisf pitch
c++ Eigen（二）
weixin_40378209的博客
12-22 1090
参考： 强大的C++矩阵处理库-Eigenlink Eigen库:常见错误(最坑的库，没有之一）link c++ Eigen库常见错误汇总（1）link 摘自： https://github.com/gaoxiang12/slambook2/tree/master/ch3/useGeometry 代码： eigenGeometry1.cpp #include <iostream> #include <cmath> #include <Eigen/Core> #incl
Eigen旋量的表示及PCL可视化显示
weixin_39458342的博客
11-23 612
Eigen旋量的表示及PCL可视化显示
Eigen::Isometry3d
05-20
Eigen::Isometry3d 是 Eigen 库中的一个类，表示三维空间中的欧氏变换矩阵（Isometry Matrix）。它是一个 4x4 的矩阵，其中前三行为旋转矩阵，第四行为平移向量。它可以用来表示三维空间中的刚体变换，可以进行向量和点的变换。Eigen::Isometry3d 继承自 Eigen::Transform 类，同时也支持 Eigen 库中的各种线性代数操作和运算符重载。
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

黄家驹beyond

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值