From 1fc14bfac66222d621faa56b6a2768160bdf093f Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 3 Mar 2024 16:47:24 -0800 Subject: [PATCH 1/8] Added draft c++ gsd implementation --- include/cv/localization.hpp | 5 +++++ src/cv/localization.cpp | 43 +++++++++++++++++++++++++++++++++++-- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/include/cv/localization.hpp b/include/cv/localization.hpp index b1ace0b2..9a266c12 100644 --- a/include/cv/localization.hpp +++ b/include/cv/localization.hpp @@ -12,6 +12,8 @@ #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 // Localization is responsible for calculating the real world latitude/longitude // of competition targets. @@ -81,6 +83,9 @@ class ECEFLocalization : Localization { class GSDLocalization : Localization { public: GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) override; + + private: + GPSCoord CalcOffset(const std::vector& param); }; #endif // INCLUDE_CV_LOCALIZATION_HPP_ diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index fab764cf..f5b9fa15 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -31,7 +31,7 @@ ECEFLocalization::ECEFCoordinates ECEFLocalization::ENUtoECEF(ENUCoordinates off // 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; @@ -107,5 +107,44 @@ GPSCoord ECEFLocalization::localize(const ImageTelemetry& telemetry, const Bbox& } GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { - return GPSCoord(); + GPSCoord gps; + + float GSD = (SENSOR_WIDTH * (telemetry.altitude)) / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + + float img_mid_x = IMG_WIDTH_PX / 2; + float img_mid_y = IMG_HEIGHT_PX / 2; + + float length = (sqrt(pow((plane_data[4] - img_mid_x), 2) + pow((plane_data[5] - img_mid_y), 2) * GSD)); + + float target_camera_cord_x = plane_data[4] - img_mid_x; + float target_camera_cord_y = plane_data[5] - img_mid_y; + + float thetaB = plane_data[3] + atan(target_camera_cord_x / target_camera_cord_y); + + float calc_cam_offset_x = target_camera_cord_x * GSD * 0.001; //mm to M + float calc_cam_offset_y = target_camera_cord_y * GSD * 0.001; //mm to M + + std::vector input {calc_cam_offset_x, calc_cam_offset_y, telemetry.latitude, telemetry.longitude}; + + GPSCoord calc_coord = calc_offset(input); + + // for (const float& f : floats) { + // integers.push_back(static_cast(f)); + // } + return calc_coord; +} + +GPSCoord GSDLocalization::CalcOffset(const std::vector& param) { + float dLat = param[0] / EARTH_RADIUS_M; + float dLon = param[1] / (EARTH_RADIUS_M * cos(M_PI * param[2] / 180)); + + float latO = param[2] + dLat * 180/M_PI; + float lonO = param[3] + dLon * 180/M_PI; + + GPDCoord output; + + output.set_latitude(latO); + output.set_longitude(lonO); + + return output; } \ No newline at end of file From cefec89a0bcabb5f59d32ebbfcbb1ee1a7af47cc Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sun, 10 Mar 2024 16:58:24 -0700 Subject: [PATCH 2/8] Added fixes to localization.cpp --- src/cv/localization.cpp | 51 ++++++++++++++++++++++++++++++----------- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index f5b9fa15..7eea929b 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -106,40 +106,63 @@ GPSCoord ECEFLocalization::localize(const ImageTelemetry& telemetry, const Bbox& return targetCoord; } + GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) { GPSCoord gps; + // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx float GSD = (SENSOR_WIDTH * (telemetry.altitude)) / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + // Midpoints of the image float img_mid_x = IMG_WIDTH_PX / 2; float img_mid_y = IMG_HEIGHT_PX / 2; - float length = (sqrt(pow((plane_data[4] - img_mid_x), 2) + pow((plane_data[5] - img_mid_y), 2) * GSD)); + //midpoints of bounding box around the target + float target_x = (targetBbox.x1 + targetBbox.x2)/2; + float target_y = (targetBbox.y1 + targetBbox.y2)/2; + + // calculations of bearing + // L = (distance(middle, bbox))*GSD + float length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * GSD)); - float target_camera_cord_x = plane_data[4] - img_mid_x; - float target_camera_cord_y = plane_data[5] - img_mid_y; + //Translate Image Cordinates to Camera Cordinate Frame (Origin to Center of Image instead of Top Left) + float target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); + float target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; + //Angle of Bearing (Angle from north to target) float thetaB = plane_data[3] + 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 float calc_cam_offset_x = target_camera_cord_x * GSD * 0.001; //mm to M float calc_cam_offset_y = target_camera_cord_y * GSD * 0.001; //mm to M - std::vector input {calc_cam_offset_x, calc_cam_offset_y, telemetry.latitude, telemetry.longitude}; - - GPSCoord calc_coord = calc_offset(input); + //Calculates the cordinates using the offset + GPSCoord calc_coord = calc_offset(calc_cam_offset_y, calc_cam_offset_x, telemetry.latitude, telemetry.longitude); - // for (const float& f : floats) { - // integers.push_back(static_cast(f)); - // } return calc_coord; } -GPSCoord GSDLocalization::CalcOffset(const std::vector& param) { - float dLat = param[0] / EARTH_RADIUS_M; - float dLon = param[1] / (EARTH_RADIUS_M * cos(M_PI * param[2] / 180)); +/* +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 float offset_x, const float offset_y, const float lat, const float lon) { + float dLat = offset_y / EARTH_RADIUS_M; + float dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180)); - float latO = param[2] + dLat * 180/M_PI; - float lonO = param[3] + dLon * 180/M_PI; + float latO = lat + dLat * 180/M_PI; + float lonO = lon + dLon * 180/M_PI; GPDCoord output; From 9d17ab47c45fd41928022090302e518fd0aa0557 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Tue, 12 Mar 2024 18:24:56 -0700 Subject: [PATCH 3/8] Added Localization Test Cases from Blender Sim --- src/cv/localization.cpp | 4 +-- tests/unit/cv/localization.cpp | 48 +++++++++++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 5 deletions(-) diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 7eea929b..0a14f6df 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -130,7 +130,7 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& float target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; //Angle of Bearing (Angle from north to target) - float thetaB = plane_data[3] + atan(target_camera_cord_x / target_camera_cord_y); + float thetaB = telemetry.bearing + 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){ @@ -142,7 +142,7 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& float calc_cam_offset_y = target_camera_cord_y * GSD * 0.001; //mm to M //Calculates the cordinates using the offset - GPSCoord calc_coord = calc_offset(calc_cam_offset_y, calc_cam_offset_x, telemetry.latitude, telemetry.longitude); + GPSCoord calc_coord = calc_offset(calc_cam_offset_x, calc_cam_offset_y, telemetry.latitude, telemetry.longitude); return calc_coord; } diff --git a/tests/unit/cv/localization.cpp b/tests/unit/cv/localization.cpp index f162ff36..75fce743 100644 --- a/tests/unit/cv/localization.cpp +++ b/tests/unit/cv/localization.cpp @@ -28,10 +28,52 @@ TEST(CVLocalization, LocalizationAccuracy) { 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), }, + + { + "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), + } // TODO: move blender localization test cases here // { // "another test case", @@ -48,7 +90,7 @@ TEST(CVLocalization, LocalizationAccuracy) { GPSCoord ecefTargetCoord = ecefLocalizer.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); assertLocalizationAccuracy(testCase.expectedTargetCoord, ecefTargetCoord); - // GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); - // assertLocalizationAccuracy(testCase.expectedTargetCoord, gsdTargetCoord); + GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); + assertLocalizationAccuracy(testCase.expectedTargetCoord, gsdTargetCoord); }; } From c52e5cab3b2f9a10a4137ef069658c4e5f055505 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Mon, 15 Apr 2024 14:51:23 -0700 Subject: [PATCH 4/8] Updated localization tests (1-13ft) Accuracy --- .gitmodules | 2 +- include/camera/interface.hpp | 3 +- include/cv/localization.hpp | 6 ++-- protos | 2 +- src/camera/interface.cpp | 3 +- src/camera/mock.cpp | 2 +- src/cv/localization.cpp | 55 ++++++++++++++++++++++------------ tests/unit/cv/localization.cpp | 23 +++++++------- 8 files changed, 58 insertions(+), 38 deletions(-) diff --git a/.gitmodules b/.gitmodules index faa17ada..a982fbf1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "protos"] path = protos - url = git@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 de4deb52..44643070 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -12,12 +12,13 @@ // In the future this could be in a mavlink file. class ImageTelemetry { public: - ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double yaw, + ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double heading, double yaw, double pitch, double roll); const double latitude; const double longitude; const double altitude; const double airspeed; + const double heading; const double yaw; const double pitch; const double roll; diff --git a/include/cv/localization.hpp b/include/cv/localization.hpp index 9a266c12..ebf153e1 100644 --- a/include/cv/localization.hpp +++ b/include/cv/localization.hpp @@ -14,6 +14,7 @@ #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. @@ -83,9 +84,8 @@ class ECEFLocalization : Localization { class GSDLocalization : Localization { public: GPSCoord localize(const ImageTelemetry& telemetry, const Bbox& targetBbox) override; - - private: - GPSCoord CalcOffset(const std::vector& param); + 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/protos b/protos index 4d1c0c23..abe95730 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit 4d1c0c235358a4e837cd09bffae2f4560a94cfd1 +Subproject commit abe95730634b146f39fb5127ee96bac511e9cbd5 diff --git a/src/camera/interface.cpp b/src/camera/interface.cpp index 75f0113d..9644524a 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -1,11 +1,12 @@ #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 0a14f6df..1ae95e0d 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -111,26 +111,26 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& GPSCoord gps; // Ground Sample Distance (mm/pixel), 1.0~2.5cm per px is ideal aka 10mm~25mm ppx - float GSD = (SENSOR_WIDTH * (telemetry.altitude)) / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); + double GSD = (SENSOR_WIDTH * (telemetry.altitude)) / (FOCAL_LENGTH_MM * IMG_WIDTH_PX); // Midpoints of the image - float img_mid_x = IMG_WIDTH_PX / 2; - float img_mid_y = IMG_HEIGHT_PX / 2; + double img_mid_x = IMG_WIDTH_PX / 2; + double img_mid_y = IMG_HEIGHT_PX / 2; //midpoints of bounding box around the target - float target_x = (targetBbox.x1 + targetBbox.x2)/2; - float target_y = (targetBbox.y1 + targetBbox.y2)/2; + double target_x = (targetBbox.x1 + targetBbox.x2)/2; + double target_y = (targetBbox.y1 + targetBbox.y2)/2; // calculations of bearing // L = (distance(middle, bbox))*GSD - float length = (sqrt(pow((target_x - img_mid_x), 2) + pow((target_y - img_mid_y), 2) * 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 Frame (Origin to Center of Image instead of Top Left) - float target_camera_cord_x = target_x - (IMG_WIDTH_PX / 2); - float target_camera_cord_y = (IMG_HEIGHT_PX / 2) - target_y; + 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) - float thetaB = telemetry.bearing + atan(target_camera_cord_x / target_camera_cord_y); + 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){ @@ -138,15 +138,31 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& } //Finds the offset of the bbox - float calc_cam_offset_x = target_camera_cord_x * GSD * 0.001; //mm to M - float calc_cam_offset_y = target_camera_cord_y * GSD * 0.001; //mm to M + 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 = calc_offset(calc_cam_offset_x, calc_cam_offset_y, telemetry.latitude, telemetry.longitude); + GPSCoord calc_coord = CalcOffset((calc_cam_offset_x), (calc_cam_offset_y), (telemetry.latitude), (telemetry.longitude)); return calc_coord; } +double GSDLocalization::distanceInMetersBetweenCords(const double lat1, const double lon1, const double lat2, const double lon2){ + 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 @@ -157,17 +173,18 @@ Takes the position of the camera in blender and the position of the generated ta @returns true (mostly) world cordinate of target */ -GPSCoord GSDLocalization::CalcOffset(const float offset_x, const float offset_y, const float lat, const float lon) { - float dLat = offset_y / EARTH_RADIUS_M; - float dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180)); +GPSCoord GSDLocalization::CalcOffset(const double offset_x, const double offset_y, const double lat, const double lon) { + double dLat = offset_y / EARTH_RADIUS_M; + double dLon = offset_x / (EARTH_RADIUS_M * cos(M_PI * lat / 180)); - float latO = lat + dLat * 180/M_PI; - float lonO = lon + dLon * 180/M_PI; + double latO = lat + dLat * 180/M_PI; + double lonO = lon + dLon * 180/M_PI; - GPDCoord output; + GPSCoord output; output.set_latitude(latO); output.set_longitude(lonO); return output; -} \ No newline at end of file +} + diff --git a/tests/unit/cv/localization.cpp b/tests/unit/cv/localization.cpp index 75fce743..d6bc68e9 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,7 +23,7 @@ TEST(CVLocalization, LocalizationAccuracy) { GPSCoord expectedTargetCoord; }; - + const std::vector testCases{{ { "directly underneath", @@ -74,23 +73,25 @@ TEST(CVLocalization, LocalizationAccuracy) { Bbox(3499, 2008, 3499, 2008), makeGPSCoord(-0.00000292339877027894, 0.00001211822682158354, 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), - // } }}; for (const auto &testCase : testCases) { ECEFLocalization ecefLocalizer; GSDLocalization gsdLocalization; + std::cout << testCase.name << std::endl; - GPSCoord ecefTargetCoord = ecefLocalizer.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); - assertLocalizationAccuracy(testCase.expectedTargetCoord, ecefTargetCoord); + // GPSCoord ecefTargetCoord = ecefLocalizer.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); + // assertLocalizationAccuracy(testCase.expectedTargetCoord, ecefTargetCoord); GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); + //TODO: FLOAT VS DOUBLE + 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); }; } From e38a74157f8e4b05e86ebeac4de70626d9945384 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Fri, 26 Apr 2024 00:23:24 +0000 Subject: [PATCH 5/8] Added Comments to distance func --- protos | 2 +- src/cv/localization.cpp | 12 ++++++++++++ tests/unit/cv/localization.cpp | 1 - 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/protos b/protos index abe95730..c5841043 160000 --- a/protos +++ b/protos @@ -1 +1 @@ -Subproject commit abe95730634b146f39fb5127ee96bac511e9cbd5 +Subproject commit c5841043ba805c065997232b5a6ae281873d9969 diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 1ae95e0d..c1d09d0e 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -147,6 +147,18 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& 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){ double e1 = lat1 * M_PI / 180; double e2 = lat2 * M_PI / 180; diff --git a/tests/unit/cv/localization.cpp b/tests/unit/cv/localization.cpp index d6bc68e9..a6fda468 100644 --- a/tests/unit/cv/localization.cpp +++ b/tests/unit/cv/localization.cpp @@ -84,7 +84,6 @@ TEST(CVLocalization, LocalizationAccuracy) { // assertLocalizationAccuracy(testCase.expectedTargetCoord, ecefTargetCoord); GPSCoord gsdTargetCoord = gsdLocalization.localize(testCase.inputImageTelemetry, testCase.inputTargetBbox); - //TODO: FLOAT VS DOUBLE std::cout << "Error: " << gsdLocalization.distanceInMetersBetweenCords( (testCase.expectedTargetCoord.latitude()), (testCase.expectedTargetCoord.longitude()), From 40a22d5b68ba978b55e63cea9fab3993df33facd Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Fri, 26 Apr 2024 22:53:09 +0000 Subject: [PATCH 6/8] Added Fixes --- include/camera/interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 27f3338c..bbfa46ce 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -17,7 +17,7 @@ struct ImageTelemetry { const double longitude; const double altitude; const double airspeed; - const double heading + const double heading; const double yaw; const double pitch; const double roll; From 7e58721b56438f9aff0b01b967cd8e60f63b52b3 Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Fri, 26 Apr 2024 16:02:53 -0700 Subject: [PATCH 7/8] Fixed merge issues --- include/camera/interface.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index bbfa46ce..8f469403 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -13,14 +13,14 @@ struct ImageTelemetry { ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double heading, double yaw, double pitch, double roll); - const double latitude; - const double longitude; - const double altitude; - const double airspeed; - const double heading; - const double yaw; - const double pitch; - const double roll; + double latitude; + double longitude; + double altitude; + double airspeed; + double heading; + double yaw; + double pitch; + double roll; }; /* From c1232535fdec97c513cb2b7dfb5f4780b593232f Mon Sep 17 00:00:00 2001 From: miyatakazuya Date: Sat, 27 Apr 2024 05:27:03 +0000 Subject: [PATCH 8/8] addressed the linting/format errors --- include/camera/interface.hpp | 20 ++++++++++---------- include/cv/localization.hpp | 8 +++++--- src/camera/interface.cpp | 3 ++- src/cv/localization.cpp | 29 +++++++++++++++-------------- 4 files changed, 32 insertions(+), 28 deletions(-) diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index 8f469403..149115de 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -11,16 +11,16 @@ // 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 heading, double yaw, - double pitch, double roll); - double latitude; - double longitude; - double altitude; - double airspeed; - double heading; - 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 903c22f8..54c5d58d 100644 --- a/include/cv/localization.hpp +++ b/include/cv/localization.hpp @@ -13,7 +13,7 @@ #define IMG_WIDTH_PX 5472 #define IMG_HEIGHT_PX 3648 #define EARTH_RADIUS_M 6378137 -#define SENSOR_WIDTH 15.86 //mm +#define SENSOR_WIDTH 15.86 // mm #define METER_TO_FT 3.28084 // Localization is responsible for calculating the real world latitude/longitude @@ -90,8 +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); + 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 9644524a..3cf055e9 100644 --- a/src/camera/interface.cpp +++ b/src/camera/interface.cpp @@ -1,6 +1,7 @@ #include "camera/interface.hpp" -ImageTelemetry::ImageTelemetry(double latitude, double longitude, double altitude, double airspeed, double heading, +ImageTelemetry::ImageTelemetry(double latitude, double longitude, double altitude, + double airspeed, double heading, double yaw, double pitch, double roll) : latitude(latitude), longitude(longitude), diff --git a/src/cv/localization.cpp b/src/cv/localization.cpp index 86e3ff95..9cf8b4e9 100644 --- a/src/cv/localization.cpp +++ b/src/cv/localization.cpp @@ -131,32 +131,33 @@ GPSCoord GSDLocalization::localize(const ImageTelemetry& telemetry, const Bbox& double img_mid_x = IMG_WIDTH_PX / 2; double img_mid_y = IMG_HEIGHT_PX / 2; - //midpoints of bounding box around the target + // 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 + // 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 Frame (Origin to Center of Image instead of Top Left) + // 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); + // 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){ + // 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 + // 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)); + // 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; } @@ -173,7 +174,7 @@ Takes the in two cordinaates and outputs their 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){ +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; @@ -199,7 +200,7 @@ Takes the position of the camera in blender and the position of the generated ta @returns true (mostly) world cordinate of target */ -GPSCoord GSDLocalization::CalcOffset(const double offset_x, const double offset_y, const double lat, const double lon) { +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));