Ortorettifica FIgSpec FS-60C

Per georiferire il dato di FigSpec FS60-C si puo' usare il file figspec.gps

(da notare che il file gps e' un csv che include anche dei campi sull'assetto di volo del drone yaw,pitch and roll che sono valorizzati sempre a zero) 

Da un primo volo con una risoluzione spaziale a 480 px, 1 nm e quota di volo 120 m AGL si ha un GSD di 5 cm al Nadir 

Considerando che il volo ha ripreso 135.5  m x 68.8 m quindi circa 9322 metri quadri si ha una produzione di 88 Mb a metro quadro

python3 orthorectify_pushbroom.py true_color.png figspec.gps --flip-across-track --fov 12.375

 

#!/usr/bin/env python3

"""

orthorectify_pushbroom.py

Ortorettifica un'immagine true-color pushbroom usando la traccia GPS/IMU

(CSV) e il FOV noto del sensore, tramite georeferenziazione diretta (Direct

Georeferencing, DG) e resampling su una griglia regolare UTM.

Come funziona (in breve)

-------------------------

1. Interpola la posizione (lat, lon, altitudine) del velivolo per OGNI riga

dell'immagine, a partire dai fix GPS sparsi nel file CSV (che ne contiene

uno ogni N righe, non uno per riga).

2. Stima la prua/heading del volo dalla traccia GPS stessa (bearing tra punti

consecutivi, o un singolo heading globale se il volo e' una passata

dritta), dato che i campi Pitch/Roll/Yaw nel file GPS risultano spesso

non popolati (costanti a 0) nei log di alcuni sistemi.

3. Per ogni pixel (riga, colonna), calcola l'angolo di vista rispetto al

nadir (assumendo FOV a distribuzione angolare lineare/equiangolare sulle

colonne) e proietta il raggio di vista a terra assumendo TERRENO PIATTO

all'altitudine data (nessun DEM), con velivolo puntato a nadir (pitch=

roll=0, cioe' nessuna correzione di assetto oltre l'heading).

4. Converte le coordinate di terra cosi' ottenute (lat/lon per ogni pixel,

la "griglia di geolocazione") in coordinate UTM metriche, e ricampiona

(resample) i pixel sorgente su una griglia regolare in UTM per produrre

un'ortofoto vera e propria, salvata come GeoTIFF georeferenziato.

Assunzioni e limiti (importanti)

----------------------------------

- NESSUN modello del terreno (DEM): il terreno e' assunto piatto

all'altitudine di volo fornita. Su terreno con rilievo significativo

introduce "relief displacement" (spostamento radiale dal nadir

proporzionale al dislivello). Per un rilievo di pianura/campo agricolo

come questo, l'errore e' generalmente trascurabile.

- Nessuna correzione di boresight/misalignment tra IMU e camera (si assume

il sensore perfettamente allineato con l'asse di volo).

- Pitch e Roll sono assunti nulli (nadir stabilizzato) se non disponibili

nel file GPS o se costanti a zero (tipico quando quel canale non e'

popolato dal logger). Se il tuo file GPS ha valori di pitch/roll validi

(non tutti zero), lo script li usa automaticamente.

- Il FOV e' assunto equiangolare e lineare sulle colonne (buona

approssimazione per sensori pushbroom a stretto campo di vista).

- La direzione "sinistra/destra" del sensore rispetto alla direzione di

volo (cioe' se la colonna 0 e' a destra o sinistra del velivolo) NON e'

nota a priori: usa --flip-across-track se l'output risulta specchiato

rispetto alla realta' (confronta con una mappa/basemap nota).

Uso

---

python3 orthorectify_pushbroom.py true_color.png figspec.gps \\

--fov 24.75 --out ortho_true_color.tif

# se l'immagine esce specchiata lateralmente:

python3 orthorectify_pushbroom.py true_color.png figspec.gps \\

--fov 24.75 --flip-across-track --out ortho_true_color.tif

# con dimensione pixel di output forzata (metri):

python3 orthorectify_pushbroom.py true_color.png figspec.gps \\

--fov 24.75 --pixel-size 0.05 --out ortho_true_color.tif

"""

import argparse

import sys

from pathlib import Path

import numpy as np

import pandas as pd

from PIL import Image

import pyproj

import rasterio

from rasterio.transform import Affine

