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/cartographer_ros/msg_conversion.cc

@@ -16,6 +16,9 @@
 
 #include "cartographer_ros/msg_conversion.h"
 
+#include <cmath>
+
+#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 a = 6378137.;  // semi-major axis, equator to center.
-  constexpr double f = 1 / 298.257223563;
+  constexpr double f = 1. / 298.257223563;
-  constexpr double b = a * (1 - f);  // semi-minor axis, pole to center.
+  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 = std::sin(cartographer::common::DegToRad(latitude));
-  const double sin_phi = sin(latitude * kDegreesToRadians);
+  const double cos_phi = std::cos(cartographer::common::DegToRad(latitude));
-  const double cos_phi = cos(latitude * kDegreesToRadians);
+  const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude));
-  const double sin_lambda = sin(longitude * kDegreesToRadians);
+  const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude));
-  const double cos_lambda = cos(longitude * kDegreesToRadians);
+  const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi);
-  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);
 }
+
+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

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_

cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -17,6 +17,7 @@
 #include "cartographer_ros/msg_conversion.h"
 
 #include <cmath>
+#include <random>
 
 #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<double> lat_distribution(-90., 90.);
+  std::uniform_real_distribution<double> long_distribution(-180., 180.);
+  std::uniform_real_distribution<double> 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