ENU<->ECEF transforms fix.

mavros/include/mavros/frame_tf.h

@@ -132,6 +132,14 @@ Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF tran
  */
 Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform);
 
+/**
+ * @brief Transform data expressed in one frame to another frame
+ * with additional map origin parameter.
+ *
+ * General function. Please use specialized variants.
+ */
+Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticTF transform);
+
 inline double transform_frame_yaw(double yaw) {
 	return -yaw;
 }
@@ -214,19 +222,25 @@ inline T transform_frame_baselink_aircraft(const T &in) {
 /**
  * @brief Transform data expressed in ECEF frame to ENU frame.
  *
+ * in - local ECEF coordinates [m],
+ * map_origin - geodetic origin [lla],
+ * returns local ENU coordinates [m].
  */
 template<class T>
-inline T transform_frame_ecef_enu(const T &in) {
-	return detail::transform_static_frame(in, StaticTF::ECEF_TO_ENU);
+inline T transform_frame_ecef_enu(const T &in, const T &map_origin) {
+	return detail::transform_static_frame(in, map_origin, StaticTF::ECEF_TO_ENU);
 }
 
 /**
  * @brief Transform data expressed in ENU frame to ECEF frame.
  *
+ * in - local ENU coordinates [m].
+ * map_origin - geodetic origin [lla],
+ * returns local ECEF coordinates [m].
  */
 template<class T>
-inline T transform_frame_enu_ecef(const T &in) {
-	return detail::transform_static_frame(in, StaticTF::ENU_TO_ECEF);
+inline T transform_frame_enu_ecef(const T &in, const T &map_origin) {
+	return detail::transform_static_frame(in, map_origin, StaticTF::ENU_TO_ECEF);
 }
 
 /**

mavros/src/lib/ftf_frame_conversions.cpp

@@ -82,13 +82,6 @@ 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:
@@ -97,44 +90,6 @@ 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;
 	}
 }
 
@@ -213,6 +168,47 @@ Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF tran
 	}
 }
 
+Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticTF transform)
+{
+	//! Degrees to radians
+	static constexpr double DEG_TO_RAD = (M_PI / 180.0);
+
+	// Don't forget to convert from degrees to radians
+	const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD);
+	const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD);
+	const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD);
+	const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD);
+
+	/**
+	 * @brief Compute transform from ECEF to ENU:
+	 * http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
+	 * ϕ = latitude
+	 * λ = longitude
+	 * The rotation is composed by a counter-clockwise rotation over the Z-axis
+	 * by an angle of 90 + λ followed by a counter-clockwise rotation over the east-axis by
+	 * an angle of 90 - ϕ.
+	 * R = [-sinλ         cosλ         0.0
+	 *      -cosλ*sinϕ   -sinλ*sinϕ    cosϕ
+	 *       cosλ*cosϕ    sinλ*cosϕ    sinϕ   ]
+	 * [East, North, Up] = R * [∂x, ∂y, ∂z]
+	 * where both [East, North, Up] and [∂x, ∂y, ∂z] are local coordinates relative to map origin.
+	 */
+	Eigen::Matrix3d R;
+	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;
+
+	switch (transform) {
+	case StaticTF::ECEF_TO_ENU:
+		return R * vec;
+
+	case StaticTF::ENU_TO_ECEF:
+		// ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose.
+		R.transposeInPlace();
+		return R * vec;
+	}
+}
+
 Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q)
 {
 	Eigen::Affine3d transformation(q);

mavros/src/plugins/global_position.cpp

@@ -126,8 +126,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 
 	double rot_cov;
 
-	Eigen::Vector3d map_origin {};	//!< origin of map frame
-	Eigen::Vector3d local_ecef {};	//!< local ECEF coordinates on map frame
+	Eigen::Vector3d map_origin {};	//!< geodetic origin of map frame [lla]
+	Eigen::Vector3d ecef_origin {};	//!< geocentric origin of map frame [m]
+	Eigen::Vector3d local_ecef {};	//!< local ECEF coordinates on map frame [m]
 
 	template<typename MsgT>
 	inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix)
@@ -289,14 +290,14 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		vel_cov_out.fill(0.0);
 		vel_cov_out(0) = -1.0;
 
-		// ECEF point in "map" frame
+		// Current fix in ECEF
 		Eigen::Vector3d map_point;
 
 		try {
 			/**
 			 * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
 			 *
-			 * Note: "map_origin" is the origin of "map" frame, in ECEF, and the local coordinates are
+			 * Note: "ecef_origin" is the origin of "map" frame, in ECEF, and the local coordinates are
 			 * in spherical coordinates, with the orientation in ENU (just like what is applied
 			 * on Gazebo)
 			 */
@@ -305,31 +306,33 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 
 			/**
 			 * @brief Checks if the "map" origin is set.
-			 * - If not, and the home position is also not received, it sets the current local ecef as the origin;
+			 * - If not, and the home position is also not received, it sets the current fix as the origin;
 			 * - If the home position is received, it sets the "map" origin;
 			 * - If the "map" origin is set, then it applies the rotations to the offset between the origin
 			 * and the current local geocentric coordinates.
 			 */
+			// Current fix to ECEF
+			map.Forward(fix->latitude, fix->longitude, fix->altitude,
+						map_point.x(), map_point.y(), map_point.z());
+
+			// Set the current fix as the "map" origin if it's not set
 			if (!is_map_init) {
-				map.Forward(fix->latitude, fix->longitude, fix->altitude,
-							map_origin.x(), map_origin.y(), map_origin.z());
+				map_origin.x() = fix->latitude;
+				map_origin.y() = fix->longitude;
+				map_origin.z() = fix->altitude;
 
-				local_ecef = map_origin;
+				ecef_origin = map_point; // Local position is zero
 				is_map_init = true;
 			}
-			// If origin is set, compute the local coordinates in ENU
-			else {
-				map.Forward(fix->latitude, fix->longitude, fix->altitude,
-							map_point.x(), map_point.y(), map_point.z());
-
-				local_ecef = map_origin - map_point;
-			}
 		}
 		catch (const std::exception& e) {
 			ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
 		}
 
-		tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef), odom->pose.pose.position);
+		// Compute the local coordinates in ECEF
+		local_ecef = map_point - ecef_origin;
+		// Compute the local coordinates in ENU
+		tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), odom->pose.pose.position);
 
 		/**
 		 * @brief By default, we are using the relative altitude instead of the geocentric
@@ -450,6 +453,21 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		map_origin.y() = req->geo.longitude;
 		map_origin.z() = req->geo.altitude;
 
+		try {
+			/**
+			 * @brief Conversion from geodetic coordinates (LLA) to ECEF (Earth-Centered, Earth-Fixed)
+			 */
+			GeographicLib::Geocentric map(GeographicLib::Constants::WGS84_a(),
+						GeographicLib::Constants::WGS84_f());
+
+			// map_origin to ECEF
+			map.Forward(map_origin.x(), map_origin.y(), map_origin.z(),
+						ecef_origin.x(), ecef_origin.y(), ecef_origin.z());
+		}
+		catch (const std::exception& e) {
+			ROS_INFO_STREAM("GP: Caught exception: " << e.what() << std::endl);
+		}
+
 		is_map_init = true;
 	}
 

