Reference frame transformation eigen c++

  c++, eigen

Let’s say I have 5 Coordinate systems A,B,C,D and W, represented on the following image (this is a view from the top):

The reference frames viewed from the top

We know the following values:

  • p_B_in_A: the position (as an Eigen::Vector3d) of B in A
  • q_B_in_A: the orientation (as an Eigen::Quaterniond) of B in the A frame
  • p_C_in_B: the position of C in B (and it is (0,0,0))
  • q_C_in_B: the orientation of C in B
  • p_D_in_C: the position of D in C
  • q_D_in_C: the orientation of D in C
  • p_D_in_W: the position of D in the W (World) frame
  • q_D_in_W: the orientation of D in W

All position are Eigen::Vector3d and all orientation are Eigen::Quaterniond.

How can I find the position and orientation of A in the W frame (p_A_in_W and q_A_in_W)?

At this moment, I have:


template <typename T>
static Eigen::Matrix<T, 3, 1> getEuler(const Eigen::Quaternion<T>& orientation)
    auto euler = orientation.toRotationMatrix().eulerAngles(0, 1, 2);
    return euler;

template <typename T>
static Eigen::Quaternion<T> getQuaternion(const Eigen::Matrix<T,3,1>& orientation)
    Eigen::Quaternion<T> q = Eigen::AngleAxis<T>(orientation.x(), Eigen::Matrix<T,3,1>::UnitX())
                    * Eigen::AngleAxis<T>(orientation.y(), Eigen::Matrix<T,3,1>::UnitY())
                    * Eigen::AngleAxis<T>(orientation.z(), Eigen::Matrix<T,3,1>::UnitZ());
    return q;

int main(int argc, char** argv)
    Eigen::Vector3d p_B_in_A = Eigen::Vector3d(2,-1,1);
    Eigen::Quaterniond q_B_in_A = getQuaternion<double>(Eigen::Vector3d(0,0,0));
    Eigen::Vector3d p_C_in_B = Eigen::Vector3d(0,0,0);
    Eigen::Quaterniond q_C_in_B = getQuaternion<double>(Eigen::Vector3d(0, -M_PI_2, M_PI_2));
    Eigen::Vector3d p_D_in_C = Eigen::Vector3d(0,1,13.5);
    Eigen::Quaterniond q_D_in_C = getQuaternion<double>(Eigen::Vector3d(0, -M_PI_4, 0));
    Eigen::Vector3d p_D_in_W = Eigen::Vector3d(1,2,-3);
    Eigen::Quaterniond q_D_in_W = getQuaternion<double>(Eigen::Vector3d(M_PI_2, 0, M_PI_2));

    Eigen::Vector3d p_D_in_A = p_B_in_A  + (q_B_in_A * q_C_in_B * p_D_in_C);
    Eigen::Quaterniond q_D_in_A = q_B_in_A * q_C_in_B * q_D_in_C;
    Eigen::Vector3d p_A_in_W = p_D_in_W + (q_D_in_W * (q_D_in_A.inverse() * p_D_in_A));
    Eigen::Quaterniond q_A_in_W = q_D_in_W * q_D_in_A.inverse();

    std::cout << "p_D_in_A   " << p_D_in_A.transpose() << std::endl;
    std::cout << "q_D_in_A   " << getEuler<double>(q_D_in_A).transpose() << std::endl;
    return 0;

but the orientations doesn’t seem to be right and I have no idea why. Could the error come from the getEuler or getQuaternion functions?

PS: At this moment, I use euler angles to create the quaternions, but later, the values will directly be quaternions.

Source: Windows Questions C++