from scipy.ndimage import distance_transform_edt

import matplotlib

matplotlib.use("Agg")

import matplotlib.pyplot as plt

# ---------------------------------------------------------------------

# GPS: interpolazione posizione per riga + heading

# ---------------------------------------------------------------------

def load_and_interpolate_gps(csv_path, n_rows, line_col="Lines",

lat_col="Latitude", lon_col="Longitude", alt_col="Altitude"):

"""

Interpola lat/lon/altitudine per ogni riga dell'immagine (0..n_rows-1),

a partire dai fix GPS registrati (uno ogni N righe). Extrapolazione

costante (clamp) fuori dal range coperto dai fix, con avviso.

"""

df = pd.read_csv(csv_path).sort_values(line_col)

lines = df[line_col].values.astype(float)

lat = df[lat_col].values.astype(float)

lon = df[lon_col].values.astype(float)

alt = df[alt_col].values.astype(float)

row_idx = np.arange(n_rows, dtype=float)

if row_idx.min() < lines.min() or row_idx.max() > lines.max():

print(f"[attenzione] Le righe dell'immagine (0-{n_rows-1}) escono dal range coperto "

f"dai fix GPS ({lines.min():.0f}-{lines.max():.0f}); le righe fuori range "

f"useranno il valore del fix piu' vicino (estrapolazione costante).")

lat_i = np.interp(row_idx, lines, lat)

lon_i = np.interp(row_idx, lines, lon)

alt_i = np.interp(row_idx, lines, alt)

return lat_i, lon_i, alt_i, df

def heading_from_track(lat, lon, mode="global"):

"""

Stima l'heading (azimuth, gradi da nord) per ogni riga.

mode='global': un unico heading (PCA su tutta la traccia) per tutte le righe

- robusto, adatto a voli rettilinei (il caso tipico).

mode='local' : bearing punto-punto (differenza centrata), utile se il

volo non e' perfettamente dritto.

"""

n = len(lat)

lat0 = np.radians(np.mean(lat))

R = 6371000.0

x = np.radians(lon - lon.mean()) * R * np.cos(lat0)

y = np.radians(lat - lat.mean()) * R

pts = np.column_stack([x, y])

if mode == "global":

pts_c = pts - pts.mean(axis=0)

cov = np.cov(pts_c.T)

eigvals, eigvecs = np.linalg.eigh(cov)

main_dir = eigvecs[:, np.argmax(eigvals)]

az = np.degrees(np.arctan2(main_dir[0], main_dir[1])) % 360

# verifica il verso (potrebbe essere az o az+180): usa il verso del

# moto reale (dal primo all'ultimo punto) per orientarlo correttamente

overall = np.degrees(np.arctan2(x[-1] - x[0], y[-1] - y[0])) % 360

if min(abs(az - overall), 360 - abs(az - overall)) > 90:

az = (az + 180) % 360

return np.full(n, az)

# locale: bearing centrato, con gestione dei bordi

heading = np.zeros(n)

for i in range(n):

i0 = max(0, i - 1)

i1 = min(n - 1, i + 1)

dx = x[i1] - x[i0]

dy = y[i1] - y[i0]

heading[i] = np.degrees(np.arctan2(dx, dy)) % 360

return heading

# ---------------------------------------------------------------------

# Geometria pushbroom: da (riga, colonna) a (lat, lon) di terra

# ---------------------------------------------------------------------

def build_geolocation_grid(lat, lon, alt_agl, heading_deg, n_cols, fov_deg,

pitch_deg=None, roll_deg=None, flip_across_track=False):

"""

Calcola lat/lon di terra per ogni pixel (n_rows x n_cols), assumendo

terreno piatto e velivolo nadir-stabilizzato (salvo pitch/roll forniti).

Ritorna (lat_grid, lon_grid) shape (n_rows, n_cols).

"""

n_rows = len(lat)

half_fov = fov_deg / 2.0

col_idx = np.arange(n_cols)

center = (n_cols - 1) / 2.0

theta_c = (col_idx - center) / center * half_fov # gradi, - a sx, + a dx (o viceversa)

if flip_across_track:

theta_c = -theta_c

theta_c_rad = np.radians(theta_c)

if pitch_deg is None:

pitch_deg = np.zeros(n_rows)

if roll_deg is None:

roll_deg = np.zeros(n_rows)

