从零开始学习SLAM(三)

旋转向量

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

AngleAxisd 类有几种构造函数,其中最常用的是:

Eigen::AngleAxisd(const Scalar& angle, const Axis& axis);
  • angle 是旋转的角度,通常以弧度表示。
  • axis 是旋转的轴,通常是一个单位向量,表示旋转的方向。

例如,要创建一个围绕 Z 轴顺时针旋转 π/4 弧度(45度)的 AngleAxisd 对象,可以这样写:

Eigen::AngleAxisd rotation_angle_axis(M_PI / 4.0, Eigen::Vector3d::UnitZ());

或者是

Eigen::AngleAxisd rotation_angle_axis(M_PI / 4.0, Eigen::Vector3d(0,0,1));

使用

使用 AngleAxisd 对象可以执行以下操作:

  • 将其应用于向量或点进行旋转:

    // 通过将 rotation_angle_axis 应用到 point 上得到的旋转后的新点
    Eigen::Vector3d point(1.0, 0.0, 0.0);
    Eigen::Vector3d rotated_point = rotation_angle_axis * point;
    
  • 转换为旋转矩阵或四元数:

    // 通过 toRotationMatrix() 方法从 rotation_angle_axis 转换而来,用于表示相同旋转的旋转矩阵
    Eigen::Matrix3d rotation_matrix = rotation_angle_axis.toRotationMatrix();
    // 通过直接将 rotation_angle_axis 赋值给 Eigen::Quaterniond 类型得到,也表示了同样旋转的四元数
    Eigen::Quaterniond quaternion(rotation_angle_axis);
    
  • 进行组合和插值操作:

    // rotation_angle_axis 和 another_rotation 相乘(组合)得到的新的 Eigen::AngleAxisd 对象,它代表了先进行 rotation_angle_axis 的旋转,然后进行 another_rotation 的旋转。
    Eigen::AngleAxisd another_rotation(M_PI / 3.0, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd combined_rotation = rotation_angle_axis * another_rotation;
    // interpolated_rotation 是通过 slerp() 方法进行球面线性插值(Slerp)得到的 Eigen::AngleAxisd 对象。这种方法可以在两个旋转之间进行平滑的过渡,第一个参数是插值参数,通常是一个介于0到1之间的值,表示两个旋转的相对比例。
    Eigen::AngleAxisd interpolated_rotation = rotation_angle_axis.slerp(0.5, another_rotation);
    

欧氏变换

Eigen::Isometry3d 是 Eigen 库中用于表示三维欧氏空间中的等距变换(Isometry)的类。它继承自 Eigen::Transform,具体表示了包括平移和旋转在内的等距变换。

主要特点和用途:

  • Identity Isometry(单位等距变换):单位等距变换表示没有任何旋转或平移,即一个点经过单位等距变换后位置不变。在三维空间中,单位等距变换的旋转部分是单位矩阵,平移部分是零向量。
  • 静态成员函数 Identity():这个函数是通过 Isometry3d 类访问的静态函数,它返回一个默认的单位等距变换对象。通过调用 Eigen::Isometry3d::Identity(),可以获得一个已经初始化为单位变换的 Isometry3d 对象,通常用于开始定义其他具体的变换。
  1. 等距变换(Isometry)Isometry3d 能够表示旋转和平移的组合,保持点之间的距离不变。在计算机图形学、机器人学和几何计算等领域中,等距变换非常重要。
  2. 旋转和平移的组合:通过 Isometry3d,可以方便地表示和操作三维空间中的物体的姿态和位置。
  3. Eigen 库支持:Eigen 是一个开源的线性代数库,专注于高性能的矩阵和向量运算。Isometry3d 类充分利用了 Eigen 的矩阵和向量运算优势,提供了高效的数学运算。

示例:

以下是一个简单的示例,展示如何使用 Isometry3d 类创建和操作等距变换:

#include <Eigen/Geometry>

int main() {
    // 创建一个 Isometry3d 对象
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    // 设置旋转部分(绕Z轴旋转90度)
    T.rotate(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));

    // 设置平移部分(平移向量)
    T.pretranslate(Eigen::Vector3d(1, 2, 3));

    // 使用 Isometry3d 进行点变换
    Eigen::Vector3d point(1, 0, 0);
    Eigen::Vector3d transformed_point = T * point;

    // 输出变换后的点
    std::cout << "Transformed point: " << transformed_point.transpose() << std::endl;

    return 0;
}
#include <Eigen/Dense>
#include <iostream>

