From b0a937b7ed6a910193b1be534d179196f5f9480d Mon Sep 17 00:00:00 2001 From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com> Date: Fri, 12 Jan 2018 11:39:29 +0100 Subject: [PATCH] Transform from ECEF to a local frame where z points up. (#662) For a given latitude and longitude, return a transformation that takes a point in ECEF coordinates to a local frame, where the z axis points up. PAIR=wohe [RFC=0007](https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md) --- .../cartographer_ros/msg_conversion.cc | 32 ++++++++++---- .../cartographer_ros/msg_conversion.h | 5 +++ .../cartographer_ros/msg_conversion_test.cc | 42 +++++++++++++++++++ 3 files changed, 70 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index e03355b..07c9501 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -16,6 +16,9 @@ #include "cartographer_ros/msg_conversion.h" +#include + +#include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/transform/proto/transform.pb.h" @@ -252,22 +255,33 @@ geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) { Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude, const double altitude) { // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates - constexpr double a = 6378137; // semi-major axis, equator to center. - constexpr double f = 1 / 298.257223563; - constexpr double b = a * (1 - f); // semi-minor axis, pole to center. + constexpr double a = 6378137.; // semi-major axis, equator to center. + constexpr double f = 1. / 298.257223563; + constexpr double b = a * (1. - f); // semi-minor axis, pole to center. constexpr double a_squared = a * a; constexpr double b_squared = b * b; constexpr double e_squared = (a_squared - b_squared) / a_squared; - constexpr double kDegreesToRadians = M_PI / 180; - const double sin_phi = sin(latitude * kDegreesToRadians); - const double cos_phi = cos(latitude * kDegreesToRadians); - const double sin_lambda = sin(longitude * kDegreesToRadians); - const double cos_lambda = cos(longitude * kDegreesToRadians); - const double N = a / sqrt(1 - e_squared * sin_phi * sin_phi); + const double sin_phi = std::sin(cartographer::common::DegToRad(latitude)); + const double cos_phi = std::cos(cartographer::common::DegToRad(latitude)); + const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude)); + const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude)); + const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi); const double x = (N + altitude) * cos_phi * cos_lambda; const double y = (N + altitude) * cos_phi * sin_lambda; const double z = (b_squared / a_squared * N + altitude) * sin_phi; return Eigen::Vector3d(x, y, z); } + +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong( + const double latitude, const double longitude) { + const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.); + const Eigen::Quaterniond rotation = + Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.), + Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude), + Eigen::Vector3d::UnitZ()); + return cartographer::transform::Rigid3d(rotation * -translation, rotation); +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index 242998c..c66ddef 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -74,6 +74,11 @@ Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude, double altitude); +// Returns a transform that takes ECEF coordinates from nearby points to a local +// frame that has z pointing upwards. +cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude, + double longitude); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_MSG_CONVERSION_H_ diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 4b68388..576bcdd 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -17,6 +17,7 @@ #include "cartographer_ros/msg_conversion.h" #include +#include #include "gtest/gtest.h" #include "sensor_msgs/LaserScan.h" @@ -109,5 +110,46 @@ TEST(MsgConversion, LatLongAltToEcef) { << somewhere_on_earth; } +TEST(MsgConversion, ComputeLocalFrameFromLatLong) { + cartographer::transform::Rigid3d north_pole = + ComputeLocalFrameFromLatLong(90., 0.); + EXPECT_TRUE((north_pole * LatLongAltToEcef(90., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())); + cartographer::transform::Rigid3d south_pole = + ComputeLocalFrameFromLatLong(-90., 0.); + EXPECT_TRUE((south_pole * LatLongAltToEcef(-90., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())); + cartographer::transform::Rigid3d prime_meridian_equator = + ComputeLocalFrameFromLatLong(0., 0.); + EXPECT_TRUE((prime_meridian_equator * LatLongAltToEcef(0., 0., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())) + << prime_meridian_equator * LatLongAltToEcef(0., 0., 1.); + cartographer::transform::Rigid3d meridian_90_equator = + ComputeLocalFrameFromLatLong(0., 90.); + EXPECT_TRUE((meridian_90_equator * LatLongAltToEcef(0., 90., 1.)) + .isApprox(Eigen::Vector3d::UnitZ())) + << meridian_90_equator * LatLongAltToEcef(0., 90., 1.); + + std::mt19937 rng(42); + std::uniform_real_distribution lat_distribution(-90., 90.); + std::uniform_real_distribution long_distribution(-180., 180.); + std::uniform_real_distribution alt_distribution(-519., 519.); + + for (int i = 0; i < 1000; ++i) { + const double latitude = lat_distribution(rng); + const double longitude = long_distribution(rng); + const double altitude = alt_distribution(rng); + cartographer::transform::Rigid3d transform_to_local_frame = + ComputeLocalFrameFromLatLong(latitude, longitude); + EXPECT_TRUE((transform_to_local_frame * + LatLongAltToEcef(latitude, longitude, altitude)) + .isApprox(altitude * Eigen::Vector3d::UnitZ(), 1e-6)) + << transform_to_local_frame * + LatLongAltToEcef(latitude, longitude, altitude) + << "\n" + << altitude; + } +} + } // namespace } // namespace cartographer_ros