Get one(pitch) euler angle between two orientations (quaternions)

edit

Hi,

I want to get the angle between 2 quaternions (as euler angle), but the angle always needs to be from q1 to q2 (not the shortest angle). But since there are two ways to get an angle, I need to find the one I need. The following code works, but I'm looking for a nicer solution.

// calculate difference
ori_diff = q1.inverse() * q1;

//just use the pitch part
ori_diff.setValue(0., ori_diff.y(), 0., ori_diff.w());
ori_diff.normalize();

//get euler angle
tf::Matrix3x3 m;
m.setRotation(ori_diff);

tfScalar r1, r2, p1, p2, y1, y2;
m.getRPY(r1, p1, y1, 1);
m.getRPY(r2, p2, y2, 2);

if (        r1 == 0 && y1 == 0 ) {
  euler_angle = p1;
} else if ( r2 == 0 && y2 == 0) {
  euler_angle = p2;
} else {
  ROS_ERROR( ... );
}