R_earth = 6371000.0

lat_grid = np.empty((n_rows, n_cols))

lon_grid = np.empty((n_rows, n_cols))

psi = np.radians(heading_deg) # heading (yaw), rad

right_az = psi + np.pi / 2 # direzione "a destra" del volo (azimuth)

for r in range(n_rows):

h_agl = alt_agl[r]

if h_agl <= 0:

h_agl = np.nan # evita risultati assurdi se l'altitudine e' invalida

# NB: pitch/roll qui non applicati con rotazione completa (si assume

# nadir); un'estensione futura puo' aggiungere la rotazione 3D

# completa (roll attorno all'asse di avanzamento, pitch attorno

# all'asse trasversale) se il file GPS fornisce valori attendibili.

ground_offset = h_agl * np.tan(theta_c_rad) # metri, lungo l'asse across-track

north_off = ground_offset * np.cos(right_az[r])

east_off = ground_offset * np.sin(right_az[r])

dlat = (north_off / R_earth) * (180.0 / np.pi)

dlon = (east_off / (R_earth * np.cos(np.radians(lat[r])))) * (180.0 / np.pi)

lat_grid[r, :] = lat[r] + dlat

lon_grid[r, :] = lon[r] + dlon

return lat_grid, lon_grid

# ---------------------------------------------------------------------

# Proiezione UTM e resampling su griglia regolare

# ---------------------------------------------------------------------

def utm_epsg_from_lonlat(lon, lat):

zone = int((lon + 180) / 6) + 1

return (32600 + zone) if lat >= 0 else (32700 + zone)

def fill_small_gaps(image_uint8, filled_mask, max_fill_dist_px):

"""

Riempie i piccoli vuoti isolati (1-2 pixel) con il valore del pixel

valido piu' vicino, entro una distanza massima (in pixel di output).

Questo NON riempie le grandi zone nodata ai bordi/angoli del bounding

box (quelle sono corrette: lo swath e' un rettangolo ruotato rispetto

agli assi E/N della griglia UTM, quindi il bounding box allineato agli

assi contiene inevitabilmente due triangoli vuoti agli angoli - non e'

un difetto, e' la geometria della rotazione). Serve solo a colmare

micro-vuoti dovuti a piccola sotto-densita' locale di campionamento.

"""

empty = ~filled_mask

if not empty.any():

return image_uint8, filled_mask

dist, (idx_r, idx_c) = distance_transform_edt(empty, return_indices=True)

fillable = empty & (dist <= max_fill_dist_px)

out = image_uint8.copy()

new_mask = filled_mask.copy()

for ch in range(image_uint8.shape[2]):

channel = image_uint8[:, :, ch]

filled_channel = channel[idx_r, idx_c]

ch_out = out[:, :, ch]

ch_out[fillable] = filled_channel[fillable]

out[:, :, ch] = ch_out

new_mask[fillable] = True

return out, new_mask

def orthorectify(image_path, gps_csv, out_path, fov_deg, nodata_thresh=3.0,

pixel_size=None, flip_across_track=False, heading_mode="global",

agl_height_override=None, line_col="Lines", fill_gap_px=3):

img = Image.open(image_path).convert("RGB")

arr = np.asarray(img).astype(np.float64)

n_rows, n_cols = arr.shape[0], arr.shape[1]

print(f"Immagine caricata: {n_rows} righe x {n_cols} colonne")

luma = arr.mean(axis=2)

valid_mask = luma > nodata_thresh

lat, lon, alt, gps_df = load_and_interpolate_gps(gps_csv, n_rows, line_col=line_col)

if agl_height_override is not None:

alt = np.full(n_rows, agl_height_override)

print(f"Altitudine AGL forzata a {agl_height_override} m per tutte le righe")

else:

print(f"Altitudine AGL dal file GPS: min={alt.min():.1f} m, max={alt.max():.1f} m, media={alt.mean():.1f} m")

pitch = gps_df["Pitch"].values.astype(float) if "Pitch" in gps_df.columns else None

roll = gps_df["Roll"].values.astype(float) if "Roll" in gps_df.columns else None

if pitch is not None and np.allclose(pitch, 0) and roll is not None and np.allclose(roll, 0):

