Feat/cv orchestrator localize, (GSD Implementation with Unit Tests) #158
Conversation
There was a problem hiding this comment.
Great job on getting this completed and putting in all the work to make sure the code is readable and well documented. Test cases are a great addition that will give us a lot more confidence in our localization algorithm.
This should be ready to merge once you update this branch with the recent changes on main. You can do this by running git fetch and then git merge origin/main. There will be a few conflicting files so feel free to let me know if you're unsure about which changes to keep.
include/cv/localization.hpp
Outdated
| @@ -81,6 +84,8 @@ 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); | |||
There was a problem hiding this comment.
nitpick: I wouldn't hate a more descriptive name instead of CalcOffset but it's not that big of a deal.
src/cv/localization.cpp
Outdated
| 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 Frame (Origin to Center of Image instead of Top Left) | ||
| 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; | ||
| } |
There was a problem hiding this comment.
Fantastic comments on this function. They really make it a lot easier to parse what's going on.
src/cv/localization.cpp
Outdated
| */ | ||
|
|
||
| double GSDLocalization::distanceInMetersBetweenCords(const double lat1, const double lon1, const double lat2, const double lon2){ |
There was a problem hiding this comment.
I really like the inclusion of this function for reasoning about accuracy in terms of feet/meters instead of latitude/longitude.
|
@atar13 What do you want me to do for the interface.hpp ImageTelemetry? I added the heading parameter because the GSD requires it but it is not on the main version of ImageTelemetry. |
We should keep your version with heading. You'll just need to update anywhere the |
|
@atar13 The merge was good and it compiles along with the tests. It didn't pass the format checker so let me know if I should try to resolve that. Otherwise the localization unit test still outputs cout values so if I should remove those lmk but they do help with seeing the current state/accuracy of localization. |
Yeah, the last thing before merging will be to address the linting/format errors. You can see what it's complaining about here or by running I think the prints in the unit test should be fine. Some of the other tests already print to standard out so it's not an issue. |
Closes #20