Merge pull request #158 from tritonuas/feat/cv-orchestrator-localize

Feat/cv orchestrator localize, (GSD Implementation with Unit Tests)

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "protos"]
 	path = protos
-    url = https://github.com/tritonuas/protos.git
+	url = https://github.com/tritonuas/protos.git

include/camera/interface.hpp

@@ -11,13 +11,13 @@
 // class to contain all telemetry that should be tagged with an image.
 // In the future this could be in a mavlink file.
 struct ImageTelemetry {
-    ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double yaw,
-                   double pitch, double roll);
-
+    ImageTelemetry(double latitude, double longitude, double altitude, double airspeed,
+                   double heading, double yaw, double pitch, double roll);
     double latitude;
     double longitude;
     double altitude;
     double airspeed;
+    double heading;
     double yaw;
     double pitch;
     double roll;

include/cv/localization.hpp

@@ -12,6 +12,9 @@
 #define FOCAL_LENGTH_MM 50
 #define IMG_WIDTH_PX 5472
 #define IMG_HEIGHT_PX 3648
+#define EARTH_RADIUS_M 6378137
+#define SENSOR_WIDTH 15.86  // mm
+#define METER_TO_FT 3.28084
 
 // Localization is responsible for calculating the real world latitude/longitude
 // of competition targets.
@@ -87,6 +90,10 @@ class ECEFLocalization : Localization {
 class GSDLocalization : Localization {
  public:
     GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) override;
+    GPSCoord CalcOffset(const double offset_x, const double offset_y,
+                        const double lat, const double lon);
+    double distanceInMetersBetweenCords(const double lat1, const double lon1,
+                                        const double lat2, const double lon2);
 };
 
 #endif  // INCLUDE_CV_LOCALIZATION_HPP_

src/camera/interface.cpp

@@ -1,11 +1,13 @@
 #include "camera/interface.hpp"
 
-ImageTelemetry::ImageTelemetry(double latitude, double longitude, double altitude, double airspeed,
+ImageTelemetry::ImageTelemetry(double latitude, double longitude, double altitude,
+                               double airspeed, double heading,
                                double yaw, double pitch, double roll)
     : latitude(latitude),
       longitude(longitude),
       altitude(altitude),
       airspeed(airspeed),
+      heading(heading),
       yaw(yaw),
       pitch(pitch),
       roll(roll) {}

src/camera/mock.cpp

@@ -9,7 +9,7 @@ bool MockCamera::verifyConnection() { return true; }
 void MockCamera::takePicture() {
     ImageData newImg("mock_image.jpg", "/real/path/mock_image.jpg",
                      cv::Mat(cv::Size(4000, 3000), CV_8UC3, cv::Scalar(255)),
-                     ImageTelemetry(38.31568, 76.55006, 75, 20, 100, 5, 3));
+                     ImageTelemetry(38.31568, 76.55006, 75, 20, 0, 100, 5, 3));
 
     lastPicture = std::make_unique<ImageData>(newImg);
 }

src/cv/localization.cpp

@@ -41,7 +41,7 @@ ECEFLocalization::ECEFCoordinates ECEFLocalization::ENUtoECEF(
 // Converts ECEF coordinates to GPS coordinates using Heikkinen's procedure
 GPSCoord ECEFLocalization::ECEFtoGPS(ECEFCoordinates ecef) {
     GPSCoord gps;
-    double a = 6378137;
+    double a = EARTH_RADIUS_METERS;
     double b = 6356752;
     double e2 = 1 - ((b*b)/(a*a));
     double ep2 = ((a*a)/(b*b)) - 1;
@@ -120,6 +120,98 @@ GPSCoord ECEFLocalization::localize(const ImageTelemetry& telemetry, const Bbox&
     return targetCoord;
 }
 
+
 GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) {
-    return GPSCoord();
+    GPSCoord gps;
+
+    // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx
+    double GSD = (SENSOR_WIDTH * (telemetry.altitude)) / (FOCAL_LENGTH_MM * IMG_WIDTH_PX);
+
+    // Midpoints of the image
+    double img_mid_x = IMG_WIDTH_PX / 2;
+    double img_mid_y = IMG_HEIGHT_PX / 2;
+
+    // midpoints of bounding box around the target
+    double target_x = (targetBbox.x1 + targetBbox.x2)/2;
+    double target_y = (targetBbox.y1 + targetBbox.y2)/2;
+
+    // calculations of bearing
+    // L = (distance(middle, bbox))*GSD
+    double length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD));
+
+    // Translate Image Cordinates to Camera Cordinate (Origin to Center of Image instead of Top Left) NOLINT
+    double target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2);
+    double target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y;
+
+    // Angle of Bearing (Angle from north to target)
+    double thetaB = telemetry.heading + atan(target_camera_cord_x / target_camera_cord_y);
+
+    // Translate bearing to the 3 quadrant if applicable
+    if (target_camera_cord_x < 0 && target_camera_cord_y < 0) {
+        thetaB = 180.0 + thetaB;
+    }
+
+    // Finds the offset of the bbox
+    double calc_cam_offset_x = target_camera_cord_x * GSD * 0.001;  // mm to M
+    double calc_cam_offset_y = target_camera_cord_y * GSD * 0.001;  // mm to M
+
+    // Calculates the cordinates using the offset
+    GPSCoord calc_coord = CalcOffset((calc_cam_offset_x), (calc_cam_offset_y),
+                                    (telemetry.latitude), (telemetry.longitude));
+
+    return calc_coord;
 }