print("[info] Pitch/Roll nel file GPS sono costanti a zero (probabilmente non popolati): "

"assumo velivolo nadir-stabilizzato (nessuna correzione di assetto oltre l'heading).")

pitch_interp = roll_interp = None

elif pitch is not None and roll is not None:

row_idx = np.arange(n_rows, dtype=float)

lines = gps_df[line_col].values.astype(float)

pitch_interp = np.interp(row_idx, lines, pitch)

roll_interp = np.interp(row_idx, lines, roll)

print("[info] Uso i valori di Pitch/Roll forniti nel file GPS (non applicata rotazione 3D completa "

"in questa versione semplificata: solo heading applicato).")

else:

pitch_interp = roll_interp = None

heading = heading_from_track(lat, lon, mode=heading_mode)

print(f"Heading stimato: min={heading.min():.1f} deg, max={heading.max():.1f} deg "

f"({'costante (modo global)' if heading_mode == 'global' else 'variabile (modo local)'})")

print(f"FOV: {fov_deg:.2f} deg | flip_across_track={flip_across_track}")

lat_grid, lon_grid = build_geolocation_grid(

lat, lon, alt, heading, n_cols, fov_deg,

pitch_deg=pitch_interp, roll_deg=roll_interp, flip_across_track=flip_across_track

)

# --- proiezione in UTM ---

epsg = utm_epsg_from_lonlat(np.nanmean(lon_grid), np.nanmean(lat_grid))

print(f"CRS di output scelto: EPSG:{epsg} (UTM automatico dal baricentro dell'area)")

transformer = pyproj.Transformer.from_crs("EPSG:4326", f"EPSG:{epsg}", always_xy=True)

easting, northing = transformer.transform(lon_grid, lat_grid)

valid = valid_mask & np.isfinite(easting) & np.isfinite(northing)

if not valid.any():

raise ValueError("Nessun pixel valido da ortorettificare (controlla nodata_thresh o i dati GPS)")

e_min, e_max = easting[valid].min(), easting[valid].max()

n_min, n_max = northing[valid].min(), northing[valid].max()

print(f"Estensione ortofoto: E [{e_min:.1f}, {e_max:.1f}] m, N [{n_min:.1f}, {n_max:.1f}] m "

f"({e_max-e_min:.1f} x {n_max-n_min:.1f} m)")

if pixel_size is None:

# GSD approssimato al nadir: altitudine media * (FOV in rad / n_cols)

gsd = float(np.nanmean(alt)) * np.radians(fov_deg) / n_cols

pixel_size = max(gsd, 0.01)

print(f"Dimensione pixel di output (auto, da GSD al nadir): {pixel_size:.4f} m")

else:

print(f"Dimensione pixel di output (specificata): {pixel_size:.4f} m")

out_cols = int(np.ceil((e_max - e_min) / pixel_size)) + 1

out_rows = int(np.ceil((n_max - n_min) / pixel_size)) + 1

print(f"Dimensioni griglia di output: {out_rows} righe x {out_cols} colonne")

# --- binning (splat) dei pixel sorgente sulla griglia di output ---

col_out = ((easting - e_min) / pixel_size).astype(np.int64)

row_out = ((n_max - northing) / pixel_size).astype(np.int64) # riga 0 = Nord (northing massimo)

ok = valid & (col_out >= 0) & (col_out < out_cols) & (row_out >= 0) & (row_out < out_rows)

sum_rgb = np.zeros((out_rows, out_cols, 3), dtype=np.float64)

count = np.zeros((out_rows, out_cols), dtype=np.int64)

flat_row = row_out[ok]

flat_col = col_out[ok]

flat_idx = flat_row * out_cols + flat_col

for ch in range(3):

vals = arr[:, :, ch][ok]

sums = np.bincount(flat_idx, weights=vals, minlength=out_rows * out_cols)

sum_rgb[:, :, ch] = sums.reshape(out_rows, out_cols)

counts_flat = np.bincount(flat_idx, minlength=out_rows * out_cols)

count = counts_flat.reshape(out_rows, out_cols)

with np.errstate(invalid="ignore", divide="ignore"):

ortho = sum_rgb / count[:, :, np.newaxis]

nodata_out = count == 0

ortho[nodata_out] = 0

ortho_uint8 = np.clip(ortho, 0, 255).astype(np.uint8)

coverage_pct = 100 * (~nodata_out).sum() / nodata_out.size

