diff --git a/.gitmodules b/.gitmodules index 60a1de12..a982fbf1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "protos"] path = protos - url = https://github.com/tritonuas/protos.git + url = https://github.com/tritonuas/protos.git diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 78428226..149115de 100644 --- a/include/camera/interface.hpp +++ b/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; diff --git a/include/cv/localization.hpp b/include/cv/localization.hpp index 87e8fa2e..54c5d58d 100644 --- a/include/cv/localization.hpp +++ b/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_ diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 75f0113d..3cf055e9 100644 --- a/src/camera/interface.cpp +++ b/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) {} diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index c1662a6b..ef37cb65 100644 --- a/src/camera/mock.cpp +++ b/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(newImg); } diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 6e3f29b4..9cf8b4e9 100644 --- a/src/cv/localization.cpp +++ b/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; +} + diff --git a/tests/unit/cv/localization.cpp b/tests/unit/cv/localization.cpp index 6959d75a..a6fda468 100644 --- a/tests/unit/cv/localization.cpp +++ b/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 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); }; }