Localization: use SIMD to accelerate lidar localization & fix map pool release bug (#1982)

* localization: update lidar channel to lidar128

* localization: fix warnings in lossy_map_matrix_2d.cc

* localization: support selecting map from DV

* localization: support selecting map from DV

* localization: update localizaiion.dag

* localization: support double-antena static initialization

* localization: change msf_config.proto to gflags

* localization: change threadpool to cyber::task

* localization: change local_map test style

* localization: change unit test style

* localization: implement frame_transform with proj4 and add avx support for lidar localization

* localization: implement frame_transform with proj4 and add avx support for lidar localization

* localization: fix bug

* localization: fix map pool release bug and set if_use_avx = true

modules/localization/common/localization_gflags.cc

@@ -89,6 +89,9 @@ DEFINE_double(lidar_map_coverage_theshold, 0.9,
               "Threshold to detect wether vehicle is out of map");
 DEFINE_bool(lidar_debug_log_flag, false, "Lidar Debug switch.");
 DEFINE_int32(point_cloud_step, 2, "Point cloud step");
+DEFINE_bool(if_use_avx, false,
+            "if use avx to accelerate lidar localization, "
+            "need cpu to support AVX intel instrinsics");
 
 // integ module
 DEFINE_bool(integ_ins_can_self_align, false, "");

modules/localization/common/localization_gflags.h

@@ -61,6 +61,7 @@ DECLARE_double(lidar_imu_max_delay_time);
 DECLARE_double(lidar_map_coverage_theshold);
 DECLARE_bool(lidar_debug_log_flag);
 DECLARE_int32(point_cloud_step);
+DECLARE_bool(if_use_avx);
 
 // integ module
 DECLARE_bool(integ_ins_can_self_align);

modules/localization/conf/localization.conf

@@ -201,3 +201,8 @@
 
 # The tem folder for localziation visualziation
 --map_visual_dir=data/map_visual
+
+# if use avx to accelerate lidar localization
+# need cpu to support avx intel intrinsics
+# default: false
+--if_use_avx=true

modules/localization/msf/common/util/BUILD

@@ -34,13 +34,10 @@ cc_library(
     name = "localization_msf_common_util_frame_transform",
     srcs = ["frame_transform.cc"],
     hdrs = ["frame_transform.h"],
-    linkopts = [
-        "-lboost_filesystem",
-        "-lboost_system",
-    ],
     deps = [
         "//cyber",
         "@eigen",
+        "@proj4",
     ],
 )
 

modules/localization/msf/common/util/frame_transform.cc

@@ -14,443 +14,107 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include <cmath>
-#include "cyber/common/log.h"
 #include "modules/localization/msf/common/util/frame_transform.h"
+#include <sstream>
+#include <string>
 
 namespace apollo {
 namespace localization {
 namespace msf {
 
-/* Ellipsoid model constants (actual values here are for WGS84) */
-const double kSmA = 6378137.0;
-const double kSmB = 6356752.31425;
-const double kUtmScaleFactor = 0.9996;
-const double kSinsRadToDeg = 57.295779513;
-const double kSinsDegToRad = 0.01745329252;
-
-const double kSinsR0 = 6378137.0;
-const double kSinsE = 0.08181919108425;
-const double kSinsE2 = 0.00669437999013;
-
-/*
- * Arclength0fMeridian
- *
- * Computes the ellipsoidal distance from the equator to a point at a
- * given latitude.
- *
- * Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
- * GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
- *
- * Inputs:
- *     phi - Latitude of the point, in radians.
- *
- * Globals:
- *     kSmA - Ellipsoid model major axis.
- *     kSmB - Ellipsoid model minor axis.
- *
- * Returns:
- *     The ellipsoidal distance of the point from the equator, in meters.
- *
- */
-static double Arclength0fMeridian(const double phi) {
-  /* Precalculate n */
-  double n = (kSmA - kSmB) / (kSmA + kSmB);
-
-  /* Precalculate alpha */
-  double alpha = ((kSmA + kSmB) / 2.0) *
-                 (1.0 + (pow(n, 2.0) / 4.0) + (pow(n, 4.0) / 64.0));
-
-  /* Precalculate beta */
-  double beta = (-3.0 * n / 2.0) + (9.0 * pow(n, 3.0) / 16.0) +
-                (-3.0 * pow(n, 5.0) / 32.0);
-
-  /* Precalculate gamma */
-  double gamma = (15.0 * pow(n, 2.0) / 16.0) + (-15.0 * pow(n, 4.0) / 32.0);
-
-  /* Precalculate delta */
-  double delta = (-35.0 * pow(n, 3.0) / 48.0) + (105.0 * pow(n, 5.0) / 256.0);
-
-  /* Precalculate epsilon */
-  double epsilon = (315.0 * pow(n, 4.0) / 512.0);
-
-  /* Now calculate the sum of the series and return */
-  double result =
-      alpha * (phi + (beta * sin(2.0 * phi)) + (gamma * sin(4.0 * phi)) +
-               (delta * sin(6.0 * phi)) + (epsilon * sin(8.0 * phi)));
-
-  return result;
-}
-
-/*
- * UtmCentralMeridian
- *
- * Determines the central meridian for the given UTM zone.
- *
- * Inputs:
- *     zone - An integer value designating the UTM zone, range [1,60].
- *
- * Returns:
- *   The central meridian for the given UTM zone, in radians, or zero
- *   if the UTM zone parameter is outside the range [1,60].
- *   Range of the central meridian is the radian equivalent of [-177,+177].
- *
- */
-inline double UtmCentralMeridian(const int zone) {
-  return (-183.0 + (zone * 6.0)) * kSinsDegToRad;
-}
-
-/*
- * FootpointLatitude
- *
- * Computes the footpoint latitude for use in converting transverse
- * Mercator coordinates to ellipsoidal coordinates.
- *
- * Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
- *   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
- *
- * Inputs:
- *   y - The UTM northing coordinate, in meters.
- *
- * Returns:
- *   The footpoint latitude, in radians.
- *
- */
-double FootpointLatitude(const double y) {
-  /* Precalculate n (Eq. 10.18) */
-  double n = (kSmA - kSmB) / (kSmA + kSmB);
-
-  /* Precalculate alpha (Eq. 10.22) */
-  /* (Same as alpha in Eq. 10.17) */
-  double alpha =
-      ((kSmA + kSmB) / 2.0) * (1 + (pow(n, 2.0) / 4) + (pow(n, 4.0) / 64));
-
-  /* Precalculate yy (Eq. 10.23) */
-  double yy = y / alpha;
-
-  /* Precalculate beta (Eq. 10.22) */
-  double beta = (3.0 * n / 2.0) + (-27.0 * pow(n, 3.0) / 32.0) +
-                (269.0 * pow(n, 5.0) / 512.0);
-
-  /* Precalculate gamma (Eq. 10.22) */
-  double gamma = (21.0 * pow(n, 2.0) / 16.0) + (-55.0 * pow(n, 4.0) / 32.0);
-
-  /* Precalculate delta (Eq. 10.22) */
-  double delta = (151.0 * pow(n, 3.0) / 96.0) + (-417.0 * pow(n, 5.0) / 128.0);
-
-  /* Precalculate epsilon (Eq. 10.22) */
-  double epsilon = (1097.0 * pow(n, 4.0) / 512.0);
-
-  /* Now calculate the sum of the series (Eq. 10.21) */
-  double result = yy + (beta * sin(2.0 * yy)) + (gamma * sin(4.0 * yy)) +
-                  (delta * sin(6.0 * yy)) + (epsilon * sin(8.0 * yy));
-
-  return result;
-}
-
-/*
- * MaplatlonToXY
- *
- * Converts a latitude/longitude pair to x and y coordinates in the
- * Transverse Mercator projection.  Note that Transverse Mercator is not
- * the same as UTM; a scale factor is required to convert between them.
- *
- * Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
- * GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
- *
- * Inputs:
- *    phi - Latitude of the point, in radians.
- *    lambda - Longitude of the point, in radians.
- *    lambda0 - Longitude of the central meridian to be used, in radians.
- *
- * Outputs:
- *    xy - A 2-element array containing the x and y coordinates
- *         of the computed point.
- *
- * Returns:
- *    The function does not return a value.
- *
- */
-void MaplatlonToXY(const double phi, const double lambda, double lambda0,
-                   UTMCoor *xy) {
-  CHECK_NOTNULL(xy);
-
-  /* Precalculate ep2 */
-  double ep2 = (pow(kSmA, 2.0) - pow(kSmB, 2.0)) / pow(kSmB, 2.0);
-
-  /* Precalculate nu2 */
-  double nu2 = ep2 * pow(cos(phi), 2.0);
-
-  /* Precalculate nn */
-  double nn = pow(kSmA, 2.0) / (kSmB * sqrt(1 + nu2));
-
-  /* Precalculate t */
-  double t = tan(phi);
-  double t2 = t * t;
-  // double tmp = (t2 * t2 * t2) - pow(t, 6.0);
-
-  /* Precalculate l */
-  double l = lambda - lambda0;
-
-  /* Precalculate coefficients for l**nn in the equations below
-   so a normal human being can read the expressions for easting
-   and northing
-   -- l**1 and l**2 have coefficients of 1.0 */
-  double l3coef = 1.0 - t2 + nu2;
-
-  double l4coef = 5.0 - t2 + 9 * nu2 + 4.0 * (nu2 * nu2);
-
-  double l5coef = 5.0 - 18.0 * t2 + (t2 * t2) + 14.0 * nu2 - 58.0 * t2 * nu2;
-
-  double l6coef = 61.0 - 58.0 * t2 + (t2 * t2) + 270.0 * nu2 - 330.0 * t2 * nu2;
-
-  double l7coef = 61.0 - 479.0 * t2 + 179.0 * (t2 * t2) - (t2 * t2 * t2);
-
-  double l8coef = 1385.0 - 3111.0 * t2 + 543.0 * (t2 * t2) - (t2 * t2 * t2);
-
-  /* Calculate easting (x) */
-  xy->x = nn * cos(phi) * l +
-          (nn / 6.0 * pow(cos(phi), 3.0) * l3coef * pow(l, 3.0)) +
-          (nn / 120.0 * pow(cos(phi), 5.0) * l5coef * pow(l, 5.0)) +
-          (nn / 5040.0 * pow(cos(phi), 7.0) * l7coef * pow(l, 7.0));
-
-  /* Calculate northing (y) */
-  xy->y = Arclength0fMeridian(phi) +
-          (t / 2.0 * nn * pow(cos(phi), 2.0) * pow(l, 2.0)) +
-          (t / 24.0 * nn * pow(cos(phi), 4.0) * l4coef * pow(l, 4.0)) +
-          (t / 720.0 * nn * pow(cos(phi), 6.0) * l6coef * pow(l, 6.0)) +
-          (t / 40320.0 * nn * pow(cos(phi), 8.0) * l8coef * pow(l, 8.0));
-  return;
-}
-
-/*
- * MapXYToLatlon
- *
- * Converts x and y coordinates in the Transverse Mercator projection to
- * a latitude/longitude pair.  Note that Transverse Mercator is not
- * the same as UTM; a scale factor is required to convert between them.
- *
- * Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
- *   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
- *
- * Inputs:
- *   x - The easting of the point, in meters.
- *   y - The northing of the point, in meters.
- *   lambda0 - Longitude of the central meridian to be used, in radians.
- *
- * Outputs:
- *   philambda - A 2-element containing the latitude and longitude
- *               in radians.
- *
- * Returns:
- *   The function does not return a value.
- *
- * Remarks:
- *   The local variables Nf, nuf2, tf, and tf2 serve the same purpose as
- *   N, nu2, t, and t2 in MapLatLonToXY, but they are computed with respect
- *   to the footpoint latitude phif.
- *
- *   x1frac, x2frac, x2poly, x3poly, etc. are to enhance readability and
- *   to optimize computations.
- *
- */
-void MapXYToLatlon(const double x, const double y, const double lambda0,
-                   WGS84Corr *philambda) {
-  CHECK_NOTNULL(philambda);
-
-  /* Get the value of phif, the footpoint latitude. */
-  double phif = FootpointLatitude(y);
-
-  /* Precalculate ep2 */
-  double ep2 = (pow(kSmA, 2.0) - pow(kSmB, 2.0)) / pow(kSmB, 2.0);
-
-  /* Precalculate cos (phif) */
-  double cf = cos(phif);
-
-  /* Precalculate nuf2 */
-  double nuf2 = ep2 * pow(cf, 2.0);
-
-  /* Precalculate nf and initialize nfpow */
-  double nf = pow(kSmA, 2.0) / (kSmB * sqrt(1 + nuf2));
-  double nfpow = nf;
-
-  /* Precalculate tf */
-  double tf = tan(phif);
-  double tf2 = tf * tf;
-  double tf4 = tf2 * tf2;
-
-  /* Precalculate fractional coefficients for x**n in the equations
-   below to simplify the expressions for latitude and longitude. */
-  double x1frac = 1.0 / (nfpow * cf);
-
-  nfpow *= nf; /* now equals nf**2) */
-  double x2frac = tf / (2.0 * nfpow);
-
-  nfpow *= nf; /* now equals nf**3) */
-  double x3frac = 1.0 / (6.0 * nfpow * cf);
-
-  nfpow *= nf; /* now equals nf**4) */
-  double x4frac = tf / (24.0 * nfpow);
-
-  nfpow *= nf; /* now equals nf**5) */
-  double x5frac = 1.0 / (120.0 * nfpow * cf);
-
-  nfpow *= nf; /* now equals nf**6) */
-  double x6frac = tf / (720.0 * nfpow);
-
-  nfpow *= nf; /* now equals nf**7) */
-  double x7frac = 1.0 / (5040.0 * nfpow * cf);
-
-  nfpow *= nf; /* now equals nf**8) */
-  double x8frac = tf / (40320.0 * nfpow);
-
-  /* Precalculate polynomial coefficients for x**n.
-   -- x**1 does not have a polynomial coefficient. */
-  double x2poly = -1.0 - nuf2;
-
-  double x3poly = -1.0 - 2 * tf2 - nuf2;
-
-  double x4poly = 5.0 + 3.0 * tf2 + 6.0 * nuf2 - 6.0 * tf2 * nuf2 -
-                  3.0 * (nuf2 * nuf2) - 9.0 * tf2 * (nuf2 * nuf2);
-
-  double x5poly = 5.0 + 28.0 * tf2 + 24.0 * tf4 + 6.0 * nuf2 + 8.0 * tf2 * nuf2;
-
-  double x6poly =
-      -61.0 - 90.0 * tf2 - 45.0 * tf4 - 107.0 * nuf2 + 162.0 * tf2 * nuf2;
-
-  double x7poly = -61.0 - 662.0 * tf2 - 1320.0 * tf4 - 720.0 * (tf4 * tf2);
-
-  double x8poly = 1385.0 + 3633.0 * tf2 + 4095.0 * tf4 + 1575 * (tf4 * tf2);
-
-  /* Calculate latitude */
-  philambda->lat =
-      phif + x2frac * x2poly * (x * x) + x4frac * x4poly * pow(x, 4.0) +
-      x6frac * x6poly * pow(x, 6.0) + x8frac * x8poly * pow(x, 8.0);
-
-  /* Calculate longitude */
-  philambda->log = lambda0 + x1frac * x + x3frac * x3poly * pow(x, 3.0) +
-                   x5frac * x5poly * pow(x, 5.0) +
-                   x7frac * x7poly * pow(x, 7.0);
-  return;
-}
-
-/*
- * LatlonToUtmXY
- *
- * Converts a latitude/longitude pair to x and y coordinates in the
- * Universal Transverse Mercator projection.
- *
- * Inputs:
- *   lat - Latitude of the point, in radians.
- *   lon - Longitude of the point, in radians.
- *   zone - UTM zone to be used for calculating values for x and y.
- *          If zone is less than 1 or greater than 60, the routine
- *          will determine the appropriate zone from the value of lon.
- *
- * Outputs:
- *   xy - A 2-element array where the UTM x and y values will be stored.
- *
- * Returns:
- *   void
- *
- */
-void LatlonToUtmXY(const double lon_rad, const double lat_rad, UTMCoor *xy) {
-  CHECK_NOTNULL(xy);
-
+bool FrameTransform::LatlonToUtmXY(double lon_rad, double lat_rad,
+                                   UTMCoor *utm_xy) {
+  projPJ pj_latlon;
+  projPJ pj_utm;
   int zone = 0;
-  zone = static_cast<int>((lon_rad * kSinsRadToDeg + 180) / 6) + 1;
-
-  MaplatlonToXY(lat_rad, lon_rad, UtmCentralMeridian(zone), xy);
-
-  /* Adjust easting and northing for UTM system. */
-  xy->x = xy->x * kUtmScaleFactor + 500000.0;
-  xy->y = xy->y * kUtmScaleFactor;
-  if (xy->y < 0.0) {
-    xy->y += 10000000.0;
+  zone = static_cast<int>((lon_rad * RAD_TO_DEG + 180) / 6) + 1;
+  std::string latlon_src =
+      "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+  std::ostringstream utm_dst;
+  utm_dst << "+proj=utm +zone=" << zone << " +ellps=GRS80 +units=m +no_defs";
+  if (!(pj_latlon = pj_init_plus(latlon_src.c_str()))) {
+    return false;
   }
-  return;
+  if (!(pj_utm = pj_init_plus(utm_dst.str().c_str()))) {
+    return false;
+  }
+  double longitude = lon_rad;
+  double latitude = lat_rad;
+  pj_transform(pj_latlon, pj_utm, 1, 1, &longitude, &latitude, NULL);
+  utm_xy->x = longitude;
+  utm_xy->y = latitude;
+  pj_free(pj_latlon);
+  pj_free(pj_utm);
+  return true;
 }
-
-/*
- * UtmXYToLatlon
- *
- * Converts x and y coordinates in the Universal Transverse Mercator
- * projection to a latitude/longitude pair.
- *
- * Inputs:
- *	x - The easting of the point, in meters.
- *	y - The northing of the point, in meters.
- *	zone - The UTM zone in which the point lies.
- *	southhemi - True if the point is in the southern hemisphere;
- *               false otherwise.
- *
- * Outputs:
- *	latlon - A 2-element array containing the latitude and
- *            longitude of the point, in radians.
- *
- * Returns:
- *	The function does not return a value.
- *
- */
-void UtmXYToLatlon(const double x, const double y, const int zone,
-                   const bool southhemi, WGS84Corr *latlon) {
-  CHECK_NOTNULL(latlon);
-
-  double xx = x;
-  xx -= 500000.0;
-  xx /= kUtmScaleFactor;
-
-  /* If in southern hemisphere, adjust y accordingly. */
-  double yy = y;
-  if (southhemi) {
-    yy -= 10000000.0;
+bool FrameTransform::UtmXYToLatlon(double x, double y, int zone, bool southhemi,
+                                   WGS84Corr *latlon) {
+  projPJ pj_latlon;
+  projPJ pj_utm;
+  std::string latlon_src =
+      "+proj=longlat +ellps=GRS80 +towgs84=0,0,0,0,0,0,0 +no_defs";
+  std::ostringstream utm_dst;
+  utm_dst << "+proj=utm +zone=" << zone << " +ellps=GRS80 +units=m +no_defs";
+  if (!(pj_latlon = pj_init_plus(latlon_src.c_str()))) {
+    return false;
   }
-  yy /= kUtmScaleFactor;
-
-  double cmeridian = UtmCentralMeridian(zone);
-  MapXYToLatlon(xx, yy, cmeridian, latlon);
-  return;
+  if (!(pj_utm = pj_init_plus(utm_dst.str().c_str()))) {
+    return false;
+  }
+  pj_transform(pj_utm, pj_latlon, 1, 1, &x, &y, NULL);
+  latlon->log = x;
+  latlon->lat = y;
+  pj_free(pj_latlon);
+  pj_free(pj_utm);
+  return true;
 }
 
-void XYZToBlh(const Eigen::Vector3d &xyz, Eigen::Vector3d *blh) {
-  CHECK_NOTNULL(blh);
-
-  //    double e2=FE_WGS84*(2.0-FE_WGS84);
-  double r2 = xyz[0] * xyz[0] + xyz[1] * xyz[1];
-  double z = 0.0;
-  double zk = 0.0;
-  double v = kSinsR0;
-
-  for (z = xyz[2], zk = 0.0; fabs(z - zk) >= 1E-4;) {
-    zk = z;
-    const double sinp = z / sqrt(r2 + z * z);
-    v = kSinsR0 / sqrt(1.0 - kSinsE2 * sinp * sinp);
-    z = xyz[2] + v * kSinsE2 * sinp;
+bool FrameTransform::XYZToBlh(const Vector3d &xyz,
+                              Vector3d *blh) {
+  projPJ pj_xyz;
+  projPJ pj_blh;
+  std::string xyz_src = "+proj=geocent +datum=WGS84";
+  std::string blh_dst = "+proj=latlong +datum=WGS84";
+  if (!(pj_xyz = pj_init_plus(xyz_src.c_str()))) {
+    return false;
   }
-  (*blh)[0] = r2 > 1E-12 ? atan2(xyz[1], xyz[0]) : 0.0;
-  (*blh)[1] = r2 > 1E-12 ? atan(z / sqrt(r2))
-                         : (xyz[2] > 0.0 ? M_PI / 2.0 : -M_PI / 2.0);
-  (*blh)[2] = sqrt(r2 + z * z) - v;
-  return;
+  if (!(pj_blh = pj_init_plus(blh_dst.c_str()))) {
+    return false;
+  }
+  double x = xyz[0];
+  double y = xyz[1];
+  double z = xyz[2];
+  pj_transform(pj_xyz, pj_blh, 1, 1, &x, &y, &z);
+  (*blh)[0] = x;
+  (*blh)[1] = y;
+  (*blh)[2] = z;
+  pj_free(pj_xyz);
+  pj_free(pj_blh);
+  return true;
 }
-
-void BlhToXYZ(const Eigen::Vector3d &blh, Eigen::Vector3d *xyz) {
-  CHECK_NOTNULL(xyz);
-
-  double sin_lati_2 = sin(blh[1]) * sin(blh[1]);
-  double temp_a = sqrt(1.0 - kSinsE2 * sin_lati_2);
-  double rn = kSinsR0 / temp_a;
-  // double rm = rn * (1.0 - kSinsE2) / (1.0 - kSinsE2 * sin_lati_2);
-
-  double cos_lat = cos(blh[1]);
-  double sin_lat = sin(blh[1]);
-  double cos_long = cos(blh[0]);
-  double sin_long = sin(blh[0]);
-
-  (*xyz)[0] = (rn + blh[2]) * cos_lat * cos_long;
-  (*xyz)[1] = (rn + blh[2]) * cos_lat * sin_long;
-  (*xyz)[2] = ((1 - kSinsE2) * rn + blh[2]) * sin_lat;
-  return;
+bool FrameTransform::BlhToXYZ(const Vector3d &blh,
+                              Vector3d *xyz) {
+  projPJ pj_xyz;
+  projPJ pj_blh;
+  std::string blh_src = "+proj=latlong +datum=WGS84";
+  std::string xyz_dst = "+proj=geocent +datum=WGS84";
+
+  if (!(pj_blh = pj_init_plus(blh_src.c_str()))) {
+    return false;
+  }
+  if (!(pj_xyz = pj_init_plus(xyz_dst.c_str()))) {
+    return false;
+  }
+  double longitude = blh[0];
+  double latitude = blh[1];
+  double height = blh[2];
+  pj_transform(pj_blh, pj_xyz, 1, 1, &longitude, &latitude, &height);
+  (*xyz)[0] = longitude;
+  (*xyz)[1] = latitude;
+  (*xyz)[2] = height;
+  pj_free(pj_xyz);
+  pj_free(pj_blh);
+  return true;
 }
 
 }  // namespace msf

modules/localization/msf/common/util/frame_transform.h

@@ -13,17 +13,17 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
-
 #pragma once
 
-#include <Eigen/Core>
+#include <proj_api.h>
 #include <Eigen/Geometry>
-#include <cstdio>
 
 namespace apollo {
 namespace localization {
 namespace msf {
 
+typedef Eigen::Vector3d Vector3d;
+
 /**@brief the UTM coordinate struct including x and y. */
 struct UTMCoor {
   UTMCoor() : x(0.0), y(0.0) {}
@@ -39,15 +39,18 @@ struct WGS84Corr {
   double lat;      // latitude
 };
 
-void LatlonToUtmXY(const double lon, const double lat, UTMCoor *xy);
-
-void UtmXYToLatlon(const double x, const double y, const int zone,
-                     const bool southhemi, WGS84Corr *latlon);
+class FrameTransform {
+ public:
+  static bool LatlonToUtmXY(double lon, double lat, UTMCoor *utm_xy);
+  static bool UtmXYToLatlon(double x, double y, int zone, bool southhemi,
+                            WGS84Corr *latlon);
+  static bool XYZToBlh(const Vector3d& xyz, Vector3d *blh);
+  static bool BlhToXYZ(const Vector3d& blh, Vector3d *xyz);
 
-void XYZToBlh(const Eigen::Vector3d &xyz, Eigen::Vector3d *blh);
-
-void BlhToXYZ(const Eigen::Vector3d &blh, Eigen::Vector3d *xyz);
+//  static bool XyzToBlh(const Vector3d& xyz, Position *blh);
+//  static bool BlhToXyz(const Position& blh, Vector3d *xyz);
+};
 
-}   // namespace msf
-}   // namespace localization
-}   // namespace apollo
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_integ/localization_integ_impl.cc

@@ -595,7 +595,8 @@ void LocalizationIntegImpl::TransferGnssMeasureToLocalization(
   headerpb->set_timestamp_sec(timestamp);
 
   UTMCoor utm_xy;
-  LatlonToUtmXY(measure.gnss_pos.longitude, measure.gnss_pos.latitude, &utm_xy);
+  FrameTransform::LatlonToUtmXY(measure.gnss_pos.longitude,
+                                measure.gnss_pos.latitude, &utm_xy);
 
   apollo::common::PointENU* position = posepb->mutable_position();
   position->set_x(utm_xy.x);

modules/localization/msf/local_integ/localization_integ_process.cc

@@ -184,7 +184,8 @@ void LocalizationIntegProcess::GetResult(IntegState *state,
   apollo::common::PointENU *position_loc = posepb_loc->mutable_position();
   apollo::common::Quaternion *quaternion = posepb_loc->mutable_orientation();
   UTMCoor utm_xy;
-  LatlonToUtmXY(ins_pva_.pos.longitude, ins_pva_.pos.latitude, &utm_xy);
+  FrameTransform::LatlonToUtmXY(ins_pva_.pos.longitude, ins_pva_.pos.latitude,
+                                &utm_xy);
   position_loc->set_x(utm_xy.x);
   position_loc->set_y(utm_xy.y);
   position_loc->set_z(ins_pva_.pos.height);

modules/localization/msf/local_integ/localization_lidar.cc

@@ -118,7 +118,8 @@ void LocalizationLidar::SetDeltaPitchRollLimit(double limit) {
 int LocalizationLidar::Update(const unsigned int frame_idx,
                               const Eigen::Affine3d& pose,
                               const Eigen::Vector3d velocity,
-                              const LidarFrame& lidar_frame) {
+                              const LidarFrame& lidar_frame,
+                              bool use_avx) {
   // check whether loaded map
   if (is_map_loaded_ == false) {
     map_.LoadMapArea(pose.translation(), resolution_id_,
@@ -167,7 +168,8 @@ int LocalizationLidar::Update(const unsigned int frame_idx,
                                      pose_quat.x(),
                                      pose_quat.y(),
                                      pose_quat.z(),
-                                     pose_quat.w());
+                                     pose_quat.w(),
+                                     use_avx);
 
   return error;
 }

modules/localization/msf/local_integ/localization_lidar.h

@@ -102,7 +102,8 @@ typedef apollo::localization::msf::LossyMapConfig2D LossyMapConfig;
   void SetDeltaPitchRollLimit(double limit);
 
   int Update(const unsigned int frame_idx, const Eigen::Affine3d& pose,
-             const Eigen::Vector3d velocity, const LidarFrame& lidar_frame);
+             const Eigen::Vector3d velocity, const LidarFrame& lidar_frame,
+             bool use_avx = false);
 
   void GetResult(Eigen::Affine3d *location,
                  Eigen::Matrix3d *covariance,

modules/localization/msf/local_integ/localization_lidar_process.cc

@@ -87,6 +87,7 @@ Status LocalizationLidarProcess::Init(const LocalizationIntegParam& params) {
 
   is_unstable_reset_ = params.is_lidar_unstable_reset;
   unstable_threshold_ = params.unstable_reset_threshold;
+  if_use_avx_ = params.if_use_avx;
 
   lidar_status_ = LidarState::NOT_VALID;
 
@@ -197,7 +198,7 @@ void LocalizationLidarProcess::PcdProcess(const LidarFrame& lidar_frame) {
   velocity_ = cur_predict_location_.translation() - pre_location_.translation();
 
   int ret = locator_->Update(pcd_index++, cur_predict_location_, velocity_,
-                             lidar_frame);
+                             lidar_frame, if_use_avx_);
 
   UpdateState(ret, lidar_frame.measurement_time);
 

modules/localization/msf/local_integ/localization_lidar_process.h

@@ -143,6 +143,9 @@ class LocalizationLidarProcess {
 
   bool reinit_flag_ = false;
 
+  // if use avx to accelerate lidar localization algorithm
+  bool if_use_avx_ = false;
+
   // imu and lidar max delay time
   double imu_lidar_max_delay_time_ = 0.4;
 

modules/localization/msf/local_integ/localization_params.h

@@ -92,6 +92,7 @@ struct LocalizationIntegParam {
   int utm_zone_id = 50;
   bool is_lidar_unstable_reset = true;
   double unstable_reset_threshold = 0.3;
+  bool if_use_avx = false;
 
   bool is_using_novatel_heading = true;
   std::string ant_imu_leverarm_file = "";

modules/localization/msf/local_integ/measure_republish_process.cc

@@ -206,7 +206,7 @@ void MeasureRepublishProcess::GnssLocalProcess(
   pos_xyz[2] = measure_data.gnss_pos.height;
 
   Eigen::Vector3d pos_blh = Eigen::Vector3d::Zero();
-  apollo::localization::msf::XYZToBlh(pos_xyz, &pos_blh);
+  apollo::localization::msf::FrameTransform::XYZToBlh(pos_xyz, &pos_blh);
   measure_data.gnss_pos.longitude = pos_blh[0];
   measure_data.gnss_pos.latitude = pos_blh[1];
   measure_data.gnss_pos.height = pos_blh[2];
@@ -343,7 +343,7 @@ bool MeasureRepublishProcess::LidarLocalProcess(
   measure_data.time = lidar_local_msg.measurement_time();
 
   apollo::localization::msf::WGS84Corr temp_wgs;
-  apollo::localization::msf::UtmXYToLatlon(
+  apollo::localization::msf::FrameTransform::UtmXYToLatlon(
       lidar_local_msg.pose().position().x(),
       lidar_local_msg.pose().position().y(), local_utm_zone_id_, false,
       &temp_wgs);

modules/localization/msf/local_integ/measure_republish_process.h

@@ -119,8 +119,6 @@ class MeasureRepublishProcess {
 
   GnssMode gnss_mode_;
 
-  static constexpr double DEG_TO_RAD = 0.017453292519943;
-  static constexpr double RAD_TO_DEG = 57.295779513082323;
   static constexpr double GNSS_XY_STD_THRESHOLD = 5.0;
   static constexpr double BESTPOSE_TIME_MAX_INTERVAL = 1.05;
   static constexpr int BESTPOSE_GOOD_COUNT = 10;

modules/localization/msf/local_map/base_map/base_map_pool.cc

@@ -43,8 +43,11 @@ void BaseMapNodePool::Initial(const BaseMapConfig* map_config,
 }
 
 void BaseMapNodePool::Release() {
-  if (node_reset_workers_.valid()) {
-    node_reset_workers_.get();
+  while (node_reset_workers_.size() != 0) {
+    if (node_reset_workers_.front().valid()) {
+      node_reset_workers_.front().wait();
+      node_reset_workers_.pop();
+    }
   }
   typename std::list<BaseMapNode*>::iterator i = free_list_.begin();
   while (i != free_list_.end()) {
@@ -65,8 +68,12 @@ void BaseMapNodePool::Release() {
 
 BaseMapNode* BaseMapNodePool::AllocMapNode() {
   if (free_list_.empty()) {
-    if (node_reset_workers_.valid()) {
-      node_reset_workers_.wait();
+    while (node_reset_workers_.size() != 0) {
+      if (node_reset_workers_.front().valid()) {
+        node_reset_workers_.front().wait();
+        node_reset_workers_.pop();
+        break;
+      }
     }
   }
   boost::unique_lock<boost::mutex> lock(mutex_);
@@ -88,8 +95,8 @@ BaseMapNode* BaseMapNodePool::AllocMapNode() {
 }
 
 void BaseMapNodePool::FreeMapNode(BaseMapNode* map_node) {
-  node_reset_workers_ =
-    cyber::Async(&BaseMapNodePool::FreeMapNodeTask, this, map_node);
+  node_reset_workers_.push(
+    cyber::Async(&BaseMapNodePool::FreeMapNodeTask, this, map_node));
 }
 
 void BaseMapNodePool::FreeMapNodeTask(BaseMapNode* map_node) {

modules/localization/msf/local_map/base_map/base_map_pool.h

@@ -18,6 +18,7 @@
 
 #include <list>
 #include <set>
+#include <queue>
 
 #include "boost/thread.hpp"
 
@@ -83,7 +84,7 @@ class BaseMapNodePool {
   /**@brief The size of memory pool. */
   unsigned int pool_size_;
   /**@brief The thread pool for release node. */
-  std::future<void> node_reset_workers_;
+  std::queue<std::future<void>> node_reset_workers_;
   /**@brief The mutex for release thread.*/
   boost::mutex mutex_;
   /**@brief The mutex for release thread.*/

modules/localization/msf/msf_localization.cc

@@ -76,6 +76,7 @@ void MSFLocalization::InitParams() {
   localization_param_.lidar_filter_size = FLAGS_lidar_filter_size;
   localization_param_.map_coverage_theshold = FLAGS_lidar_map_coverage_theshold;
   localization_param_.imu_lidar_max_delay_time = FLAGS_lidar_imu_max_delay_time;
+  localization_param_.if_use_avx = FLAGS_if_use_avx;
 
   AINFO << "map: " << localization_param_.map_path;
   AINFO << "lidar_extrin: " << localization_param_.lidar_extrinsic_file;
@@ -148,6 +149,7 @@ void MSFLocalization::InitParams() {
     CHECK(LoadGnssAntennaExtrinsic(ant_imu_leverarm_file, &offset_x, &offset_y,
                                    &offset_z, &uncertainty_x, &uncertainty_y,
                                    &uncertainty_z));
+    localization_param_.ant_imu_leverarm_file = ant_imu_leverarm_file;
 
     localization_param_.imu_to_ant_offset.offset_x = offset_x;
     localization_param_.imu_to_ant_offset.offset_y = offset_y;