#### Reference frame transformation eigen c++

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):

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:

``````
#include<Eigen/Geometry>
#include<iostream>

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++