Something went wrong. Try again.
Inject EXIF GPS data into a meshroom point cloud
Something went wrong. Try again.
16 kB · 429 lines · Created 1mo ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430"""GPS-based georeferencing of a Meshroom dense point cloud.
Reads SfMData (which contains camera poses in reconstruction space + GPS EXIFper image), solves a 7-parameter Helmert similarity transform (Umeyamaalgorithm) between the two coordinate sets, then applies it to a PLY pointcloud to produce a georeferenced output in a real-world projected CRS.
Expected accuracy: ~5–15 m (limited by consumer GPS quality)."""
import argparseimport globimport jsonimport loggingimport shutilimport subprocessimport sysimport tempfilefrom pathlib import Path
import numpy as npimport trimeshfrom 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()