diff --git a/query/IMG_20251212_153550.jpg b/query/IMG_20251212_153550.jpg new file mode 100644 index 00000000..f2d081f7 Binary files /dev/null and b/query/IMG_20251212_153550.jpg differ diff --git a/query/IMG_20251212_154309.jpg b/query/IMG_20251212_154309.jpg new file mode 100644 index 00000000..6c02ca7c Binary files /dev/null and b/query/IMG_20251212_154309.jpg differ diff --git a/query/img1.jpg b/query/img1.jpg new file mode 100644 index 00000000..9adc0690 Binary files /dev/null and b/query/img1.jpg differ diff --git a/query/img2.jpg b/query/img2.jpg new file mode 100644 index 00000000..f7c3dfbf Binary files /dev/null and b/query/img2.jpg differ diff --git a/src/localize.py b/src/localize.py new file mode 100644 index 00000000..a96c2723 --- /dev/null +++ b/src/localize.py @@ -0,0 +1,171 @@ +"""Minimal single-image localization against an existing SfM model. + +Goals: +- Do NOT re-process the mapping dataset. +- Reuse mapping outputs: SfM model + DB local features + DB global descriptors. +- Only compute query descriptors/features if they are missing (unless OVERWRITE=True). +""" + +from pathlib import Path +import shutil +import uuid + +import pycolmap + +from hloc import extract_features, localize_sfm, match_features, pairs_from_retrieval + + +# --- CONFIGURATION (edit these) --- +outputs = Path('outputs/kulliye/') +sfm_dir = outputs / 'sfm' +query_dir = Path('query') +query_name = 'IMG_20251212_153550.jpg' +top_k = 50 +OVERWRITE = False +KEEP_CACHE = False # if False, only the pose output is kept + +# Must match your mapping pipeline +feature_conf = extract_features.confs['aliked-n16'] +retrieval_conf = extract_features.confs['netvlad'] +matcher_conf = match_features.confs['aliked+lightglue'] + +# Mapping artifacts (must already exist) +features_db = outputs / f"{feature_conf['output']}.h5" +global_db = outputs / f"{retrieval_conf['output']}.h5" + +# Output directory +loc_outputs = outputs / 'localization' +loc_outputs.mkdir(exist_ok=True) +results = loc_outputs / 'poses.txt' + + +def _select_sfm_model(root: Path) -> Path: + models_dir = root / 'models' + if not models_dir.exists(): + return root + best_dir = None + best_n = -1 + for sub in sorted(models_dir.iterdir()): + if not sub.is_dir(): + continue + required = ['cameras.bin', 'images.bin', 'points3D.bin'] + if not all((sub / f).exists() for f in required): + continue + try: + rec = pycolmap.Reconstruction(sub) + except Exception: + continue + if len(rec.images) > best_n: + best_n = len(rec.images) + best_dir = sub + return best_dir or root + + +def main(): + if not sfm_dir.exists(): + raise FileNotFoundError(f"SfM model not found: {sfm_dir}") + if not features_db.exists(): + raise FileNotFoundError(f"DB local features missing: {features_db}") + if not global_db.exists(): + raise FileNotFoundError( + f"DB global descriptors missing: {global_db}\n" + "Run mapping once with NetVLAD to create it (global-feats-netvlad.h5)." + ) + + query_path = query_dir / query_name + if not query_path.exists(): + raise FileNotFoundError(f"Query image not found: {query_path}") + + sfm_model = _select_sfm_model(sfm_dir) + ref = pycolmap.Reconstruction(sfm_model) + if len(ref.images) == 0 or len(ref.cameras) == 0: + raise RuntimeError("Reference reconstruction is empty.") + + db_names = [img.name for img in ref.images.values()] + k = min(top_k, len(db_names)) + + # Put intermediate artifacts into a temp directory unless the user wants caching. + # This keeps the workspace clean and prevents accumulation across different queries. + qtag = Path(query_name).stem + if KEEP_CACHE: + work_dir = loc_outputs + global_query = work_dir / f'global_query_{qtag}.h5' + features_query = work_dir / f'query_features_{qtag}.h5' + retrieval_pairs = work_dir / f'retrieval_{qtag}_top{k}.txt' + matches_query_db = work_dir / f'query_db_matches_{qtag}.h5' + queries_list = work_dir / f'queries_with_intrinsics_{qtag}.txt' + cleanup_dir = None + else: + work_dir = loc_outputs / f"tmp_localize_{qtag}_{uuid.uuid4().hex[:8]}" + if work_dir.exists(): + shutil.rmtree(work_dir, ignore_errors=True) + work_dir.mkdir(parents=True, exist_ok=True) + global_query = work_dir / 'global_query.h5' + features_query = work_dir / 'query_features.h5' + retrieval_pairs = work_dir / f'retrieval_top{k}.txt' + matches_query_db = work_dir / 'query_db_matches.h5' + queries_list = work_dir / 'queries_with_intrinsics.txt' + cleanup_dir = work_dir + + # Intrinsics: reuse first camera (minimal; replace with your real intrinsics if needed) + ref_cam = next(iter(ref.cameras.values())) + model_str = getattr(ref_cam, 'model_name', None) or str(ref_cam.model) + if '.' in model_str: + model_str = model_str.split('.')[-1] + params = ' '.join(map(str, ref_cam.params)) + try: + queries_list.write_text( + f"{query_name} {model_str} {ref_cam.width} {ref_cam.height} {params}\n" + ) + + # 1) Retrieval (query descriptor is computed only if missing) + extract_features.main( + retrieval_conf, + query_dir, + image_list=[query_name], + feature_path=global_query, + overwrite=OVERWRITE, + ) + pairs_from_retrieval.main( + descriptors=global_query, + output=retrieval_pairs, + num_matched=k, + db_model=sfm_model, + db_descriptors=global_db, + ) + + # 2) Query local features + matching to DB + extract_features.main( + feature_conf, + query_dir, + image_list=[query_name], + feature_path=features_query, + overwrite=OVERWRITE, + ) + match_features.main( + matcher_conf, + retrieval_pairs, + features=features_query, + features_ref=features_db, + matches=matches_query_db, + overwrite=OVERWRITE, + ) + + # 3) PnP + localize_sfm.main( + reference_sfm=sfm_model, + queries=queries_list, + retrieval=retrieval_pairs, + features=features_query, + matches=matches_query_db, + results=results, + ) + finally: + if cleanup_dir is not None: + shutil.rmtree(cleanup_dir, ignore_errors=True) + + print(f"Pose written to: {results}") + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/sfm.py b/src/sfm.py new file mode 100644 index 00000000..86586946 --- /dev/null +++ b/src/sfm.py @@ -0,0 +1,84 @@ +from pathlib import Path +from hloc import ( + extract_features, + match_features, + pairs_from_exhaustive, + pairs_from_retrieval, + reconstruction, +) + +# --- CONFIGURATION --- +images_path = Path('datasets/12-12-2025/') # Reference images (mapping set) +outputs = Path('outputs/kulliye/') # Where to write SfM artifacts + +# SfM mapping settings +# - For accuracy, prefer retrieval-based pairing to avoid many weak/outlier matches. +# - Set FORCE_RECOMPUTE=True when you changed the dataset or configs. +PAIRING = 'retrieval' # 'retrieval' | 'exhaustive' +NUM_MATCHED = 50 # top-K retrieval pairs per image (typical: 20-50) +FORCE_RECOMPUTE = False + +# Create output directory if it doesn't exist +outputs.mkdir(parents=True, exist_ok=True) + +# Choose models (pretrained) +# Local features + matcher (recommended for higher accuracy) +local_conf = extract_features.confs['aliked-n16'] +matcher_conf = match_features.confs['aliked+lightglue'] + +# Global descriptors for retrieval pairing +global_conf = extract_features.confs['netvlad'] + +# Define file paths (include conf names to avoid accidental mixing) +sfm_dir = outputs / 'sfm' +local_features = outputs / f"{local_conf['output']}.h5" +global_descriptors = outputs / f"{global_conf['output']}.h5" +sfm_pairs = outputs / ( + f"pairs-sfm-{PAIRING}{'' if PAIRING != 'retrieval' else f'-netvlad{NUM_MATCHED}'}.txt" +) +matches = outputs / f"{matcher_conf['output']}.h5" + +def run_mapping(): + print(f"1. Extracting local features ({local_conf['model']['name']})...") + extract_features.main( + local_conf, + images_path, + feature_path=local_features, + overwrite=FORCE_RECOMPUTE, + ) + + print(f"2. Generating pairs ({PAIRING})...") + if PAIRING == 'exhaustive': + pairs_from_exhaustive.main(sfm_pairs, features=local_features) + elif PAIRING == 'retrieval': + print(f"2a. Extracting global descriptors ({global_conf['model']['name']})...") + extract_features.main( + global_conf, + images_path, + feature_path=global_descriptors, + overwrite=FORCE_RECOMPUTE, + ) + pairs_from_retrieval.main( + descriptors=global_descriptors, + output=sfm_pairs, + num_matched=NUM_MATCHED, + ) + else: + raise ValueError(f"Unknown PAIRING={PAIRING!r}. Use 'retrieval' or 'exhaustive'.") + + print(f"3. Matching features ({matcher_conf['model']['name']})...") + match_features.main( + matcher_conf, + sfm_pairs, + features=local_features, + matches=matches, + overwrite=FORCE_RECOMPUTE, + ) + + print("4. Running SfM reconstruction (COLMAP)...") + reconstruction.main(sfm_dir, images_path, sfm_pairs, local_features, matches) + + print(f"Mapping complete! Model saved at: {sfm_dir}") + +if __name__ == '__main__': + run_mapping() \ No newline at end of file diff --git a/src/sfm_geo_align.py b/src/sfm_geo_align.py new file mode 100644 index 00000000..2f0bdcce --- /dev/null +++ b/src/sfm_geo_align.py @@ -0,0 +1,372 @@ +"""Align a COLMAP/pycolmap SfM model to GPS (lat/lon/alt) using a similarity transform. + +This script is designed for your CSV format: + filename,lat,lon,meters,degrees,datetime + +We use: +- lat, lon in decimal degrees +- meters as altitude (meters above sea level) + +What it does: +1) Reads SfM camera centers from a reconstruction (COLMAP model). +2) Converts GPS points to a local ENU frame (meters). +3) Fits a Sim(3) transform: enu ~= s * R * C_sfm + t. +4) Optionally converts a HLoc poses.txt (cam_from_world) to lat/lon/alt. + +Notes: +- The "degrees" column looks like compass heading; it is currently ignored. +- This does not modify the SfM model; it writes alignment parameters to JSON. +""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +import pycolmap + + +# WGS84 constants +_A = 6378137.0 +_F = 1.0 / 298.257223563 +_E2 = _F * (2.0 - _F) +_B = _A * (1.0 - _F) +_EP2 = (_A * _A - _B * _B) / (_B * _B) + + +@dataclass(frozen=True) +class LLA: + lat_deg: float + lon_deg: float + alt_m: float + + +def _deg2rad(x: float) -> float: + return x * math.pi / 180.0 + + +def _rad2deg(x: float) -> float: + return x * 180.0 / math.pi + + +def lla_to_ecef(p: LLA) -> np.ndarray: + lat = _deg2rad(p.lat_deg) + lon = _deg2rad(p.lon_deg) + slat = math.sin(lat) + clat = math.cos(lat) + slon = math.sin(lon) + clon = math.cos(lon) + N = _A / math.sqrt(1.0 - _E2 * slat * slat) + x = (N + p.alt_m) * clat * clon + y = (N + p.alt_m) * clat * slon + z = (N * (1.0 - _E2) + p.alt_m) * slat + return np.array([x, y, z], dtype=float) + + +def ecef_to_lla(xyz: np.ndarray) -> LLA: + x, y, z = map(float, xyz) + lon = math.atan2(y, x) + p = math.hypot(x, y) + if p < 1e-9: + lat = math.copysign(math.pi / 2.0, z) + N = _A / math.sqrt(1.0 - _E2 * math.sin(lat) ** 2) + alt = abs(z) - N * (1.0 - _E2) + return LLA(_rad2deg(lat), _rad2deg(lon), alt) + + theta = math.atan2(z * _A, p * _B) + st = math.sin(theta) + ct = math.cos(theta) + lat = math.atan2(z + _EP2 * _B * st**3, p - _E2 * _A * ct**3) + slat = math.sin(lat) + N = _A / math.sqrt(1.0 - _E2 * slat * slat) + alt = p / math.cos(lat) - N + return LLA(_rad2deg(lat), _rad2deg(lon), alt) + + +def ecef_to_enu(ecef: np.ndarray, origin: LLA) -> np.ndarray: + lat0 = _deg2rad(origin.lat_deg) + lon0 = _deg2rad(origin.lon_deg) + slat0 = math.sin(lat0) + clat0 = math.cos(lat0) + slon0 = math.sin(lon0) + clon0 = math.cos(lon0) + + ecef0 = lla_to_ecef(origin) + d = (ecef - ecef0).astype(float) + + R = np.array( + [ + [-slon0, clon0, 0.0], + [-slat0 * clon0, -slat0 * slon0, clat0], + [clat0 * clon0, clat0 * slon0, slat0], + ], + dtype=float, + ) + return R @ d + + +def enu_to_ecef(enu: np.ndarray, origin: LLA) -> np.ndarray: + lat0 = _deg2rad(origin.lat_deg) + lon0 = _deg2rad(origin.lon_deg) + slat0 = math.sin(lat0) + clat0 = math.cos(lat0) + slon0 = math.sin(lon0) + clon0 = math.cos(lon0) + + ecef0 = lla_to_ecef(origin) + R = np.array( + [ + [-slon0, clon0, 0.0], + [-slat0 * clon0, -slat0 * slon0, clat0], + [clat0 * clon0, clat0 * slon0, slat0], + ], + dtype=float, + ) + return ecef0 + R.T @ enu + + +def mean_lla(values: list[LLA]) -> LLA: + if not values: + raise ValueError("No GPS values") + return LLA( + lat_deg=float(np.mean([v.lat_deg for v in values])), + lon_deg=float(np.mean([v.lon_deg for v in values])), + alt_m=float(np.mean([v.alt_m for v in values])), + ) + + +def _to_float(s: str) -> float: + return float(str(s).strip().replace(",", ".")) + + +def read_coordinates_csv(path: Path) -> dict[str, LLA]: + """Read your 5-column CSV and return mapping filename -> LLA.""" + with path.open(newline="") as f: + reader = csv.DictReader(f) + if not reader.fieldnames: + raise ValueError("CSV has no header") + fields = {h.strip().lower(): h for h in reader.fieldnames} + + def col(*names: str) -> str: + for n in names: + if n in fields: + return fields[n] + raise ValueError(f"Missing required column(s): {names}. Got: {reader.fieldnames}") + + c_name = col("filename", "name") + c_lat = col("lat", "latitude") + c_lon = col("lon", "lng", "longitude") + c_alt = col("meters", "alt", "altitude", "height") + + gps: dict[str, LLA] = {} + for row in reader: + name = (row.get(c_name) or "").strip() + if not name: + continue + lat = _to_float(row[c_lat]) + lon = _to_float(row[c_lon]) + alt = _to_float(row[c_alt]) + gps[name] = LLA(lat, lon, alt) + return gps + + +def camera_center_from_image(img: pycolmap.Image) -> np.ndarray: + pose = img.cam_from_world() + try: + c = pose.inverse().translation + return np.array([c[0], c[1], c[2]], dtype=float) + except Exception: + R = np.array(pose.rotation.matrix(), dtype=float) + t = np.array(pose.translation, dtype=float) + return -R.T @ t + + +def umeyama_sim3(X: np.ndarray, Y: np.ndarray) -> tuple[float, np.ndarray, np.ndarray]: + """Fit Y ≈ s R X + t (least squares). X,Y are Nx3.""" + if X.shape != Y.shape or X.ndim != 2 or X.shape[1] != 3: + raise ValueError("X and Y must be Nx3 with same shape") + n = X.shape[0] + if n < 3: + raise ValueError("Need at least 3 correspondences") + + mx = X.mean(axis=0) + my = Y.mean(axis=0) + Xc = X - mx + Yc = Y - my + + cov = (Yc.T @ Xc) / float(n) + U, D, Vt = np.linalg.svd(cov) + + S = np.eye(3) + if np.linalg.det(U) * np.linalg.det(Vt) < 0: + S[2, 2] = -1.0 + + R = U @ S @ Vt + var_x = float((Xc**2).sum() / n) + s = float((D * np.diag(S)).sum() / var_x) + t = my - s * (R @ mx) + return s, R, t + + +def robust_fit_sim3(X: np.ndarray, Y: np.ndarray) -> tuple[float, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """Two-pass robust fit: fit once, drop large residuals, fit again.""" + s, R, t = umeyama_sim3(X, Y) + pred = (s * (X @ R.T)) + t + res = np.linalg.norm(pred - Y, axis=1) + + med = float(np.median(res)) + thr = max(3.0 * med, 5.0) + inliers = res <= thr + if inliers.sum() >= 3: + s, R, t = umeyama_sim3(X[inliers], Y[inliers]) + pred = (s * (X @ R.T)) + t + res = np.linalg.norm(pred - Y, axis=1) + return s, R, t, inliers, res + + +def read_pose_file_centers(path: Path) -> list[tuple[str, np.ndarray]]: + centers = [] + for line in path.read_text().splitlines(): + line = line.strip() + if not line: + continue + parts = line.split() + if len(parts) != 8: + raise ValueError(f"Unexpected pose format (expected 8 tokens): {line}") + name = parts[0] + qw, qx, qy, qz = map(float, parts[1:5]) + tx, ty, tz = map(float, parts[5:8]) + + # Quaternion (w,x,y,z) -> R + n = qw * qw + qx * qx + qy * qy + qz * qz + if n == 0.0: + R = np.eye(3, dtype=float) + else: + s2 = 2.0 / n + wx, wy, wz = s2 * qw * qx, s2 * qw * qy, s2 * qw * qz + xx, xy, xz = s2 * qx * qx, s2 * qx * qy, s2 * qx * qz + yy, yz, zz = s2 * qy * qy, s2 * qy * qz, s2 * qz * qz + R = np.array( + [ + [1.0 - (yy + zz), xy - wz, xz + wy], + [xy + wz, 1.0 - (xx + zz), yz - wx], + [xz - wy, yz + wx, 1.0 - (xx + yy)], + ], + dtype=float, + ) + t = np.array([tx, ty, tz], dtype=float) + C = -R.T @ t + centers.append((name, C)) + return centers + + +def main() -> None: + ap = argparse.ArgumentParser() + ap.add_argument( + "--sfm", + type=Path, + required=True, + help="SfM model dir (contains cameras.bin/images.bin/points3D.bin)", + ) + ap.add_argument( + "--csv", + type=Path, + required=True, + help="coordinates_5cols.csv (filename,lat,lon,meters,degrees,datetime)", + ) + ap.add_argument( + "--out", + type=Path, + default=Path("outputs/kulliye/geo_align.json"), + help="Output JSON path", + ) + ap.add_argument( + "--poses", + type=Path, + default=None, + help="Optional poses.txt (HLoc) to convert to lat/lon/alt", + ) + ap.add_argument( + "--poses_out", + type=Path, + default=None, + help="Output CSV for converted poses (default: poses.geo.csv)", + ) + args = ap.parse_args() + + gps = read_coordinates_csv(args.csv) + rec = pycolmap.Reconstruction(args.sfm) + if len(rec.images) == 0: + raise RuntimeError("SfM model has 0 images") + + names: list[str] = [] + Xs = [] + Ls: list[LLA] = [] + for img in rec.images.values(): + if not img.name: + continue + if img.name not in gps: + continue + names.append(img.name) + Xs.append(camera_center_from_image(img)) + Ls.append(gps[img.name]) + + if len(Xs) < 3: + raise RuntimeError( + f"Need >=3 GPS-tagged images present in SfM. Found {len(Xs)}." + ) + + origin = mean_lla(Ls) + X = np.stack(Xs, axis=0) + Y = np.stack([ecef_to_enu(lla_to_ecef(gps[n]), origin) for n in names], axis=0) + + s, R, t, inliers, res = robust_fit_sim3(X, Y) + in_res = res[inliers] if np.any(inliers) else res + rmse = float(math.sqrt(float(np.mean(in_res**2)))) + + out = { + "sfm": str(args.sfm), + "csv": str(args.csv), + "origin": {"lat": origin.lat_deg, "lon": origin.lon_deg, "alt": origin.alt_m}, + "sim3": {"scale": float(s), "R": R.tolist(), "t": t.tolist()}, + "stats": { + "num_used": int(len(names)), + "num_inliers": int(int(inliers.sum())), + "rmse_m": rmse, + "median_m": float(np.median(in_res)), + "max_m": float(np.max(in_res)), + }, + "residuals_m": {names[i]: float(res[i]) for i in range(len(names))}, + "inliers": {names[i]: bool(inliers[i]) for i in range(len(names))}, + } + + args.out.parent.mkdir(parents=True, exist_ok=True) + args.out.write_text(json.dumps(out, indent=2)) + print(f"Wrote alignment: {args.out}") + print( + f"RMSE (inliers): {rmse:.2f} m | inliers {int(inliers.sum())}/{len(names)} | median {out['stats']['median_m']:.2f} m" + ) + + if args.poses is not None: + poses_out = args.poses_out + if poses_out is None: + poses_out = args.poses.with_suffix(".geo.csv") + centers = read_pose_file_centers(args.poses) + with poses_out.open("w", newline="") as f: + w = csv.writer(f) + w.writerow(["name", "lat", "lon", "alt", "east_m", "north_m", "up_m"]) + for name, C_sfm in centers: + enu = (s * (C_sfm @ R.T)) + t + ecef = enu_to_ecef(enu, origin) + lla = ecef_to_lla(ecef) + w.writerow([name, lla.lat_deg, lla.lon_deg, lla.alt_m, enu[0], enu[1], enu[2]]) + print(f"Wrote converted poses: {poses_out}") + + +if __name__ == "__main__": + main()