PIXEL TO LATLON

PIXEL_TO_LATLON.PY

import math

# Konstanta radius bumi dalam meter (digunakan untuk perhitungan jarak dan konversi koordinat)
EARTH_RADIUS = 6371000  # meter


def predict_from_pixel(
    pixel_x,           # Koordinat X pixel dari objek di gambar (horizontal)
    image_width,       # Lebar total gambar dalam pixel
    focal_mm,          # Focal length kamera dalam milimeter
    sensor_width_mm,   # Lebar sensor kamera dalam milimeter
    cam_lat,           # Latitude posisi kamera dalam derajat
    cam_lon,           # Longitude posisi kamera dalam derajat
    heading_camera_deg,# Arah heading kamera dalam derajat (0° = Utara, searah jarum jam)
    distance_meter     # Jarak dari kamera ke objek dalam meter
):
    """
    Fungsi untuk memprediksi koordinat lat/lon objek dari posisi pixel di gambar kamera.
    
    Proses:
    1. Hitung focal length dalam pixel
    2. Hitung sudut dari pixel terhadap center
    3. Hitung heading target (arah objek dari kamera)
    4. Konversi jarak dan heading ke koordinat lat/lon
    """

    # --- Hitung focal pixel ---
    # fx = focal length dalam pixel (bukan mm)
    # Rumus: fx = (focal_mm / sensor_width_mm) × image_width
    # Contoh: (15mm / 6.4mm) × 1280px = 3000 pixel
    fx = (focal_mm / sensor_width_mm) * image_width
    
    # cx = center X, titik tengah gambar secara horizontal
    # Untuk gambar 1280px, cx = 640px
    cx = image_width / 2

    # --- Sudut dari pixel ---
    # dx = delta X, jarak horizontal pixel objek dari center gambar
    # Positif jika objek di kanan center, negatif jika di kiri
    # Contoh: pixel 668.5 - 640 = 28.5px (objek sedikit di kanan center)
    dx = pixel_x - cx
    
    # theta_rad = sudut deviasi dalam radian
    # Dihitung dengan arctan(dx/fx) - sudut antara garis pandang center dan objek
    # Semakin jauh objek dari center, semakin besar sudutnya
    # Contoh: arctan(28.5/3000) = 0.0095 radian
    theta_rad = math.atan(dx / fx)
    
    # theta_deg = sudut deviasi dalam derajat
    # Konversi dari radian ke derajat untuk lebih mudah dipahami
    # Contoh: 0.0095 rad × (180/π) = 0.544°
    theta_deg = math.degrees(theta_rad)

    # --- Heading target ---
    # heading_target = arah absolut objek dari Utara (0°)
    # Dihitung dengan menambahkan sudut deviasi ke heading kamera
    # Contoh: 250.630° + 0.544° = 251.174°
    # Jika objek di kiri center, theta_deg negatif sehingga heading berkurang
    heading_target = heading_camera_deg + theta_deg

    # --- Konversi ke radian ---
    # heading_rad = heading target dalam radian (untuk perhitungan trigonometri)
    # Semua fungsi trigonometri Python (sin, cos) menggunakan radian
    heading_rad = math.radians(heading_target)
    
    # lat_rad = latitude kamera dalam radian
    # Diperlukan untuk koreksi longitude (karena longitude menyempit di kutub)
    lat_rad = math.radians(cam_lat)

    # --- Hitung delta ---
    # delta_lat = perubahan latitude dalam radian
    # cos(heading) memberikan komponen Utara-Selatan:
    #   - heading 0° (Utara): cos(0°) = 1 → pergerakan penuh ke Utara
    #   - heading 90° (Timur): cos(90°) = 0 → tidak ada pergerakan Utara-Selatan
    #   - heading 180° (Selatan): cos(180°) = -1 → pergerakan penuh ke Selatan
    # Dibagi EARTH_RADIUS untuk konversi dari meter ke radian
    delta_lat = (distance_meter * math.cos(heading_rad)) / EARTH_RADIUS
    
    # delta_lon = perubahan longitude dalam radian
    # sin(heading) memberikan komponen Timur-Barat:
    #   - heading 0° (Utara): sin(0°) = 0 → tidak ada pergerakan Timur-Barat
    #   - heading 90° (Timur): sin(90°) = 1 → pergerakan penuh ke Timur
    #   - heading 270° (Barat): sin(270°) = -1 → pergerakan penuh ke Barat
    # Dibagi cos(lat_rad) karena garis longitude mendekat di kutub
    # Di khatulistiwa (lat=0°), cos(0°)=1 → tidak ada koreksi
    # Di kutub (lat=90°), cos(90°)=0 → longitude sangat rapat
    delta_lon = (distance_meter * math.sin(heading_rad)) / (
        EARTH_RADIUS * math.cos(lat_rad)
    )

    # lat_obj = latitude objek dalam derajat
    # Tambahkan delta_lat (dalam radian) yang sudah dikonversi ke derajat
    lat_obj = cam_lat + math.degrees(delta_lat)
    
    # lon_obj = longitude objek dalam derajat
    # Tambahkan delta_lon (dalam radian) yang sudah dikonversi ke derajat
    lon_obj = cam_lon + math.degrees(delta_lon)

    # Return koordinat objek dan heading target
    return lat_obj, lon_obj, heading_target

# ===== PARAMETER INPUT =====

# Posisi kamera (koordinat GPS)
kamera_lat = -5.926424556714948  # Latitude kamera
kamera_lon = 105.9951442371121    # Longitude kamera

# Offset heading (koreksi jika ada perbedaan antara heading kompas dan heading sebenarnya)
off_heading_camera = 0

# Arah kamera (heading dalam derajat, 0° = Utara, searah jarum jam)
heading_camera = 250.630 - off_heading_camera

# Spesifikasi kamera
focal_mm = 15           # Focal length lensa dalam milimeter
sensor_width = 6.4      # Lebar sensor kamera dalam milimeter

# Jarak target dari kamera dalam meter
#distance_target = 253  # target 1 (x1 bawah kiri dan x2 bawah kanan)
distance_target = 2006  # target 2 (x1 bawah kiri dan x2 bawah kanan)

# Spesifikasi gambar
image_width = 1280      # Lebar gambar dalam pixel

# Koordinat pixel X dari objek di gambar
# Dihitung dari rata-rata batas kiri dan kanan bounding box
#x_center = (3 + 1277) / 2    # target 1 (x1 bawah kiri dan x2 bawah kanan)
x_center = (580 + 757) / 2    # target 2 (x1 bawah kiri dan x2 bawah kanan) = 668.5 pixel


# ===== EKSEKUSI FUNGSI =====

# Panggil fungsi untuk menghitung koordinat objek
lat_obj, lon_obj, heading_target = predict_from_pixel(
    pixel_x=x_center,                    # Posisi X pixel objek
    image_width=image_width,             # Lebar gambar
    focal_mm=focal_mm,                   # Focal length
    sensor_width_mm=sensor_width,        # Lebar sensor
    cam_lat=kamera_lat,                  # Latitude kamera
    cam_lon=kamera_lon,                  # Longitude kamera
    heading_camera_deg=heading_camera,   # Heading kamera
    distance_meter=distance_target       # Jarak ke objek
)

# Tampilkan hasil koordinat objek
print(lat_obj, lon_obj)