""" GPS-based georeferencing of a Meshroom dense point cloud. Reads SfMData (which contains camera poses in reconstruction space + GPS EXIF per image), solves a 7-parameter Helmert similarity transform (Umeyama algorithm) between the two coordinate sets, then applies it to a PLY point cloud to produce a georeferenced output in a real-world projected CRS. Expected accuracy: ~5–15 m (limited by consumer GPS quality). """ import argparse import glob import json import logging import shutil import subprocess import sys import tempfile from pathlib import Path import numpy as np import trimesh from pyproj import CRS, Transformer # --------------------------------------------------------------------------- # GPS parsing helpers # --------------------------------------------------------------------------- def _dms_to_decimal(dms_str, ref): """Convert a DMS string like '48, 51, 24.123' + ref 'N'/'S'/'E'/'W' to decimal degrees.""" parts = [float(x.strip()) for x in dms_str.split(",")] decimal = parts[0] + parts[1] / 60.0 + parts[2] / 3600.0 if ref in ("S", "W"): decimal = -decimal return decimal def _parse_altitude(alt_str): """Parse an altitude string that may be a fraction like '1234/10'.""" alt_str = str(alt_str).strip() if "/" in alt_str: num, den = alt_str.split("/", 1) return float(num) / float(den) return float(alt_str) # --------------------------------------------------------------------------- # SfMData parsing # --------------------------------------------------------------------------- def load_camera_pairs(sfm_path): """ Return a list of (sfm_xyz, lat, lon, alt_m, h_error_or_None) tuples for every camera that has both a pose and valid GPS EXIF data. """ # Detect binary Alembic files early and give an actionable error. with open(sfm_path, "rb") as fb: magic = fb.read(8) if magic[:2] == b"\xff\xff" or magic[4:8] == b"Oggy" or not ( magic.lstrip(b" \t\r\n").startswith(b"{") ): raise ValueError( f"The file '{sfm_path}' does not appear to be a JSON SfMData file.\n" "In Meshroom, connect 'Views and Poses' to the 'Views and Poses' output of " "StructureFromMotion (cameras.sfm), NOT the 'SfM' output (sfm.abc is binary Alembic)." ) with open(sfm_path, "r", encoding="utf-8") as f: sfm = json.load(f) # Build pose lookup: poseId -> center [x, y, z] pose_by_id = {} for entry in sfm.get("poses", []): pid = str(entry["poseId"]) center = entry["pose"]["transform"]["center"] pose_by_id[pid] = [float(v) for v in center] pairs = [] for view in sfm.get("views", []): pose_id = str(view.get("poseId", "")) if pose_id not in pose_by_id: continue # camera not reconstructed meta = view.get("metadata", {}) try: lat = _dms_to_decimal(meta["GPS:Latitude"], meta["GPS:LatitudeRef"]) lon = _dms_to_decimal(meta["GPS:Longitude"], meta["GPS:LongitudeRef"]) alt = _parse_altitude(meta.get("GPS:Altitude", "0")) except (KeyError, ValueError, TypeError): continue # no / invalid GPS in this image h_error = None try: h_error = float(meta["GPS:HPositioningError"]) except (KeyError, ValueError, TypeError): pass pairs.append((pose_by_id[pose_id], lat, lon, alt, h_error)) return pairs # --------------------------------------------------------------------------- # Coordinate system helpers # --------------------------------------------------------------------------- def auto_utm_epsg(lat, lon): """Return the EPSG code for the UTM zone that covers (lat, lon).""" zone = int((lon + 180) / 6) + 1 if lat >= 0: epsg = 32600 + zone # WGS 84 / UTM zone ##N else: epsg = 32700 + zone # WGS 84 / UTM zone ##S return epsg def gps_to_projected(lats, lons, alts, target_epsg): """Convert arrays of lat/lon/alt (WGS84) to XYZ in target_epsg (meters).""" transformer = Transformer.from_crs( CRS.from_epsg(4326), # WGS 84 geographic CRS.from_epsg(target_epsg), always_xy=True, # input order: lon, lat ) xs, ys = transformer.transform(lons, lats) return np.column_stack([xs, ys, alts]) # --------------------------------------------------------------------------- # Umeyama similarity transform # --------------------------------------------------------------------------- def umeyama(src, dst, weights=None): """ Solve a similarity transform T such that dst ≈ c * R @ src + t. Parameters ---------- src : (N, 3) array — source points (SfM reconstruction space) dst : (N, 3) array — destination points (projected world coordinates) weights : (N,) array or None — per-point weights (higher = more trusted). If None, all points are weighted equally. Returns ------- R : (3, 3) rotation matrix c : float scale factor t : (3,) translation vector transform4x4 : (4, 4) homogeneous similarity matrix """ n = src.shape[0] if weights is None: w = np.full(n, 1.0 / n) else: w = weights / weights.sum() mu_s = (w[:, None] * src).sum(axis=0) mu_d = (w[:, None] * dst).sum(axis=0) A = src - mu_s B = dst - mu_d sigma_s = (w * (A ** 2).sum(axis=1)).sum() H = (B * w[:, None]).T @ A U, S, Vt = np.linalg.svd(H) # Handle reflections d = np.sign(np.linalg.det(U @ Vt)) D = np.diag([1.0, 1.0, d]) R = U @ D @ Vt c = np.sum(S * np.array([1.0, 1.0, d])) / sigma_s t = mu_d - c * R @ mu_s # Build 4×4 homogeneous matrix T = np.eye(4) T[:3, :3] = c * R T[:3, 3] = t return R, c, t, T # --------------------------------------------------------------------------- # LAZ export # --------------------------------------------------------------------------- def _find_pdal(): """Return path to pdal executable, or None if not found.""" pdal = shutil.which("pdal") if pdal: return pdal search_patterns = [ "C:/Program Files/QGIS*/bin/pdal.exe", "C:/Program Files/QGIS*/apps/pdal/bin/pdal.exe", "C:/OSGeo4W*/bin/pdal.exe", ] for pat in search_patterns: matches = sorted(glob.glob(pat)) if matches: return matches[-1] # newest QGIS version return None def _write_laz_with_crs(xyz, colors, epsg, out_path): """Write a LAS 1.4 LAZ file with an embedded WKT1 CRS VLR.""" import laspy crs = CRS.from_epsg(epsg) wkt_bytes = crs.to_wkt(version="WKT1_GDAL").encode("utf-8") + b"\x00" crs_vlr = laspy.VLR( user_id="LASF_Projection", record_id=2112, description="OGC Coordinate System WKT", record_data=wkt_bytes, ) has_color = colors is not None and len(colors) == len(xyz) point_format = 2 if has_color else 0 # format 2 = XYZ + RGB header = laspy.LasHeader(point_format=point_format, version="1.4") header.vlrs.append(crs_vlr) header.offsets = xyz.min(axis=0) header.scales = np.array([0.001, 0.001, 0.001]) # 1 mm precision las = laspy.LasData(header=header) las.x = xyz[:, 0] las.y = xyz[:, 1] las.z = xyz[:, 2] if has_color: # trimesh colors are 0-255 uint8; LAS expects 0-65535 uint16 rgb = np.asarray(colors)[:, :3].astype(np.uint16) * 256 las.red = rgb[:, 0] las.green = rgb[:, 1] las.blue = rgb[:, 2] las.write(out_path) def _write_laz(xyz, colors, epsg, out_path, log): """ Write a georeferenced point cloud for GIS tools. Tries to produce a COPC LAZ (using PDAL from the QGIS installation) since QGIS natively reads CRS from COPC files. Falls back to a regular LAZ with an embedded WKT CRS VLR if PDAL is not found. """ Path(out_path).parent.mkdir(parents=True, exist_ok=True) pdal_exe = _find_pdal() if pdal_exe: log.info("Found PDAL: %s — writing COPC", pdal_exe) # Write intermediate LAZ, then convert to COPC with PDAL tmp_laz = Path(tempfile.mktemp(suffix=".laz")) # noqa: S306 — temp file, no user data try: _write_laz_with_crs(xyz, colors, epsg, str(tmp_laz)) result = subprocess.run( [pdal_exe, "translate", str(tmp_laz), out_path, f"--writers.copc.a_srs=EPSG:{epsg}"], capture_output=True, text=True, timeout=300, ) if result.returncode == 0: log.info("COPC file saved: %s", out_path) return log.warning("PDAL COPC conversion failed (rc=%d): %s", result.returncode, result.stderr[:400]) finally: if tmp_laz.exists(): tmp_laz.unlink() # Fallback: regular LAZ with WKT VLR. # Rename .copc.laz → .laz so the extension matches the actual format. fallback_path = out_path.replace(".copc.laz", ".laz") if out_path.endswith(".copc.laz") else out_path log.warning( "PDAL not found — writing plain LAZ instead of COPC. " "CRS (EPSG:%d) is embedded in the file but you may need to set it manually in QGIS. " "File: %s", epsg, fallback_path ) _write_laz_with_crs(xyz, colors, epsg, fallback_path) log.info("LAZ file saved: %s", fallback_path) # --------------------------------------------------------------------------- # Main # --------------------------------------------------------------------------- def main(): parser = argparse.ArgumentParser(description="Georeference a Meshroom PLY point cloud using GPS EXIF data.") parser.add_argument("--inputSfMData", required=True, help="Path to SfMData JSON file") parser.add_argument("--inputPointCloud", required=True, help="Path to input PLY point cloud") parser.add_argument("--outputPointCloud",required=True, help="Path for georeferenced output PLY") parser.add_argument("--outputTransform", required=True, help="Path for transform info JSON") parser.add_argument("--outputGPS", default="", help="Optional path for gps.json (centroid lat/lon, compatible with GetGPSData output)") parser.add_argument("--outputLAZ", default="", help="Optional path for georeferenced output LAZ (for QGIS/PDAL)") parser.add_argument("--outputFolder", default="", help="Output folder (passed by Meshroom, unused directly)") parser.add_argument("--manualOffsetX", type=float, default=0.0, help="Easting offset in metres") parser.add_argument("--manualOffsetY", type=float, default=0.0, help="Northing offset in metres") parser.add_argument("--manualOffsetZ", type=float, default=0.0, help="Elevation offset in metres") parser.add_argument("--outputCRS", default="auto", help="Target CRS as EPSG code (e.g. 'EPSG:32632') or 'auto'") parser.add_argument("--verboseLevel", default="info", help="Verbosity: debug, info, warning, error") args = parser.parse_args() log_level = getattr(logging, args.verboseLevel.upper(), logging.INFO) logging.basicConfig(level=log_level, format="%(levelname)s: %(message)s") log = logging.getLogger(__name__) # --- Load camera pairs --- log.info("Loading SfMData: %s", args.inputSfMData) pairs = load_camera_pairs(args.inputSfMData) if len(pairs) < 3: log.error("Need at least 3 cameras with GPS data and valid poses; found %d.", len(pairs)) sys.exit(1) log.info("Found %d cameras with GPS + pose.", len(pairs)) sfm_pts = np.array([p[0] for p in pairs]) # (N, 3) reconstruction space lats = np.array([p[1] for p in pairs]) lons = np.array([p[2] for p in pairs]) alts = np.array([p[3] for p in pairs]) h_errors = np.array([p[4] if p[4] is not None else np.nan for p in pairs]) # --- Build weights from GPS accuracy --- n_with_error = int(np.isfinite(h_errors).sum()) if n_with_error >= 3: # Fill missing with median so they don't dominate or vanish filled = np.where(np.isfinite(h_errors), h_errors, np.nanmedian(h_errors)) weights = 1.0 / (filled ** 2) # inverse-variance weighting log.info("Weighted Umeyama: %d/%d cameras have GPS:HPositioningError", n_with_error, len(pairs)) else: weights = None log.info("GPS:HPositioningError not available — using equal weights") # --- Choose CRS --- if args.outputCRS.lower() == "auto": epsg = auto_utm_epsg(float(np.mean(lats)), float(np.mean(lons))) crs_str = f"EPSG:{epsg}" log.info("Auto-selected CRS: %s", crs_str) else: crs_str = args.outputCRS epsg = int(crs_str.upper().replace("EPSG:", "")) world_pts = gps_to_projected(lats, lons, alts, epsg) # (N, 3) UTM meters # --- Solve similarity transform --- log.info("Solving Umeyama similarity transform...") R, c, t, T4 = umeyama(sfm_pts, world_pts, weights) log.info("Scale factor: %.6f", c) log.debug("Rotation:\n%s", R) log.debug("Translation: %s", t) # Per-camera residuals (in metres) sfm_h = np.hstack([sfm_pts, np.ones((len(sfm_pts), 1))]) predicted = (T4 @ sfm_h.T).T[:, :3] residuals = np.linalg.norm(predicted - world_pts, axis=1) log.info("Residuals mean=%.2f m max=%.2f m min=%.2f m", residuals.mean(), residuals.max(), residuals.min()) # --- Load and transform point cloud --- log.info("Loading point cloud: %s", args.inputPointCloud) cloud = trimesh.load(args.inputPointCloud, process=False) if not hasattr(cloud, "vertices"): log.error("Could not read vertices from point cloud file.") sys.exit(1) vertices = np.asarray(cloud.vertices) log.info("Transforming %d points...", len(vertices)) ones = np.ones((len(vertices), 1)) verts_h = np.hstack([vertices, ones]) transformed = (T4 @ verts_h.T).T[:, :3] # --- Apply manual offset --- offset = np.array([args.manualOffsetX, args.manualOffsetY, args.manualOffsetZ]) if np.any(offset != 0): transformed = transformed + offset log.info("Applied manual offset: X=%.3f Y=%.3f Z=%.3f m", *offset) # Preserve colours if present if isinstance(cloud, trimesh.PointCloud): out_cloud = trimesh.PointCloud(vertices=transformed, colors=cloud.colors) else: cloud.vertices = transformed out_cloud = cloud # --- Write output PLY --- Path(args.outputPointCloud).parent.mkdir(parents=True, exist_ok=True) out_cloud.export(args.outputPointCloud) log.info("Georeferenced point cloud saved: %s", args.outputPointCloud) # --- Write GPS centroid JSON (compatible with GetGPSData output) --- if args.outputGPS: Path(args.outputGPS).parent.mkdir(parents=True, exist_ok=True) gps_data = {"latitude": float(np.mean(lats)), "longitude": float(np.mean(lons))} with open(args.outputGPS, "w") as f: json.dump(gps_data, f, indent=4) log.info("GPS centroid saved: %s", args.outputGPS) # --- Write LAZ (for QGIS/GIS tools) --- if args.outputLAZ: _write_laz(transformed, getattr(out_cloud, "colors", None), epsg, args.outputLAZ, log) # --- Write transform JSON --- transform_info = { "outputCRS": crs_str, "inputCRS": "SfM reconstruction space (arbitrary)", "scaleFactor": float(c), "transformMatrix4x4": T4.tolist(), "weightedUmeyama": weights is not None, "numCamerasWithAccuracy": n_with_error, "manualOffset_m": {"x": float(offset[0]), "y": float(offset[1]), "z": float(offset[2])}, "residuals_m": { "mean": float(residuals.mean()), "max": float(residuals.max()), "min": float(residuals.min()), "per_camera": residuals.tolist(), }, "numCamerasUsed": len(pairs), } Path(args.outputTransform).parent.mkdir(parents=True, exist_ok=True) with open(args.outputTransform, "w") as f: json.dump(transform_info, f, indent=4) log.info("Transform info saved: %s", args.outputTransform) if __name__ == "__main__": main()