姿态测量代码基础知识

Zhangzhang_stella

已于 2022-04-26 16:20:41 修改

阅读量386

点赞数

文章标签： c++

于 2022-04-24 01:41:42 首次发布

本文链接：https://blog.youkuaiyun.com/Zhangzhang_stella/article/details/124368938

版权

eigen中的向量矩阵都是matrix实现的

Matrix<float, 2, 3>　　前三个参数为：数据类型，行，列

Vector3d　三维向量　实质上是　　Eigen::Matrix<double, 3, 1>

Matrix<double, Dynamic, Dynamic> matrix_dynamic;　　动态大小矩阵

matrix_23 << 1, 2, 3, 4, 5, 6;　　　注意<< 就是赋值

Matrix3d::Zero(); //初始化为零

cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;　//输出

  // 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

    vd_3d << 4,5,6;
    //乘法，不同类型需要显性的转换
    Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;

eigen的简单用法汇总_0penuel0的博客-优快云博客

  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  matrix_33 = Matrix3d::Random();      // 随机数矩阵
  cout << "random matrix: \n" << matrix_33 << endl;
  cout << "transpose: \n" << matrix_33.transpose() << endl;      // 转置
  cout << "sum: " << matrix_33.sum() << endl;            // 各元素和
  cout << "trace: " << matrix_33.trace() << endl;          // 迹
  cout << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘
  cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆
  cout << "det: " << matrix_33.determinant() << endl;    // 行列式

集合模块

// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f

  Matrix3d rotation_matrix = Matrix3d::Identity();
  //定义单位矩阵　１００
  //            ０１０
  //             ００１

// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）

AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     
//沿 Z 轴旋转 45 度
//M_PI / 4,   就是　π/４　就是45度

cout.precision(3);　　　有效数字

关于C++的精度输出中的cout.precision()的正确解释_weixin_53310362的博客-优快云博客_cout.precision

cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵

rotation_matrix = rotation_vector.toRotationMatrix();

// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

// 欧氏变换矩阵使用 Eigen::Isometry
Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;

/ 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
     << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可