+
+/*
+Takes the in two cordinaates and outputs their distance in meters. 
+
+Parameters:
+- lat1/lon1 (First Cordinate)
+- lat2/lon2 (Second Cordinate)
+
+@returns distance in meters
+
+Reference: http://www.movable-type.co.uk/scripts/latlong.html
+*/
+
+double GSDLocalization::distanceInMetersBetweenCords(const double lat1, const double lon1, const double lat2, const double lon2) { // NOLINT
+    double e1 = lat1 * M_PI / 180;
+    double e2 = lat2 * M_PI / 180;
+
+    double d1 = (lat2 - lat1) * M_PI / 180;
+    double d2 = (lon2 - lon1) * M_PI / 180;
+
+    double a = sin(d1/2) * sin(d1/2) + cos(e1) * cos(e2) * sin(d2/2) * sin(d2/2);
+
+    double c = 2 * atan2(sqrt(a), sqrt(1-a));
+
+    double d = EARTH_RADIUS_M * c;
+
+    return d;
+}
+
+/*
+Takes the position of the camera in blender and the position of the generated target in meters
+
+Parameters:
+-image_offset_x/y - meters from center of plane (0,0)
+-cam_lat/lon - Set cordinates of plane
+
+@returns true (mostly) world cordinate of target 
+*/
+
+GPSCoord GSDLocalization::CalcOffset(const double offset_x, const double offset_y, const double lat, const double lon) { // NOLINT
+    double dLat = offset_y / EARTH_RADIUS_M;
+    double dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180));
+
+    double latO = lat + dLat * 180/M_PI;
+    double lonO = lon + dLon * 180/M_PI;
+
+    GPSCoord output;
+
+    output.set_latitude(latO);
+    output.set_longitude(lonO);
+
+    return output;
+}
+

tests/unit/cv/localization.cpp

@@ -2,7 +2,6 @@
 
 #include "cv/localization.hpp"
 
-
 void assertLocalizationAccuracy(
     const GPSCoord& expectedCoord,
     const GPSCoord& predictedCoord,
@@ -24,33 +23,74 @@ TEST(CVLocalization, LocalizationAccuracy) {
 
         GPSCoord expectedTargetCoord;
     };
-
+    
     const std::vector<TestCase> testCases{{
         {
             "directly underneath",
-            ImageTelemetry(0, 0, 100, 50, 0, 0, 0),
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
             Bbox(1951, 1483, 1951, 1483),
             makeGPSCoord(0, 0, 0),
+        }, 
+
+        {
+            "Blender Case 1",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(4539, 3372, 4539, 3372),
+            makeGPSCoord(-0.00002457511350215088, 0.00002863133914919689, 0),
+        },
+
+        {
+            "Blender Case 2",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(2590, 1702, 2590, 1702),
+            makeGPSCoord(0.00000192247925447489, -0.00000231662742315047, 0),      
+        },
+
+        {
+            "Blender Case 3",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(692, 814, 692, 814),
+            makeGPSCoord(0.00001601503082197966, -0.00003243918011301731, 0),
         },
-        // TODO: move blender localization test cases here
-        // {
-        //     "another test case",
-        //     ImageTelemetry(0, 0, 100, 50, 0, 0, 0),
-        //     Bbox(1951, 1483, 1951, 1483),
-        //     makeGPSCoord(0, 0, 0),
-        // }
+
+        {
+            "Blender Case 4",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(956, 2980, 956, 2980),
+            makeGPSCoord(-0.00001836226633958357, -0.00002825011114503169, 0),
+        },
+
+        {
+            "Blender Case 5",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(3318, 1776, 3318, 1776),
+            makeGPSCoord(0.00000075046054290061, 0.00000924642417102570, 0),
+        },
+
+        {
+            "Blender Case 6",
+            ImageTelemetry(0, 0, 100, 50, 0, 0, 0, 0),
+            Bbox(3499, 2008, 3499, 2008),
+            makeGPSCoord(-0.00000292339877027894, 0.00001211822682158354, 0),
+        }
     }};
 
     for (const auto &testCase : testCases) {
         ECEFLocalization ecefLocalizer;
         GSDLocalization gsdLocalization;
+        std::cout << testCase.name << std::endl;
 
-        // NOTE: temporarily commenting out this assertion until we debug the ECEF localization algorithm
         // GPSCoord ecefTargetCoord = ecefLocalizer.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox);
         // assertLocalizationAccuracy(testCase.expectedTargetCoord, ecefTargetCoord);
 
-        // NOTE: this assertion is blocked on GSD implementation getting merged in
-        // GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox);
-        // assertLocalizationAccuracy(testCase.expectedTargetCoord, gsdTargetCoord);
+        GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox);
+        std::cout << "Error: " << gsdLocalization.distanceInMetersBetweenCords(
+            (testCase.expectedTargetCoord.latitude()), 
+            (testCase.expectedTargetCoord.longitude()), 
+            (gsdTargetCoord.longitude()), 
+            (gsdTargetCoord.latitude())) * METER_TO_FT
+            << " feet" << std::endl;
+
+        assertLocalizationAccuracy(testCase.expectedTargetCoord, gsdTargetCoord);
     };
 }