algorithms/
localize.rs

1//! Localization algorithms for the drone.
2
3use geographiclib_rs::{DirectGeodesic, Geodesic};
4
5/// This function calculates and returns the co-ordinates of the target based on the images taken
6/// by the drone camera, and the position of the camera.
7///
8/// # Arguments
9///
10/// * `camera_coords` - coordinates of the center of the camera
11/// * `altitude` - altitude of the drone at snapshot
12/// * `heading` - heading of the drone at snapshot
13/// * `target_position` - coordinates of the target; specifically, the center of the target
14pub fn resolve_gps_location(
15    //TODO check gemini's new function for exact correctness. The old function was very incorrect though, so this is better than that as least.
16    camera_coords: (f64, f64),
17    altitude: f64,
18    heading: f64,
19    target_position: (i32, i32),
20) -> (f64, f64) {
21    let h_fov_deg = constants::CAMERA_H_FOV;
22    let resolution = (
23        constants::CAMERA_RESOLUTION_H,
24        constants::CAMERA_RESOLUTION_V,
25    );
26
27    // --- Step 1: Calculate Ground Distance using 2D nadir approximation ---
28    let mpp = meters_per_pixel(altitude, h_fov_deg, resolution);
29    let camera_center_pixels = (resolution.0 / 2, resolution.1 / 2);
30
31    let pixel_distance = (((target_position.0 - camera_center_pixels.0).pow(2)
32        + (target_position.1 - camera_center_pixels.1).pow(2)) as f64)
33        .sqrt();
34
35    let meters_distance = pixel_distance * mpp;
36
37    if meters_distance < 1e-6 {
38        return camera_coords;
39    }
40
41    // --- Step 2: Calculate True Geodetic Bearing ---
42    let dx = (target_position.0 - camera_center_pixels.0) as f64;
43    let dy = (target_position.1 - camera_center_pixels.1) as f64;
44    let inverted_dy = -dy;
45
46    let math_angle_rad = inverted_dy.atan2(dx);
47    let math_angle_deg = math_angle_rad.to_degrees();
48
49    let relative_bearing = (450.0 - math_angle_deg).rem_euclid(360.0);
50    let true_bearing = (heading + relative_bearing).rem_euclid(360.0);
51
52    // --- Step 3: Calculate Final GPS Coordinates ---
53    let geodesic = Geodesic::wgs84();
54    let (target_lat, target_lon, _) = geodesic.direct(
55        camera_coords.0,
56        camera_coords.1,
57        true_bearing,
58        meters_distance,
59    );
60    (target_lat, target_lon)
61} /* resolve_gps_location() */
62
63/// Converts the image pixels to meters.
64fn meters_per_pixel(altitude: f64, h_fov_deg: f64, resolution: (i32, i32)) -> f64 {
65    let h_fov_rad = h_fov_deg.to_radians();
66
67    // Calculate vertical FOV based on aspect ratio to maintain accuracy
68    let aspect_ratio = resolution.1 as f64 / resolution.0 as f64;
69    let v_fov_rad = 2.0 * (aspect_ratio * (h_fov_rad / 2.0).tan()).atan();
70
71    let ground_width = 2.0 * altitude * (h_fov_rad / 2.0).tan();
72    let ground_height = 2.0 * altitude * (v_fov_rad / 2.0).tan();
73
74    let mpp_h = ground_width / resolution.0 as f64;
75    let mpp_v = ground_height / resolution.1 as f64;
76
77    (mpp_h + mpp_v) / 2.0
78}
algorithms/localize.rs

algorithms/
localize.rs
