Adding conversion from WGS84 to ECEF. (#660)

This converts from latitude, longitude, altitude
to a cartesian coordinate frame.

[RFC=0007](https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md)

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -249,4 +249,25 @@ geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) {
   return point;
 }
 
+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_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 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);
+}
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -70,6 +70,10 @@ Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
 
 Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
 
+// Converts from WGS84 (latitude, longitude, altitude) to ECEF.
+Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
+                                 double altitude);
+
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_MSG_CONVERSION_H_

cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -81,5 +81,33 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
       point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), 1e-6));
 }
 
+TEST(MsgConversion, LatLongAltToEcef) {
+  Eigen::Vector3d equator_prime_meridian = LatLongAltToEcef(0, 0, 0);
+  EXPECT_TRUE(equator_prime_meridian.isApprox(Eigen::Vector3d(6378137, 0, 0)))
+      << equator_prime_meridian;
+  Eigen::Vector3d plus_ten_meters = LatLongAltToEcef(0, 0, 10);
+  EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0)))
+      << plus_ten_meters;
+  Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0);
+  EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9))
+      << north_pole;
+  Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0);
+  EXPECT_TRUE(
+      also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9))
+      << also_north_pole;
+  Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0);
+  EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), 1e-9))
+      << south_pole;
+  Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20);
+  EXPECT_TRUE(
+      above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), 1e-9))
+      << above_south_pole;
+  Eigen::Vector3d somewhere_on_earth =
+      LatLongAltToEcef(48.1372149, 11.5748024, 517.1);
+  EXPECT_TRUE(somewhere_on_earth.isApprox(
+      Eigen::Vector3d(4177983, 855702, 4727457), 1e-6))
+      << somewhere_on_earth;
+}
+
 }  // namespace
 }  // namespace cartographer_ros