Skip to content

Commit

Permalink
frame tf: move ENU<->ECEF transforms to ftf_frame_conversions.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@TSC21
TSC21 committed Jun 28, 2017
1 parent c950748 commit e0a1173
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 74 deletions.
30 changes: 25 additions & 5 deletions mavros/include/mavros/frame_tf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,12 @@ using EigenMapConstCovariance9d = Eigen::Map<const Eigen::Matrix<double, 9, 9, E
* @brief Orientation transform options when applying rotations to data
*/
enum class StaticTF {
NED_TO_ENU, //!< will change orinetation from being expressed WRT NED frame to WRT ENU frame
NED_TO_ENU, //!< will change orientation from being expressed WRT NED frame to WRT ENU frame
ENU_TO_NED, //!< change from expressed WRT ENU frame to WRT NED frame
AIRCRAFT_TO_BASELINK, //!< change from expressed WRT aircraft frame to WRT to baselink frame
BASELINK_TO_AIRCRAFT //!< change from expressed WRT baselnk to WRT aircraft
BASELINK_TO_AIRCRAFT, //!< change from expressed WRT baselnk to WRT aircraft
ECEF_TO_ENU, //!< change from expressed WRT ECEF frame to WRT ENU frame
ENU_TO_ECEF //!< change from expressed WRT ENU frame to WRT ECEF frame
};

namespace detail {
Expand All @@ -72,7 +74,7 @@ namespace detail {
Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform);

/**
* @brief Transform data experessed in one frame to another frame.
* @brief Transform data expressed in one frame to another frame.
*
* General function. Please use specialized enu-ned and ned-enu variants.
*/
Expand Down Expand Up @@ -100,9 +102,9 @@ Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &
Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q);

/**
* @brief Transform data experessed in one frame to another frame.
* @brief Transform data expressed in one frame to another frame.
*
* General function. Please use specialized enu-ned and ned-enu variants.
* General function. Please use specialized variants.
*/
Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform);

Expand Down Expand Up @@ -206,6 +208,24 @@ inline T transform_frame_baselink_aircraft(const T &in) {
return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT);
}

/**
* @brief Transform data expressed in ECEF frame to ENU frame.
*
*/
template<class T>
inline T transform_frame_ecef_enu(const T &in) {
return detail::transform_static_frame(in, StaticTF::ECEF_TO_ENU);
}

/**
* @brief Transform data expressed in ENU frame to ECEF frame.
*
*/
template<class T>
inline T transform_frame_enu_ecef(const T &in) {
return detail::transform_static_frame(in, StaticTF::ENU_TO_ECEF);
}

/**
* @brief Transform data expressed in aircraft frame to NED frame.
* Assumes quaternion represents rotation from aircraft frame to NED frame.
Expand Down
45 changes: 45 additions & 0 deletions mavros/src/lib/ftf_frame_conversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,13 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat

Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform)
{
Eigen::Matrix3d R;

const double sin_lat = std::sin(vec.x());
const double sin_lon = std::sin(vec.y());
const double cos_lat = std::cos(vec.x());
const double cos_lon = std::cos(vec.y());

switch (transform) {
case StaticTF::NED_TO_ENU:
case StaticTF::ENU_TO_NED:
Expand All @@ -90,6 +97,44 @@ Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticT
case StaticTF::AIRCRAFT_TO_BASELINK:
case StaticTF::BASELINK_TO_AIRCRAFT:
return AIRCRAFT_BASELINK_AFFINE * vec;

case StaticTF::ECEF_TO_ENU:
/**
* @brief Apply transform from ECEF to ENU:
* ϕ = latitude
* λ = longitude
* The rotation is composed by a counter-clockwise rotation over the Z-axis
* by an angle of π - ϕ followed by a counter-clockwise over the same axis by
* an angle of π - λ
* R = [-sinλ cosλ 0.0
* -cosλ*sinϕ -sinλ*sinϕ cosϕ
* cosλ*cosϕ sinλ*cosϕ sinϕ ]
* East, North, Up = R * [∂x, ∂y, ∂z]
*/
R << -sin_lon, cos_lon, 0.0,
-cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat,
cos_lon * cos_lat, sin_lon * cos_lat, sin_lat;

return R * vec;

case StaticTF::ENU_TO_ECEF:
/**
* @brief Apply transform from ENU to ECEF:
* ϕ = x
* λ = y
* The rotation is composed by a clockwise rotation over the east-axis
* by an angle of π - ϕ followed by a clockwise over the Z-axis by
* an angle of π + λ
* R = [-sinλ -cosλ*sinϕ cosλ*cosϕ
* cosϕ -sinλ*sinϕ sinλ*cosϕ
* 0.0 cosϕ sinϕ ]
* x, y, z = R * [∂x, ∂y, ∂z]
*/
R << -sin_lon, -cos_lon * sin_lat, cos_lon * cos_lat,
cos_lat, -sin_lon * sin_lat, sin_lon * cos_lat,
0.0, cos_lat, sin_lat;

