CODEWITHSUNDEEP
c++mathmatrixrotationeigen
Caesar
Caesar

Reputation: 9841

Creating a rotation matrix with pitch, yaw, roll using Eigen

How do I create a rotation matrix using pitch, yaw, roll with Eigen library?

Upvotes: 25

Views: 52021

Answers (6)

Mikael Tulldahl
Mikael Tulldahl

Reputation: 11

Here is an implementation using standard ZYX order like found in wikipedia

template <typename T>
Eigen::Matrix3<T> eulerZYX_2_rot(T roll_rad, T pitch_rad, T yaw_rad) {
  // ZYX order, same as rot_z(yaw_rad) * rot_y(pitch_rad) * rot_x(roll_rad)
  static_assert(std::is_floating_point_v<T>, "Floating point required.");
  T sx = sin(roll_rad);
  T cx = cos(roll_rad);
  T sy = sin(pitch_rad);
  T cy = cos(pitch_rad);
  T sz = sin(yaw_rad);
  T cz = cos(yaw_rad);
  Eigen::Matrix3<T> rot(3, 3);
  rot(0, 0) = cz * cy;
  rot(0, 1) = cz * sy * sx - sz * cx;
  rot(0, 2) = cz * sy * cx + sz * sx;
  rot(1, 0) = sz * cy;
  rot(1, 1) = sz * sy * sx + cz * cx;
  rot(1, 2) = sz * sy * cx - cz * sx;
  rot(2, 0) = -sy;
  rot(2, 1) = cy * sx;
  rot(2, 2) = cy * cx;
  return rot;
}

Upvotes: 0

Ran Cheng
Ran Cheng

Reputation: 41

I translate their Java implementation to C++ from this site: Euler Angle Visualization Tool

#include <iostream>
#include <math.h>
#include <Eigen/Dense>

Eigen::Matrix3d rotation_from_euler(double roll, double pitch, double yaw){
    // roll and pitch and yaw in radians
    double su = sin(roll);
    double cu = cos(roll);
    double sv = sin(pitch);
    double cv = cos(pitch);
    double sw = sin(yaw);
    double cw = cos(yaw);
    Eigen::Matrix3d Rot_matrix(3, 3);
    Rot_matrix(0, 0) = cv*cw;
    Rot_matrix(0, 1) = su*sv*cw - cu*sw;
    Rot_matrix(0, 2) = su*sw + cu*sv*cw;
    Rot_matrix(1, 0) = cv*sw;
    Rot_matrix(1, 1) = cu*cw + su*sv*sw;
    Rot_matrix(1, 2) = cu*sv*sw - su*cw;
    Rot_matrix(2, 0) = -sv;
    Rot_matrix(2, 1) = su*cv;
    Rot_matrix(2, 2) = cu*cv;
    return Rot_matrix;
}



int main() {
    Eigen::Matrix3d rot_mat = rotation_from_euler(0, 0, 0.5*M_PI);
    std::cout << rot_mat << std::endl;
    return 0;
}

Upvotes: 0

David Hammen
David Hammen

Reputation: 33116

How do I create a rotation matrix using pitch, yaw, roll with Eigen library?

There are 48 ways to do this. Which one do you want? Here are the factors:

  • Extrinsic verus intrinsic.
    Are the rotations about the axes of the fixed system (extrinsic) or are they about the rotated axes (intrinsic)?

  • Rotation versus transformation.
    Do you want to represent the matrix that physically rotates some object or do you want to represent the matrix that transforms vectors from one reference frame to another?

  • Astronomical sequences.
    There are six fundamental astronomical sequences. The canonical Euler sequence involves a rotation about the z axis followed by a rotation about the (rotated) x axis followed by a third rotation about (rotated again) z axis. There are five more of these astronomical-style sequences (x-y-x, x-z-x, y-x-y, y-z-y,and z-y-z) in addition to this canonical z-x-z sequence.

  • Aerospace sequences.
    To add to the confusion, there are six fundamental aerospace sequences as well. For example, a pitch-yaw-roll sequence versus a roll-pitch-yaw sequence. While the astronomy community has pretty much settled on a z-x-z sequence, the same cannot be said of the aerospace community. Somewhere along the way you find people using every one of the six possible sequences. The six sequences in this group are x-y-z, x-z-y, y-z-x, y-x-z, z-x-y, and z-y-x.

Upvotes: 12

narcispr
narcispr

Reputation: 341

Caesar answer is ok but as David Hammen says it depends on your application. For me (underwater or aerial vehicles field) the winning combination is:

Eigen::Quaterniond
euler2Quaternion( const double roll,
                  const double pitch,
                  const double yaw )
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}

Upvotes: 22

Caesar
Caesar

Reputation: 9841

Seeing as how I couldn't find a prebuilt function that does this, I built one and here it is in case someone finds this question in the future

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

Upvotes: 39

ApproachingDarknessFish
ApproachingDarknessFish

Reputation: 14313

All you need to create a rotational matrix is the pitch, yaw, roll, and the ability to perform matrix multiplication.

First, create three rotational matrices, one for each axis of rotation (ie one for pitch, one for yaw, one for roll). These matrices will have the values:

Pitch Matrix:

1, 0, 0, 0,
0, cos(pitch), sin(pitch), 0,
0, -sin(pitch), cos(pitch), 0,
0, 0, 0, 1

Yaw Matrix: 

cos(yaw), 0, -sin(yaw),  0,
0, 1, 0, 0,
sin(yaw), 0, cos(yaw), 0,
0, 0, 0, 1

Roll Matrix: 

cos(roll), sin(roll), 0, 0,
-sin(roll), cos(roll), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1

Next, multiply all of these together. The order here is important. For normal rotations, you will want to multiply the Roll Matrix by the Yaw Matrix first and then multiply the product by the Pitch Matrix. However, if you're trying to "undo" a rotation by going backwards, you'll want to perform the multiplications in reverse order (in addition to the angles having opposite values).

Upvotes: 10

Related Questions