int main() {
    // 假设我们有一个4x4的仿射变换矩阵
    Eigen::Affine3d T;
    T.matrix() << 
        1, 0, 0, 1, // 旋转矩阵部分
        0, 1, 0, 0, // 旋转矩阵部分
        0, 0, 1, 0, // 旋转矩阵部分
        1, 2, 3, 1; // 平移向量

    // 提取旋转矩阵
    Eigen::Matrix3d R = T.linear();
    
    // 提取平移向量
    Eigen::Vector3d t = T.translation();
    
    // 打印旋转矩阵和平移向量
    std::cout << "Rotation matrix:\n" << R << std::endl;
    std::cout << "Translation vector:\n" << t.transpose() << std::endl;
    
    return 0;

}

T.linear()

这个成员函数返回仿射变换中的线性部分,即旋转矩阵。它返回的是一个Eigen::Matrix3d类型的引用,代表3x3的旋转矩阵。

T.translation()

这个成员函数返回仿射变换中的平移向量。它返回的是一个Eigen::Vector3d类型的引用,代表3D空间中的平移。

T.rotate()

这个成员函数是用来设置仿射变换的旋转部分的。你可以传递一个3x3的旋转矩阵给这个函数,它会更新T的线性部分。

旋转矩阵

Eigen::Matrix3d 在实际应用中有许多用途,以下是几个常见的例子:

1. 旋转矩阵表示旋转操作

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义一个旋转矩阵,将向量绕Z轴旋转45度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 4.0; // 45度
    rotation_matrix << cos(angle), -sin(angle), 0,
                       sin(angle), cos(angle), 0,
                       0, 0, 1;

    // 定义一个向量
    Eigen::Vector3d vector(1.0, 0.0, 0.0);

    // 将向量应用旋转
    Eigen::Vector3d rotated_vector = rotation_matrix * vector;

    // 输出结果
    std::cout << "Original vector: " << vector.transpose() << std::endl;
    std::cout << "Rotated vector: " << rotated_vector.transpose() << std::endl;

    return 0;
}

这个例子中,我们定义了一个 Eigen::Matrix3d 类型的 rotation_matrix,表示绕Z轴旋转45度的旋转矩阵。然后,我们定义了一个 Eigen::Vector3d 类型的向量 vector,并将其通过 rotation_matrix 进行旋转操作,得到 rotated_vector。最后输出了旋转前后的向量。

2. 坐标变换

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义一个坐标变换矩阵,将点从局部坐标系变换到全局坐标系
    Eigen::Matrix3d coordinate_transform;
    coordinate_transform << 1, 0, 0,
                            0, -1, 0,
                            0, 0, 1;

    // 定义一个局部坐标系下的点
    Eigen::Vector3d local_point(2.0, 3.0, 1.0);

    // 应用坐标变换
    Eigen::Vector3d global_point = coordinate_transform * local_point;

    // 输出结果
    std::cout << "Local point: " << local_point.transpose() << std::endl;
    std::cout << "Global point: " << global_point.transpose() << std::endl;

    return 0;
}

在这个例子中,我们定义了一个 Eigen::Matrix3d 类型的 coordinate_transform,表示一个坐标系的变换矩阵,用来将局部坐标系下的点 local_point 转换到全局坐标系下的 global_point。这种方式在计算机图形学和仿真中经常使用,用于物体的位置和姿态变换。

3. 矩阵运算

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义两个矩阵
    Eigen::Matrix3d A, B;
    A << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;
    B << 9, 8, 7,
         6, 5, 4,
         3, 2, 1;

    // 计算矩阵乘法
    Eigen::Matrix3d result = A * B;

    // 输出结果
    std::cout << "Matrix A:\n" << A << std::endl;
    std::cout << "Matrix B:\n" << B << std::endl;
    std::cout << "Result of A * B:\n" << result << std::endl;

    return 0;
}

当使用Eigen库进行旋转表示的转换时,可以如下操作:

1. 旋转矩阵转换为旋转向量

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义一个旋转矩阵,例如绕Y轴旋转30度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 6.0; // 30度
    rotation_matrix << cos(angle), 0, sin(angle),
                       0, 1, 0,
                       -sin(angle), 0, cos(angle);

    // 将旋转矩阵转换为旋转向量
    Eigen::AngleAxisd rotation_vector(rotation_matrix);

    // 输出结果
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;
    std::cout << "Equivalent rotation vector:\n" << rotation_vector.axis().transpose()
              << " " << rotation_vector.angle() << " radians" << std::endl;

    return 0;
}

在这个例子中,我们定义了一个旋转矩阵 rotation_matrix,表示绕Y轴旋转30度。然后,使用 Eigen::AngleAxisd 类型的构造函数将旋转矩阵转换为对应的旋转向量 rotation_vector。通过 rotation_vector.axis() 获取旋转向量的轴向量,通过 rotation_vector.angle() 获取旋转角度。