return R * vec;
}
}

Expand Down
34 changes: 1 addition & 33 deletions mavros/src/plugins/global_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,38 +144,6 @@ class GlobalPositionPlugin : public plugin::PluginBase {
fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}

inline Eigen::Vector3d ecef_to_enu_transform(Eigen::Vector3d &map_point)
{
/**
* @brief Apply transform from ECEF to ENU:
*
* ϕ = latitude
* λ = longitude
*
* The rotation is composed by a counter-clockwise rotation over the Z-axis
* by an angle of π - ϕ followed by a counter-clockwise over the same axis by
* an angle of π - λ
*
* R = [-sinλ cosλ 0.0
* -cosλ*sinϕ -sinλ*sinϕ cosϕ
* cosλ*cosϕ sinλ*cosϕ sinϕ]
*
* East, North, Up = R * [∂x, ∂y, ∂z]
*/
Eigen::Matrix3d R;

const double sin_lat = std::sin(map_point.x());
const double sin_lon = std::sin(map_point.y());
const double cos_lat = std::cos(map_point.x());
const double cos_lon = std::cos(map_point.y());

R << -sin_lon, cos_lon, 0.0,
-cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat,
cos_lon * cos_lat, sin_lon * cos_lat, sin_lat;

return R * map_point;
}

/* -*- message handlers -*- */

void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &raw_gps)
Expand Down Expand Up @@ -361,7 +329,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
}

tf::pointEigenToMsg(ecef_to_enu_transform(local_ecef), odom->pose.pose.position);
tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef), odom->pose.pose.position);

/**
* @brief By default, we are using the relative altitude instead of the geocentric
Expand Down
40 changes: 4 additions & 36 deletions mavros_extras/src/plugins/fake_gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,38 +164,6 @@ class FakeGPSPlugin : public plugin::PluginBase,
Eigen::Vector3d old_ecef; //!< previous geocentric position [m]
double old_stamp; //!< previous stamp [s]

inline Eigen::Vector3d enu_to_ecef_transform(const Eigen::Affine3d &map_point)
{
/**
* @brief Apply transform from ENU to ECEF:
*
* ϕ = x
* λ = y
*
* The rotation is composed by a clockwise rotation over the east-axis
* by an angle of π - ϕ followed by a clockwise over the Z-axis by
* an angle of π + λ
*
* R = [-sinλ -cosλ*sinϕ cosλ*cosϕ
* cosϕ -sinλ*sinϕ sinλ*cosϕ
* 0.0 cosϕ sinϕ ]
*
* x, y, z = R * [∂x, ∂y, ∂z]
*/
Eigen::Matrix3d R;

const double sin_x = std::sin(map_point.translation().x());
const double sin_y = std::sin(map_point.translation().y());
const double cos_x = std::cos(map_point.translation().x());
const double cos_y = std::cos(map_point.translation().z());

R << -sin_y, -cos_y * sin_x, cos_y * cos_x,
cos_x, -sin_y * sin_x, sin_y * cos_x,
0.0, cos_x, sin_x;

return R * map_point.translation();
}

/* -*- mid-level helpers and low-level send -*- */

/**
Expand Down Expand Up @@ -268,23 +236,23 @@ class FakeGPSPlugin : public plugin::PluginBase,
Eigen::Affine3d pos_enu;
tf::transformMsgToEigen(trans->transform, pos_enu);

send_fake_gps(trans->header.stamp, enu_to_ecef_transform(pos_enu));
send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
}

void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
{
Eigen::Affine3d pos_enu;
tf::poseMsgToEigen(req->pose, pos_enu);

send_fake_gps(req->header.stamp, enu_to_ecef_transform(pos_enu));
send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
}

void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
{
Eigen::Affine3d pos_enu;
tf::poseMsgToEigen(req->pose, pos_enu);

send_fake_gps(req->header.stamp, enu_to_ecef_transform(pos_enu));
send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
}

void transform_cb(const geometry_msgs::TransformStamped &trans)
Expand All @@ -293,7 +261,7 @@ class FakeGPSPlugin : public plugin::PluginBase,

tf::transformMsgToEigen(trans.transform, pos_enu);

send_fake_gps(trans.header.stamp, enu_to_ecef_transform(pos_enu));
send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
}
};
} // namespace extra_plugins
Expand Down

1 comment on commit e0a1173

@pavloblindnology
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ECEF <-> ENU convertions are totally screwed up.
Both convertions NEED map origin in LLA. I.e., they perform convertions between local ECEF and local ENU displacements with origin at (lat,lon,alt).
See, for example, here.
The rotation matrices are not symmetrical.
Pull requested #847

Please sign in to comment.