coordinate frame conversion on vision_position_estimate

thedinuka commented 9 years ago

I know this has been discussed before (#49) but I think the way the orientation is converted from ENU to NED in the vision_position_estimate.c is incorrect. (However, the way the position is converted is correct). In lines 142 - 145

vision_position_estimate(stamp.toNSec() / 1000,
                                       position.y(), position.x(), -position.z(),
                                       roll, -pitch, -yaw); // ??? please check!

This is assuming that Euler angles can be swapped when the axes is swapped, but that assumption is not true. Euler angles have an order associated with them and unless you follow that order of rotation, your end result can be different from what is expected.So this prevents you from swapping roll and pitch angles just because the roll axis and pitch axis is swapped. The correct way to do this I believe is to derive the right rotation matrix for NED and then convert from rotation matrix to Euler angles as follows:

tf::Matrix3x3 rot_ned2enu = [0, 1, 0,
                                             1, 0, 0,
                                             0, 0, -1];
tf::Matrix3x3 rot_enu2body(orientation);
tf::Matrix3x3 rot_ned2body =  rot_enu2body*rot_ned2enu;
rot_ned2body.getRPY(roll, pitch, yaw);

Note: I'm not sure whether orientation is equivalent to rot_enu2body or rot_body2enu. I guess it depends on the ROS node which does the actual pose calculation.

Additionally, I do not think that calculating RPY here and sending it to the FCU is probably not a good idea due to the singularities of Euler angles. Wouldn't it be better to send the quaternion directly?

mhkabir commented 9 years ago

@thedinuka We'll be sending the quaternion actually, as soon as the new message is done. See https://github.com/mavlink/mavlink/pull/333. That said, we still need to do the ENU-> NED here in ROS to maintain standards. Also, we're not sure about the current conventions, so contributions here would be welcome : https://github.com/mavlink/mavros/issues/216

tonybaltovski commented 9 years ago

@thedinuka The rotation matrix for body frames is rot_x(pi/2) = [1,0,0; 0, -1,0; 0, 0,-1]. The body frame and the inertial frame are different. The ROS body frame is defined as X is forward, Y is left and Z is up and multiplying will get you to the correct frame which is used.

LorenzMeier commented 9 years ago

Tony, do you have a link to the docs suggesting X is forward?

tonybaltovski commented 9 years ago

@LorenzMeier it's REP 103.

vooon commented 9 years ago

See also #216, i think is more general that just vision plugin.

congleetea commented 9 years ago

@thedinuka I change to vision_position_estimate(stamp.toNSec() / 1000, position.y(), position.x(), -position.z(), roll, -pitch, -yaw + 1.57); i.e change -yaw to -yaw+1.57. it works.