2. 旋转矩阵转换为四元数

Eigen::Matrix3d rotation_matrix;
// 填充旋转矩阵

Eigen::Quaterniond quaternion(rotation_matrix);

这段代码示例已经在前面提供过,展示了如何将旋转矩阵 rotation_matrix 转换为对应的四元数 quaternion

3. 旋转矩阵转换为欧拉角

Eigen库中没有直接提供将旋转矩阵转换为欧拉角的函数,但可以通过以下步骤手动实现欧拉角的计算:

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 定义一个旋转矩阵,例如绕Z轴旋转60度
    Eigen::Matrix3d rotation_matrix;
    double angle = M_PI / 3.0; // 60度
    rotation_matrix << cos(angle), -sin(angle), 0,
                       sin(angle), cos(angle), 0,
                       0, 0, 1;

    // 计算欧拉角(ZYX顺序)
    double phi = atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
    double theta = atan2(-rotation_matrix(2, 0), sqrt(rotation_matrix(2, 1) * rotation_matrix(2, 1) + rotation_matrix(2, 2) * rotation_matrix(2, 2)));
    double psi = atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));

    // 输出结果
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;
    std::cout << "Equivalent Euler angles (ZYX order):\n"
              << "Phi (Yaw): " << phi << " radians\n"
              << "Theta (pitch): " << theta << " radians\n"
              << "Psi (Roll): " << psi << " radians" << std::endl;

    return 0;
}

在这个例子中,我们定义了一个旋转矩阵 rotation_matrix,表示绕Z轴旋转60度。然后,通过手动计算欧拉角(ZYX顺序),即 Yaw (phi), pitch (theta), Roll (psi),从旋转矩阵中提取这些角度信息。

这些例子展示了如何使用 Eigen 库进行旋转表示的各种转换,从而在不同的场景中方便地处理旋转操作。

四元数

Eigen::Quateriond

Eigen::Quaterniond 是 Eigen 库中用于表示双精度(double)的四元数的类。四元数在计算机图形学和机器人学等领域中广泛用于表示旋转。让我展示如何使用 Eigen::Quaterniond 类来创建和操作四元数:

创建一个四元数

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 创建一个四元数,表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion;
    double angle = M_PI / 4.0; // 45度
    quaternion = Eigen::Quaterniond(cos(angle / 2), 0, 0, sin(angle / 2));

    // 输出四元数的信息
    std::cout << "Quaternion:\n" << quaternion.coeffs().transpose() << std::endl;

    return 0;
}

在这个例子中,我们创建了一个四元数 quaternion,它表示绕Z轴旋转45度。四元数的构造方法 Eigen::Quaterniond 接受四个参数 (w, x, y, z),分别表示四元数的实部和虚部(三个虚部分量)。在这里,我们使用角度 angle 计算实部 w 和虚部 z。quaternion.coeffs()函数返回一个Vector4d,其中包含四元数的系数,按照顺序为 ( (x, y, z, w) )

操作四元数

#include <Eigen/Dense>
#include <iostream>

int main() {
    // 创建一个四元数,表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion(cos(M_PI / 8), 0, 0, sin(M_PI / 8));

    // 获取旋转矩阵
    Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

    // 输出旋转矩阵
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;

    return 0;
}
#include <Eigen/Dense>
#include <iostream>

int main() {
    // 创建一个四元数,表示绕Z轴旋转45度
    Eigen::Quaterniond quaternion(cos(M_PI / 8), 0, 0, sin(M_PI / 8));

    // 获取旋转矩阵
    Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

    // 输出旋转矩阵
    std::cout << "Rotation matrix:\n" << rotation_matrix << std::endl;

    return 0;
}

在这里插入图片描述

总代码

#include <iostream>
# include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

// 四元数转换为旋转矩阵
Eigen::Matrix3d quaternionToRotationMatrix(const Eigen::Quaterniond& q) {
    double w, x, y, z;
    w = q.w();
    x = q.x();
    y = q.y();
    z = q.z();

    Eigen::Matrix3d R;
    R(0, 0) = 1 - 2 * y * y - 2 * z * z;
    R(0, 1) = 2 * x * y - 2 * z * w;
    R(0, 2) = 2 * x * z + 2 * y * w;
    R(1, 0) = 2 * x * y + 2 * z * w;
    R(1, 1) = 1 - 2 * x * x - 2 * z * z;
    R(1, 2) = 2 * y * z - 2 * x * w;
    R(2, 0) = 2 * x * z - 2 * y * w;
    R(2, 1) = 2 * y * z + 2 * x * w;
    R(2, 2) = 1 - 2 * x * x - 2 * y * y;

    return R;
}

