# Getting the new heading after a Transformation

Lets say I have 2d poses as such (angles in degrees):

world_to_robot = {0, 4, 45} robot_to_object = {4, -4, -45}

From a graph it would be easy to see that:

world_to_object = {4, 8, theta}, what goes in for theta? atan2(8, 4)? What if the object has a world heading?

Ultimately this will need done in a format that is implemented in code and usable with any list of poses. So I would have as such for a c++ implementation,

Eigen::Matrix<float, 3, 1> world_to_robot;
Eigen::Matrix<float, 3, 1> world_to_object;
Eigen::Matrix<float, 3, 1> robot_to_object;

// My omniscient poses below from my fake scenario
world_to_robot << 0, 4, M_PI/2;
robot_to_object << 4, -4, -M_PI/4;
wold_to_object << 4, 8, some_theta_rad;   // place holder for whatever this ends up being

Eigen::Matrix<float, 3, 3> rotationM;
rotationM << cos(M_PI/2), -sin(M_PI/2), 0,
sin(M_PI/2,   cos(M_PI/2), 4,
0,             0,     1;

Eigen::Matrix<float, 3, 1> robot_to_objectM;
robot_to_objectM << 4, -4, 1;

Eigen::Matrix<float, 3, 1> calculated_world_to_object = rotationM*robot_to_objectM;



This will give me my expected world_to_object x and y pose for object wrt the global frame, but what would some_theta_rad be?

Then lets say I have the scenario where I have world_to_object and robot_to_object. Also assume I know theta for world_to_object because we identified the object based on a signature and there is an absolute pose. I want to calculate world_to_robot. Let's use the same poses as above.

I know that to get world_to_robot, I need to do a transform consisting of world_to_object * object_to_robot. I do not have object_to_robot. I need the inverse of the robot_to_object, which is not square. Here is what I have done in this scenario:

Eigen::Matrix<float, 3, 3> rotationM;