mavros/test/test_frame_conversions.cpp

@@ -70,6 +70,58 @@ TEST(FRAME_TF, transform_static_frame__ned_to_enu_123)
 	EXPECT_NEAR(expected.z(), out.z(), epsilon);
 }
 
+TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_00)
+{
+	Eigen::Vector3d input(1, 2, 3);
+	Eigen::Vector3d map_origin(0, 0, 0);
+	Eigen::Vector3d expected(2, 3, 1);
+
+	auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ECEF_TO_ENU);
+
+	EXPECT_NEAR(expected.x(), out.x(), epsilon);
+	EXPECT_NEAR(expected.y(), out.y(), epsilon);
+	EXPECT_NEAR(expected.z(), out.z(), epsilon);
+}
+
+TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_00)
+{
+	Eigen::Vector3d input(1, 2, 3);
+	Eigen::Vector3d map_origin(0, 0, 0);
+	Eigen::Vector3d expected(3, 1, 2);
+
+	auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ENU_TO_ECEF);
+
+	EXPECT_NEAR(expected.x(), out.x(), epsilon);
+	EXPECT_NEAR(expected.y(), out.y(), epsilon);
+	EXPECT_NEAR(expected.z(), out.z(), epsilon);
+}
+
+TEST(FRAME_TF, transform_static_frame__ecef_to_enu_123_4030)
+{
+	Eigen::Vector3d input(1, 2, 3);
+	Eigen::Vector3d map_origin(40, 30, 0);
+	Eigen::Vector3d expected(1.23205080756887, 1.09867532044397, 3.35782122034753);
+
+	auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ECEF_TO_ENU);
+
+	EXPECT_NEAR(expected.x(), out.x(), epsilon);
+	EXPECT_NEAR(expected.y(), out.y(), epsilon);
+	EXPECT_NEAR(expected.z(), out.z(), epsilon);
+}
+
+TEST(FRAME_TF, transform_static_frame__enu_to_ecef_123_4030)
+{
+	Eigen::Vector3d input(1, 2, 3);
+	Eigen::Vector3d map_origin(40, 30, 0);
+	Eigen::Vector3d expected(0.3769010460539777, 1.37230445877637, 3.46045171529757);
+
+	auto out = ftf::detail::transform_static_frame(input, map_origin, ftf::StaticTF::ENU_TO_ECEF);
+
+	EXPECT_NEAR(expected.x(), out.x(), epsilon);
+	EXPECT_NEAR(expected.y(), out.y(), epsilon);
+	EXPECT_NEAR(expected.z(), out.z(), epsilon);
+}
+
 TEST(FRAME_TF, quaternion_transforms__ned_to_ned_123)
 {
 	auto input_aircraft_ned_orient = ftf::quaternion_from_rpy(1.0, 2.0, 3.0);

mavros_extras/src/plugins/fake_gps.cpp

@@ -242,23 +242,23 @@ class FakeGPSPlugin : public plugin::PluginBase,
 		Eigen::Affine3d pos_enu;
 		tf::transformMsgToEigen(trans->transform, pos_enu);
 
-		send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
+		send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
 	}
 
 	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, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
+		send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
 	}
 
 	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, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
+		send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
 	}
 
 	void transform_cb(const geometry_msgs::TransformStamped &trans)
@@ -267,7 +267,7 @@ class FakeGPSPlugin : public plugin::PluginBase,
 
 		tf::transformMsgToEigen(trans.transform, pos_enu);
 
-		send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation())));
+		send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
 	}
 };
 }	// namespace extra_plugins