print(f"Copertura griglia di output (prima del gap-fill): {coverage_pct:.1f}% delle celle riempite. "

f"NB: una copertura ben sotto il 100% e' normale e attesa: lo swath e' un rettangolo ruotato "

f"rispetto agli assi E/N della griglia UTM, quindi il bounding box allineato agli assi include "

f"inevitabilmente due triangoli vuoti agli angoli (non e' un artefatto di ricampionamento).")

if fill_gap_px > 0:

valid_out_mask = ~nodata_out

ortho_uint8, valid_out_mask = fill_small_gaps(ortho_uint8, valid_out_mask, fill_gap_px)

nodata_out = ~valid_out_mask

coverage_pct_after = 100 * valid_out_mask.sum() / valid_out_mask.size

print(f"Copertura dopo gap-fill dei soli micro-vuoti (raggio max {fill_gap_px} px): {coverage_pct_after:.1f}%")

# --- scrittura GeoTIFF ---

transform = Affine.translation(e_min, n_max) * Affine.scale(pixel_size, -pixel_size)

out_path = Path(out_path)

out_path.parent.mkdir(parents=True, exist_ok=True)

with rasterio.open(

out_path, "w",

driver="GTiff",

height=out_rows, width=out_cols, count=3,

dtype=np.uint8, crs=f"EPSG:{epsg}", transform=transform,

nodata=0,

compress="deflate",

) as dst:

for ch in range(3):

dst.write(ortho_uint8[:, :, ch], ch + 1)

print(f"\nOrtofoto GeoTIFF salvata in: {out_path}")

# --- anteprima PNG rapida ---

preview_path = out_path.with_suffix(".preview.png")

fig, ax = plt.subplots(figsize=(8, 8 * out_rows / max(out_cols, 1)))

ax.imshow(ortho_uint8, extent=[e_min, e_min + out_cols * pixel_size,

n_max - out_rows * pixel_size, n_max])

ax.set_xlabel(f"Easting (m, EPSG:{epsg})")

ax.set_ylabel("Northing (m)")

ax.set_title("Anteprima ortofoto")

fig.tight_layout()

fig.savefig(preview_path, dpi=150)

plt.close(fig)

print(f"Anteprima salvata in: {preview_path}")

return out_path

def main():

parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)

parser.add_argument("image", help="Immagine true-color pushbroom (PNG/JPEG/TIFF)")

parser.add_argument("gps_csv", help="File CSV GPS/IMU (con colonne Lines, Latitude, Longitude, Altitude, ...)")

parser.add_argument("--out", default="ortho.tif", help="Percorso del GeoTIFF di output")

parser.add_argument("--fov", type=float, required=True, help="FOV across-track totale del sensore (gradi)")

parser.add_argument("--nodata-thresh", type=float, default=3.0, help="Soglia luma per nodata (default 3.0)")

parser.add_argument("--pixel-size", type=float, default=None, help="Dimensione pixel output in metri (default: auto da GSD al nadir)")

parser.add_argument("--flip-across-track", action="store_true",

help="Inverte il lato sinistra/destra (usa se l'output risulta specchiato)")

parser.add_argument("--heading-mode", choices=["global", "local"], default="global",

help="'global': heading unico stimato via PCA su tutta la traccia (default, adatto a voli dritti). "

"'local': bearing punto-punto (per voli non rettilinei)")

parser.add_argument("--agl-height", type=float, default=None,

help="Forza l'altitudine AGL (m) per tutte le righe invece di usare la colonna Altitude del GPS")

parser.add_argument("--line-col", default="Lines", help="Nome della colonna indice-riga nel CSV GPS (default: Lines)")

parser.add_argument("--fill-gap-px", type=int, default=3,

help="Raggio massimo (in pixel di output) per il gap-fill nearest-neighbor "

"dei piccoli vuoti da aliasing griglia ruotata (default: 3, 0=disattiva)")

args = parser.parse_args()

orthorectify(args.image, args.gps_csv, args.out, args.fov,

nodata_thresh=args.nodata_thresh, pixel_size=args.pixel_size,

flip_across_track=args.flip_across_track, heading_mode=args.heading_mode,

agl_height_override=args.agl_height, line_col=args.line_col,

fill_gap_px=args.fill_gap_px)

if __name__ == "__main__":

main()