Added an option to force zones on conversionfrom WGS84 to utm. This will allow for smoother transitions across zonal boundaries (#43)

Co-authored-by: Peter Milani <[email protected]>

Author: PeterQFR

geodesy/include/geodesy/utm.h

@@ -100,7 +100,6 @@ class UTMPoint
     zone(that.zone),
     band(that.band)
   {}
-  
   UTMPoint(const geographic_msgs::msg::GeoPoint &pt);
 
   /** Create a flattened 2-D grid point. */
@@ -124,7 +123,7 @@ class UTMPoint
 
   // data members
   double easting;           ///< easting within grid zone [meters]
-  double northing;          ///< northing within grid zone [meters] 
+  double northing;          ///< northing within grid zone [meters]
   double altitude;          ///< altitude above ellipsoid [meters] or NaN
   uint8_t zone;             ///< UTM longitude zone number
   char   band;              ///< MGRS latitude band letter
@@ -147,7 +146,7 @@ class UTMPose
     position(that.position),
     orientation(that.orientation)
   {}
-  
+
   /** Create from a WGS 84 geodetic pose. */
   UTMPose(const geographic_msgs::msg::GeoPose &pose):
     position(pose.position),
@@ -175,8 +174,10 @@ class UTMPose
 }; // class UTMPose
 
 // conversion function prototypes
-void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to);
-void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to);
+void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to,
+        const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
+void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to,
+        const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
 geographic_msgs::msg::GeoPoint toMsg(const UTMPoint &from);
 geographic_msgs::msg::GeoPose toMsg(const UTMPose &from);
 

geodesy/src/conv/utm_conversions.cpp

@@ -39,7 +39,7 @@
 #include <geodesy/utm.h>
 
 /**  @file
-   
+
      @brief Universal Transverse Mercator transforms.
 
      Functions to convert WGS 84 (ellipsoidal) latitude and longitude
@@ -145,7 +145,7 @@ geographic_msgs::msg::GeoPoint toMsg(const UTMPoint &from)
   mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
              -5*eccSquared*eccSquared*eccSquared/256));
 
-  phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 
+  phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
                   + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
                   + (151*e1*e1*e1/96)*sin(6*mu));
 
@@ -183,7 +183,8 @@ geographic_msgs::msg::GeoPoint toMsg(const UTMPoint &from)
  *  @param from WGS 84 point message.
  *  @param to UTM point.
  */
-void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to)
+void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to,
+        const bool& force_zone, const char& band, const uint8_t& zone)
 {
   double Lat = from.latitude;
   double Long = from.longitude;
@@ -195,7 +196,7 @@ void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to)
   double LongOrigin;
   double eccPrimeSquared;
   double N, T, C, A, M;
-	
+
   // Make sure the longitude is between -180.00 .. 179.9
   // (JOQ: this is broken for Long < -180, do a real normalize)
   double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
@@ -204,25 +205,34 @@ void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to)
   double LongOriginRad;
 
   to.altitude = from.altitude;
-  to.zone = int((LongTemp + 180)/6) + 1;
-  
+  if (!force_zone)
+    to.zone = int((LongTemp + 180)/6) + 1;
+  else
+    to.zone = zone;
+
   if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
     to.zone = 32;
 
   // Special zones for Svalbard
-  if( Lat >= 72.0 && Lat < 84.0 ) 
+  if( Lat >= 72.0 && Lat < 84.0 )
     {
       if(      LongTemp >= 0.0  && LongTemp <  9.0 ) to.zone = 31;
       else if( LongTemp >= 9.0  && LongTemp < 21.0 ) to.zone = 33;
       else if( LongTemp >= 21.0 && LongTemp < 33.0 ) to.zone = 35;
       else if( LongTemp >= 33.0 && LongTemp < 42.0 ) to.zone = 37;
     }
   // +3 puts origin in middle of zone
-  LongOrigin = (to.zone - 1)*6 - 180 + 3; 
+  LongOrigin = (to.zone - 1)*6 - 180 + 3;
   LongOriginRad = angles::from_degrees(LongOrigin);
 
   // compute the UTM band from the latitude
-  to.band = UTMBand(Lat, LongTemp);
+  if (!force_zone)
+    to.band = UTMBand(Lat, LongTemp);
+  else
+    to.band = band;
+
+
+
 #if 0
   if (to.band == ' ')
     throw std::range_error;
@@ -236,13 +246,13 @@ void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to)
   A = cos(LatRad)*(LongRad-LongOriginRad);
 
   M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
-          - 5*eccSquared*eccSquared*eccSquared/256) * LatRad 
+          - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
          - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
             + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
          + (15*eccSquared*eccSquared/256
-            + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
+            + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
          - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
-	
+
   to.easting = (double)
     (k0*N*(A+(1-T+C)*A*A*A/6
            + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
@@ -277,7 +287,7 @@ bool isValid(const UTMPoint &pt)
 
   return true;
 }
- 
+
 /** Create UTM point from WGS 84 geodetic point. */
 UTMPoint::UTMPoint(const geographic_msgs::msg::GeoPoint &pt)
 {
@@ -291,9 +301,10 @@ UTMPoint::UTMPoint(const geographic_msgs::msg::GeoPoint &pt)
  *
  *  @todo define the orientation transformation properly
  */
-void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to)
+void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to,
+        const bool& force_zone, const char& band, const uint8_t& zone)
 {
-  fromMsg(from.position, to.position);
+  fromMsg(from.position, to.position, force_zone, band, zone);
   to.orientation = from.orientation;
 }
 

geodesy/tests/test_utm.cpp

@@ -413,6 +413,32 @@ TEST(OStream, pose)
   EXPECT_EQ(out.str(), expected);
 }
 
+TEST(ForceUTMZone, point)
+{
+
+    geographic_msgs::msg::GeoPoint zone2, zone3;
+    zone2.latitude=24.02;
+    zone2 = geodesy::toMsg(24.02, 5.999);
+    zone3 = geodesy::toMsg(24.02, 6.001);
+    geodesy::UTMPoint pt2, pt3, pt4;
+    geodesy::fromMsg(zone2, pt2);
+    geodesy::fromMsg(zone3, pt3);
+
+    EXPECT_FALSE(geodesy::sameGridZone(pt2, pt3) );
+
+    double diffx = pt2.easting - pt3.easting;
+    double diffy = pt2.northing - pt3.northing;
+    double distance = std::sqrt(diffx*diffx + diffy*diffy);
+
+    //Now force the pt3 into pt2's grid zone
+    geodesy::fromMsg(zone3, pt4, true, pt2.band, pt2.zone);
+    diffx = pt2.easting - pt4.easting;
+    diffy = pt2.northing - pt4.northing;
+    double distance2 = std::sqrt(diffx*diffx + diffy*diffy);
+    ROS_INFO("Prev Distance %f, Actual Distance %f", distance, distance2);
+    EXPECT_LT(distance2, distance);
+}
+
 // Run all the tests that were declared with TEST()
 int main(int argc, char **argv)
 {