// 四元数转换为旋转向量
Eigen::Vector3d quaternionToRotationVector(const Eigen::Quaterniond& q) {
    double theta = 2 * std::acos(q.w());
    double s = std::sqrt(1 - q.w() * q.w()); // assuming quaternion is normalized

    if (s < 0.001) { // if s is close to zero, use a Taylor expansion
        Eigen::Vector3d v(q.x(), q.y(), q.z());
        return theta * v;
    } else {
        Eigen::Vector3d v(q.x(), q.y(), q.z());
        return theta * v / s;
    }
}


int main(int argc, char** argv)
{
  // --------------------- 初始化 --------------------- //

  // 旋转向量:沿z轴旋转45°
  Eigen::AngleAxisd rotationVector (M_PI/4, Eigen::Vector3d(0, 0, 1));


  // 旋转矩阵:沿z轴旋转45°
  Eigen::Matrix3d rotationMatrix;
  double angle = M_PI / 4.0; 
  rotationMatrix <<    cos(angle), -sin(angle), 0,
                       sin(angle), cos(angle), 0,
                       0, 0, 1;
  // 沿y轴旋转45°
  rotationMatrix <<    cos(angle), 0, sin(angle),
                       0, 1, 0,
                       -sin(angle), 0, cos(angle);
  // 沿x轴旋转45°
  rotationMatrix <<    1, 0, 0,
                       0, cos(angle), -sin(angle),
                       0, sin(angle), cos(angle);


  // 四元数
  Eigen::Quaterniond quat = Eigen::Quaterniond(cos(M_PI/8), 0, 0, sin(M_PI/8) );              //  (w,x,y,z) θ=2⋅arccos(w)
  cout << "四元数输出方法1: quaternion = \n"<< quat.coeffs()<<endl;  
  // coeffs()方法用于获取四元数对象的系数,返回一个四维向量。coeffs的顺序是(x,y,z,w)
  cout << "四元数输出方法2: quaternion = \n x = "<< quat.x()<< "\n y = " << quat.y()<<  "\n z = " << quat.z()<< "\n w = " << quat.w()<<endl;


  // 欧拉角
  Eigen::Vector3d eulerAngles = Eigen::Vector3d(M_PI/4, 0, 0) ;                  // 绕 Z 轴的偏航角 (Yaw)、绕 Y 轴的俯仰角 (Pitch) 和 绕 X 轴的滚转角 (Roll)。
  

  // -----------------------------相互转换关系------------------------- //
  // 旋转向量转换为其他形式
  // 1.旋转向量 ----> 旋转矩阵
  Eigen::Matrix3d rotation_matrix = rotationVector.toRotationMatrix(); 
  // 2. 旋转向量 ----> 四元数
  Eigen::Quaterniond quaternion(rotationVector);


  // 旋转矩阵转换为其他形式
  // 1. 旋转矩阵 ----> 旋转向量
   Eigen::AngleAxisd rotation_vector(rotation_matrix);
  // 2. 旋转矩阵 ----> 欧拉角
  double phi = atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
  double theta = atan2(-rotation_matrix(2, 0), sqrt(rotation_matrix(2, 1) * rotation_matrix(2, 1) + rotation_matrix(2, 2) * rotation_matrix(2, 2)));
  double psi = atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));
  cout<<"phi(Yaw):"<<phi <<"\n theta(Pitch):"<<theta<<"\n psi(Roll):"<<psi<<endl;
  // 3. 旋转矩阵 ----> 四元数
  Eigen::Quaterniond quaternion1(rotation_matrix);


  // 四元数转换为其他形式
  // 1. 四元数 ----> 旋转向量
  Eigen::Vector3d rot_vec = quaternionToRotationVector(quat);
  std::cout << "Rotation Vector: \n" << rot_vec.transpose() << std::endl;
  // 2. 四元数 ----> 旋转矩阵
  Eigen::Matrix3d R = quaternionToRotationMatrix(quat);
  std::cout << "Rotation Matrix: \n" << R << std::endl;
  return 0;


  // 欧式变换
  // 创建一个 Isometry3d 对象
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    // 设置旋转部分(绕Z轴旋转90度)
    T.rotate(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));

    // 设置平移部分(平移向量)
    T.pretranslate(Eigen::Vector3d(1, 2, 3));

    // 提取旋转矩阵
    Eigen::Matrix3d R1 = T.linear();
    
    // 提取平移向量
    Eigen::Vector3d t = T.translation();
    cout << "Rotation matrix:\n" << R1 << endl;
    cout << "Translation vector:\n" << t.transpose() << endl;
}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值