reconstruct3d 0.1.0__py3-none-any.whl

reconstruct3d/__init__.py

@@ -0,0 +1,17 @@
+"""reconstruct3d — pipeline offline de reconstrucción 3D desde video monocular.
+
+Structure-from-Motion incremental + Bundle Adjustment + densificación MVS +
+mallado con CGAL, con front-ends de features intercambiables (SIFT, ORB,
+SuperPoint+LightGlue).
+
+API rápida:
+
+    from reconstruct3d import Pipeline
+    pipe = Pipeline("outputs/run1", frontend="sift", camera="camera.json")
+    pipe.run("video.mp4", stages=["extract", "init", "track", "dense", "mesh"])
+"""
+from reconstruct3d.api import Pipeline, run_all, ALL_STAGES, ARTIFACTS
+from reconstruct3d.core import CameraConfig
+
+__version__ = "0.1.0"
+__all__ = ["Pipeline", "run_all", "CameraConfig", "ALL_STAGES", "ARTIFACTS"]

reconstruct3d/api.py

@@ -0,0 +1,218 @@
+"""API de alto nivel de reconstruct3d.
+
+Envuelve las etapas del pipeline (que en el CLI son subcomandos) en una clase
+`Pipeline` fácil de usar desde código:
+
+    from reconstruct3d import Pipeline
+
+    pipe = Pipeline("outputs/run1", frontend="sift", camera="camera.json")
+    pipe.extract("video.mp4", k_skip=15, m_window=4)
+    pipe.init()
+    pipe.track()
+    pipe.bundle_adjust()
+    pipe.dense("video.mp4")
+    pipe.mesh(method="afront")
+    print(pipe.artifacts())
+
+O todo de una vez, con control de etapas y progreso:
+
+    def on_event(ev):  # ev = {stage, status, message, ...}
+        print(ev["stage"], ev["status"], ev.get("message", ""))
+
+    pipe = Pipeline("outputs/run1", on_event=on_event)
+    pipe.run("video.mp4", stages=["extract", "init", "track", "dense"],
+             config={"extract": {"k_skip": 15, "m_window": 4}})
+
+Las etapas comparten el `out_dir` y persisten su estado en disco (sfm_data.pkl,
+map_state.npy, *.ply), igual que el CLI.
+"""
+import os
+import time
+from typing import Callable, Dict, List, Optional, Union
+
+from reconstruct3d.core import CameraConfig
+
+# Artefactos por etapa (nombres de archivo dentro de out_dir).
+ARTIFACTS = {
+    "extract": "sfm_data.pkl",
+    "init": "init_cloud.ply",
+    "track": "tracked_cloud.ply",
+    "ba": "tracked_cloud.ply",
+    "dense": "dense_cloud.ply",
+    "mesh": "mesh.ply",
+}
+
+ALL_STAGES = ["extract", "init", "track", "ba", "dense", "mesh"]
+
+CameraLike = Union[None, str, dict, CameraConfig]
+
+
+def _to_camera(cam: CameraLike) -> Optional[CameraConfig]:
+    if cam is None or isinstance(cam, CameraConfig):
+        return cam
+    if isinstance(cam, dict):
+        return CameraConfig.from_dict(cam)
+    if isinstance(cam, str):
+        return CameraConfig.from_json(cam)
+    raise TypeError(f"camera debe ser None, ruta, dict o CameraConfig, no {type(cam)}")
+
+
+class Pipeline:
+    """Orquesta la reconstrucción 3D etapa por etapa sobre un directorio de salida.
+
+    Parámetros
+    ----------
+    out_dir : str
+        Carpeta donde se leen/escriben todos los artefactos.
+    frontend : str
+        Detector+matcher: 'sift' (default), 'orb' o 'spglue'.
+    camera : None | str | dict | CameraConfig
+        Intrínsecos: ruta a JSON, dict, CameraConfig, o None (defaults).
+    jobs : int
+        Hilos para extracción/matching/densificación (0=auto, 1=secuencial).
+    on_event : callable, opcional
+        Se llama con un dict {stage, status, message, ...} al iniciar/terminar
+        cada etapa. Útil para reportar progreso (p.ej. desde un backend).
+    """
+
+    def __init__(self, out_dir: str, frontend: str = "sift", camera: CameraLike = None,
+                 jobs: int = 0, on_event: Optional[Callable[[dict], None]] = None):
+        self.out_dir = out_dir
+        self.frontend = frontend
+        self.jobs = jobs
+        self.camera = _to_camera(camera)
+        self.on_event = on_event
+        os.makedirs(out_dir, exist_ok=True)
+
+    # ------------------------------------------------------------------ #
+    def _emit(self, stage, status, message="", **extra):
+        if self.on_event:
+            ev = {"stage": stage, "status": status, "message": message,
+                  "time": time.time(), **extra}
+            try:
+                self.on_event(ev)
+            except Exception:
+                pass
+
+    def _path(self, name):
+        return os.path.join(self.out_dir, name)
+
+    # ------------------------------------------------------------------ #
+    # Etapas                                                              #
+    # ------------------------------------------------------------------ #
+    def calibrate(self, board_video, board=(9, 6), square=0.025, k_skip=10,
+                  max_views=40, out=None) -> CameraConfig:
+        """Calibra K desde un video de tablero y la fija como cámara de la pipeline."""
+        from reconstruct3d import calibrate as _cal
+        out = out or os.path.join(self.out_dir, "camera.json")
+        self._emit("calibrate", "start", f"calibrando desde {board_video}")
+        _cal.run_calibration(board_video, board=board, square_size=square,
+                             k_skip=k_skip, max_views=max_views, out=out)
+        self.camera = CameraConfig.from_json(out)
+        self._emit("calibrate", "done", f"camera.json -> {out}", artifact=out)
+        return self.camera
+
+    def extract(self, video, k_skip=5, m_window=3, max_seconds=None,
+                start_seconds=0.0, force=False):
+        from reconstruct3d.core import process_core
+        self._emit("extract", "start", f"features+esenciales ({self.frontend})")
+        process_core(video, self.out_dir, frontend_name=self.frontend,
+                     k_skip=k_skip, m_window=m_window, max_seconds=max_seconds,
+                     start_seconds=start_seconds, camera=self.camera,
+                     reuse_db=not force, jobs=self.jobs)
+        self._emit("extract", "done", artifact=self._path(ARTIFACTS["extract"]))
+        return self
+
+    def init(self):
+        from reconstruct3d.init_sfm import init_reconstruction
+        self._emit("init", "start", "triangulando par semilla")
+        init_reconstruction(self.out_dir)
+        self._emit("init", "done", artifact=self._path(ARTIFACTS["init"]))
+        return self
+
+    def track(self, m_window=3, min_angle=1.5, local_ba=True, ba_every=1, fuse=True):
+        from reconstruct3d.track_sfm import track_frames
+        self._emit("track", "start", "registro incremental + BA local")
+        track_frames(self.out_dir, m_window=m_window, min_angle=min_angle,
+                     local_ba=local_ba, ba_every=ba_every, fuse=fuse)
+        self._emit("track", "done", artifact=self._path(ARTIFACTS["track"]))
+        return self
+
+    def bundle_adjust(self, max_reproj=4.0, max_nfev=300):
+        from reconstruct3d.bundle_adjust import run_bundle_adjustment
+        self._emit("ba", "start", "Bundle Adjustment global")
+        run_bundle_adjustment(self.out_dir, max_reproj_err=max_reproj, max_nfev=max_nfev)
+        self._emit("ba", "done", artifact=self._path(ARTIFACTS["ba"]))
+        return self
+
+    def dense(self, video, neighbor_steps=(1, 2), min_views=2, num_disp=128,
+              block=5, voxel=0.0):
+        from reconstruct3d.dense_mvs import run_dense
+        self._emit("dense", "start", "densificación MVS")
+        run_dense(self.out_dir, video, neighbor_steps=tuple(neighbor_steps),
+                  min_views=min_views, num_disp=num_disp, block=block,
+                  voxel=voxel, jobs=self.jobs)
+        self._emit("dense", "done", artifact=self._path(ARTIFACTS["dense"]))
+        return self
+
+    def mesh(self, input_ply=None, method="afront", outlier_pct=2.0, simplify=0.0,
+             smooth=0, mesh_smooth=2, min_component=0.002, afront_radius=5.0,
+             poisson_spacing_mult=1.0):
+        from reconstruct3d import mesh as _mesh
+        self._emit("mesh", "start", f"mallado CGAL ({method})")
+        _mesh.run_mesh(self.out_dir, input_ply=input_ply, method=method,
+                       outlier_pct=outlier_pct, simplify=simplify, smooth=smooth,
+                       mesh_smooth=mesh_smooth, min_component=min_component,
+                       afront_radius=afront_radius, poisson_spacing_mult=poisson_spacing_mult)
+        self._emit("mesh", "done", artifact=self._path(ARTIFACTS["mesh"]))
+        return self
+
+    # ------------------------------------------------------------------ #
+    def run(self, video, stages: Optional[List[str]] = None,
+            config: Optional[Dict[str, dict]] = None) -> Dict[str, str]:
+        """Ejecuta una secuencia de etapas. Devuelve {etapa: ruta_artefacto}.
+
+        stages: subconjunto/orden de ALL_STAGES (default: todas).
+        config: {etapa: {kwargs}} para parametrizar cada etapa.
+        """
+        stages = stages or list(ALL_STAGES)
+        config = config or {}
+        for s in stages:
+            if s not in ALL_STAGES:
+                raise ValueError(f"Etapa desconocida: {s}. Opciones: {ALL_STAGES}")
+        try:
+            for s in stages:
+                kw = dict(config.get(s, {}))
+                if s == "extract":
+                    self.extract(video, **kw)
+                elif s == "init":
+                    self.init(**kw)
+                elif s == "track":
+                    self.track(**kw)
+                elif s == "ba":
+                    self.bundle_adjust(**kw)
+                elif s == "dense":
+                    self.dense(video, **kw)
+                elif s == "mesh":
+                    self.mesh(**kw)
+        except Exception as e:  # noqa: BLE001 - reportar y propagar
+            self._emit("pipeline", "error", str(e))
+            raise
+        self._emit("pipeline", "done", "pipeline completo")
+        return self.artifacts()
+
+    def artifacts(self) -> Dict[str, str]:
+        """Rutas de artefactos que existen en disco, por etapa."""
+        out = {}
+        for stage, name in ARTIFACTS.items():
+            p = self._path(name)
+            if os.path.exists(p):
+                out[stage] = p
+        return out
+
+
+def run_all(video, out_dir, frontend="sift", camera: CameraLike = None, jobs=0,
+            stages=None, config=None, on_event=None) -> Dict[str, str]:
+    """Atajo: crea una Pipeline y corre las etapas indicadas."""
+    pipe = Pipeline(out_dir, frontend=frontend, camera=camera, jobs=jobs, on_event=on_event)
+    return pipe.run(video, stages=stages, config=config)

reconstruct3d/bundle_adjust.py

@@ -0,0 +1,370 @@
+"""Bundle Adjustment global sobre el mapa reconstruido.
+
+Re-optimiza TODAS las poses de cámara y TODOS los puntos 3D a la vez,
+minimizando el error de reproyección global. Es lo que endereza la trayectoria
+y "aprieta" la nube: el pipeline incremental (init/track) solo acumula error,
+BA lo corrige hacia atrás.
+
+Entrada/Salida: lee y reescribe `map_state.npy` (poses + puntos + obs) y
+re-exporta `tracked_cloud.ply`. Las observaciones 2D se recuperan de
+`sfm_data.pkl` vía obs[frame][kp_idx] -> features[frame].pts[kp_idx].
+
+Proyección vectorizada (Rodrigues sin bucles) + jacobiano disperso: con ~100
+cámaras y decenas de miles de puntos esto corre en segundos/minutos en CPU.
+"""
+import os
+import sys
+import argparse
+import numpy as np
+import cv2
+from scipy.optimize import least_squares
+from scipy.sparse import lil_matrix
+
+from reconstruct3d.core import (
+    SfMDatabase, Features,
+    DB_FILE, MAP_STATE_FILE, TRACKED_CLOUD_FILE,
+)
+from reconstruct3d.init_sfm import export_ply
+
+# Permitir deserializar los pickles de features
+sys.modules['__main__'].Features = Features
+
+
+def rotate(points, rot_vecs):
+    """Rota `points` (M,3) por vectores de rotación `rot_vecs` (M,6->3) usando
+    la fórmula de Rodrigues, completamente vectorizada."""
+    theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis]
+    with np.errstate(invalid='ignore', divide='ignore'):
+        v = np.nan_to_num(rot_vecs / theta)
+    dot = np.sum(points * v, axis=1)[:, np.newaxis]
+    cos_t, sin_t = np.cos(theta), np.sin(theta)
+    return cos_t * points + sin_t * np.cross(v, points) + dot * (1 - cos_t) * v
+
+
+def project(points, cam_params, K):
+    """Proyecta `points` (M,3, mundo) con `cam_params` (M,6 = [rvec|tvec]) a
+    píxeles (M,2). Una fila por observación."""
+    pc = rotate(points, cam_params[:, :3]) + cam_params[:, 3:6]
+    xy = pc[:, :2] / pc[:, 2:3]
+    fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2]
+    u = fx * xy[:, 0] + cx
+    v = fy * xy[:, 1] + cy
+    return np.stack([u, v], axis=1)
+
+
+def _residuals(params, n_cam, n_pts, cam_idx, pt_idx, pts2d, K):
+    cams = params[:n_cam * 6].reshape(n_cam, 6)
+    pts = params[n_cam * 6:].reshape(n_pts, 3)
+    proj = project(pts[pt_idx], cams[cam_idx], K)
+    return (proj - pts2d).ravel()
+
+
+def _jac_sparsity(n_cam, n_pts, cam_idx, pt_idx):
+    """Patrón de esparsidad: cada residuo (u,v) solo depende de su cámara (6
+    params) y su punto (3 params). Imprescindible para que BA sea tratable."""
+    m = cam_idx.size * 2
+    n = n_cam * 6 + n_pts * 3
+    A = lil_matrix((m, n), dtype=int)
+    i = np.arange(cam_idx.size)
+    for s in range(6):
+        A[2 * i, cam_idx * 6 + s] = 1
+        A[2 * i + 1, cam_idx * 6 + s] = 1
+    for s in range(3):
+        A[2 * i, n_cam * 6 + pt_idx * 3 + s] = 1
+        A[2 * i + 1, n_cam * 6 + pt_idx * 3 + s] = 1
+    return A
+
+
+def _pose_to_rt(P):
+    """4x4 world->cam -> vector 6 [rvec|tvec]."""
+    rv, _ = cv2.Rodrigues(P[:3, :3])
+    return np.concatenate([rv.ravel(), P[:3, 3]])
+
+
+def _rt_to_pose(rt):
+    R, _ = cv2.Rodrigues(rt[:3])
+    return np.vstack((np.hstack((R, rt[3:].reshape(3, 1))), [0, 0, 0, 1]))
+
+
+def run_local_ba(map3d, pts_of, K, window_frames, max_nfev=25):
+    """Bundle Adjustment LOCAL por ventana deslizante.
+
+    Optimiza conjuntamente las poses de `window_frames` y los puntos 3D que
+    observan (>=2 vistas en la ventana), manteniendo FIJAS: (a) la cámara ancla
+    —la más antigua de la ventana, para fijar el gauge— y (b) las cámaras
+    externas que también ven esos puntos (aportan restricciones sin moverse).
+
+    Esto corrige el error de cada pose nueva + sus puntos recién triangulados
+    JUNTOS antes de que se propaguen al siguiente frame (ataca el arrastre y el
+    lazo de realimentación). Modifica map3d['poses'] y map3d['points'] in-place.
+
+    pts_of: callable frame -> ndarray (N,2) de keypoints.
+    """
+    window = sorted(window_frames)
+    if len(window) < 3:
+        return
+    win_set = set(window)
+    anchor = window[0]
+    var_frames = window[1:]
+    var_pos = {f: i for i, f in enumerate(var_frames)}
+    n_var = len(var_frames)
+
+    # Puntos observados por >=2 cámaras de la ventana (constreñidos localmente)
+    from collections import Counter
+    cnt = Counter()
+    for f in window:
+        for pid in map3d['obs'][f].values():
+            cnt[pid] += 1
+    pts3d = map3d['points']
+    local_pids = [p for p, c in cnt.items() if c >= 2 and np.isfinite(pts3d[p]).all()]
+    if len(local_pids) < 20:
+        return
+    pid_pos = {p: i for i, p in enumerate(local_pids)}
+    local_set = set(local_pids)
+    n_pts = len(local_pids)
+
+    # Cámaras externas (fuera de la ventana) que ven puntos locales -> fijas
+    ext_fixed = [f for f, d in map3d['obs'].items()
+                 if f not in win_set and any(pid in local_set for pid in d.values())]
+
+    fixed_rt = {f: _pose_to_rt(map3d['poses'][f]) for f in [anchor] + ext_fixed}
+
+    # Separar observaciones: cámara variable vs cámara fija
+    vc_cam, vc_pt, vc_2d = [], [], []
+    fc_pose, fc_pt, fc_2d = [], [], []
+    for f in win_set | set(ext_fixed):
+        pts = pts_of(f)
+        for kp, pid in map3d['obs'][f].items():
+            if pid not in local_set:
+                continue
+            uv = pts[kp]
+            if f in var_pos:
+                vc_cam.append(var_pos[f]); vc_pt.append(pid_pos[pid]); vc_2d.append(uv)
+            else:
+                fc_pose.append(fixed_rt[f]); fc_pt.append(pid_pos[pid]); fc_2d.append(uv)
+
+    vc_cam = np.asarray(vc_cam, int); vc_pt = np.asarray(vc_pt, int)
+    vc_2d = np.asarray(vc_2d, float).reshape(-1, 2)
+    fc_pose = np.asarray(fc_pose, float).reshape(-1, 6)
+    fc_pt = np.asarray(fc_pt, int); fc_2d = np.asarray(fc_2d, float).reshape(-1, 2)
+    n_vc, n_fc = len(vc_2d), len(fc_2d)
+    if n_vc == 0:
+        return
+
+    cam0 = np.array([_pose_to_rt(map3d['poses'][f]) for f in var_frames])
+    pts0 = pts3d[local_pids].astype(float)
+    x0 = np.concatenate([cam0.ravel(), pts0.ravel()])
+
+    def resid(x):
+        cams = x[:n_var * 6].reshape(n_var, 6)
+        pts = x[n_var * 6:].reshape(n_pts, 3)
+        out = [(project(pts[vc_pt], cams[vc_cam], K) - vc_2d).ravel()]
+        if n_fc:
+            out.append((project(pts[fc_pt], fc_pose, K) - fc_2d).ravel())
+        return np.concatenate(out)
+
+    # Esparsidad: obs de cámara variable dependen de su cámara (6) + punto (3);
+    # obs de cámara fija dependen solo del punto (3).
+    m = (n_vc + n_fc) * 2
+    n = n_var * 6 + n_pts * 3
+    A = lil_matrix((m, n), dtype=int)
+    iv = np.arange(n_vc)
+    for s in range(6):
+        A[2 * iv, vc_cam * 6 + s] = 1
+        A[2 * iv + 1, vc_cam * 6 + s] = 1
+    for s in range(3):
+        A[2 * iv, n_var * 6 + vc_pt * 3 + s] = 1
+        A[2 * iv + 1, n_var * 6 + vc_pt * 3 + s] = 1
+    if n_fc:
+        jf = np.arange(n_fc) + n_vc
+        for s in range(3):
+            A[2 * jf, n_var * 6 + fc_pt * 3 + s] = 1
+            A[2 * jf + 1, n_var * 6 + fc_pt * 3 + s] = 1
+
+    res = least_squares(resid, x0, jac_sparsity=A, method='trf', loss='huber',
+                        f_scale=2.0, x_scale='jac', max_nfev=max_nfev, verbose=0)
+
+    cam_opt = res.x[:n_var * 6].reshape(n_var, 6)
+    pts_opt = res.x[n_var * 6:].reshape(n_pts, 3)
+    for f in var_frames:
+        map3d['poses'][f] = _rt_to_pose(cam_opt[var_pos[f]])
+    for p, i in pid_pos.items():
+        map3d['points'][p] = pts_opt[i]
+
+
+def fuse_map_points(map3d, voxel):
+    """Fusiona puntos 3D duplicados (mismo punto físico triangulado por separado).
+
+    Agrupa por vóxel y fusiona miembros cuyos conjuntos de frames observadores
+    son DISJUNTOS (tracks separados del mismo punto), sin que ningún frame quede
+    observando el punto fusionado dos veces. Redirige las observaciones al punto
+    superviviente y marca el duplicado como NaN (lo filtra el export).
+
+    Conservador a propósito: no fusiona puntos co-visibles (distintos puntos
+    cercanos en una pared densa), solo tracks separados.
+    """
+    from collections import defaultdict
+    pts = map3d['points']
+    finite = np.where(np.isfinite(pts).all(axis=1))[0]
+    if len(finite) == 0:
+        return 0
+
+    rev = defaultdict(list)  # pid -> [(frame, kp)]
+    for f, d in map3d['obs'].items():
+        for kp, pid in d.items():
+            rev[pid].append((f, kp))
+
+    keys = np.floor(pts[finite] / voxel).astype(np.int64)
+    groups = defaultdict(list)
+    for pid, key in zip(finite, map(tuple, keys)):
+        groups[key].append(pid)
+
+    fused = 0
+    for members in groups.values():
+        if len(members) < 2:
+            continue
+        keep = members[0]
+        keep_frames = {f for f, _ in rev[keep]}
+        coords = [pts[keep]]
+        for dup in members[1:]:
+            dup_frames = {f for f, _ in rev[dup]}
+            if keep_frames & dup_frames:      # comparten frame -> no fusionar
+                continue
+            for (f, kp) in rev[dup]:
+                map3d['obs'][f][kp] = keep
+            coords.append(pts[dup])
+            pts[dup] = np.nan
+            keep_frames |= dup_frames
+            fused += 1
+        map3d['points'][keep] = np.mean(coords, axis=0)
+    return fused
+
+
+def _build_problem(db, map3d):
+    """Empaqueta poses (rvec|tvec), puntos y observaciones (cam_idx, pt_idx, pts2d)."""
+    frames = sorted(map3d['poses'].keys())
+    fpos = {f: i for i, f in enumerate(frames)}
+    n_cam = len(frames)
+
+    cam_params = np.zeros((n_cam, 6), dtype=np.float64)
+    for f in frames:
+        P = map3d['poses'][f]
+        rvec, _ = cv2.Rodrigues(P[:3, :3])
+        cam_params[fpos[f], :3] = rvec.ravel()
+        cam_params[fpos[f], 3:] = P[:3, 3]
+
+    points = np.asarray(map3d['points'], dtype=np.float64).copy()
+
+    cam_idx, pt_idx, pts2d = [], [], []
+    for f in frames:
+        feats = db.features[f]
+        for kp, pid in map3d['obs'][f].items():
+            cam_idx.append(fpos[f]); pt_idx.append(pid); pts2d.append(feats.pts[kp])
+    cam_idx = np.asarray(cam_idx, dtype=int)
+    pt_idx = np.asarray(pt_idx, dtype=int)
+    pts2d = np.asarray(pts2d, dtype=np.float64)
+    return frames, fpos, cam_params, points, cam_idx, pt_idx, pts2d
+
+
+def _compact_finite_points(map3d):
+    """Elimina puntos NaN/inf (p.ej. duplicados marcados por fuse_map_points) y
+    re-indexa las observaciones. least_squares no tolera NaN en el vector x0."""
+    pts = np.asarray(map3d['points'])
+    keep = np.isfinite(pts).all(axis=1)
+    if keep.all():
+        return 0
+    old2new = -np.ones(len(pts), dtype=int)
+    old2new[keep] = np.arange(int(keep.sum()))
+    map3d['points'] = pts[keep]
+    map3d['colors'] = np.asarray(map3d['colors'])[keep]
+    for f in list(map3d['obs'].keys()):
+        map3d['obs'][f] = {kp: int(old2new[pid]) for kp, pid in map3d['obs'][f].items()
+                           if 0 <= pid < len(pts) and keep[pid]}
+    return int((~keep).sum())
+
+
+def run_bundle_adjustment(out_dir, max_reproj_err=4.0, ftol=1e-8, max_nfev=300):
+    db = SfMDatabase(); db.load(os.path.join(out_dir, DB_FILE))
+    K = db.get_camera().K  # intrínsecos persistidos en la DB
+    map_path = os.path.join(out_dir, MAP_STATE_FILE)
+    map3d = np.load(map_path, allow_pickle=True).item()
+
+    n_drop = _compact_finite_points(map3d)
+    if n_drop:
+        print(f"[BA] saneado: {n_drop} puntos NaN/inf eliminados antes de optimizar")
+
+    frames, fpos, cam_params, points, cam_idx, pt_idx, pts2d = _build_problem(db, map3d)
+    n_cam, n_pts = len(frames), len(points)
+
+    x0 = np.hstack([cam_params.ravel(), points.ravel()])
+    r0 = _residuals(x0, n_cam, n_pts, cam_idx, pt_idx, pts2d, K)
+    rmse0 = np.sqrt(np.mean(r0 ** 2))
+    print(f"[BA] cámaras={n_cam}  puntos={n_pts}  observaciones={len(pts2d)}")
+    print(f"[BA] RMSE reproyección inicial: {rmse0:7.3f} px")
+
+    A = _jac_sparsity(n_cam, n_pts, cam_idx, pt_idx)
+    res = least_squares(
+        _residuals, x0, jac_sparsity=A, x_scale='jac', method='trf',
+        loss='huber', f_scale=2.0, ftol=ftol, xtol=1e-10, gtol=1e-12,
+        max_nfev=max_nfev, verbose=2, args=(n_cam, n_pts, cam_idx, pt_idx, pts2d, K),
+    )
+
+    rf = _residuals(res.x, n_cam, n_pts, cam_idx, pt_idx, pts2d, K)
+    rmse1 = np.sqrt(np.mean(rf ** 2))
+    print(f"[BA] RMSE reproyección final:   {rmse1:7.3f} px  (mejora {rmse0 - rmse1:+.3f})")
+
+    cam_opt = res.x[:n_cam * 6].reshape(n_cam, 6)
+    pts_opt = res.x[n_cam * 6:].reshape(n_pts, 3)
+
+    # Poses optimizadas de vuelta a 4x4 world->cam
+    for f in frames:
+        rvec = cam_opt[fpos[f], :3]
+        tvec = cam_opt[fpos[f], 3:]
+        R, _ = cv2.Rodrigues(rvec)
+        map3d['poses'][f] = np.vstack((np.hstack((R, tvec.reshape(3, 1))), [0, 0, 0, 1]))
+
+    # Filtrar puntos con error de reproyección alto tras la optimización
+    per_obs = np.sqrt((rf.reshape(-1, 2) ** 2).sum(axis=1))
+    err_sum = np.zeros(n_pts); err_cnt = np.zeros(n_pts)
+    np.add.at(err_sum, pt_idx, per_obs)
+    np.add.at(err_cnt, pt_idx, 1)
+    mean_err = np.divide(err_sum, np.maximum(err_cnt, 1))
+    keep = (err_cnt > 0) & (mean_err <= max_reproj_err) & np.isfinite(pts_opt).all(axis=1)
+    print(f"[BA] puntos conservados: {int(keep.sum())}/{n_pts} "
+          f"(filtrados {n_pts - int(keep.sum())} con reproj > {max_reproj_err}px)")
+
+    # Re-indexar puntos y obs
+    old2new = -np.ones(n_pts, dtype=int)
+    old2new[keep] = np.arange(int(keep.sum()))
+    map3d['points'] = pts_opt[keep]
+    map3d['colors'] = np.asarray(map3d['colors'])[keep]
+    new_obs = {}
+    for f in frames:
+        d = {}
+        for kp, pid in map3d['obs'][f].items():
+            if 0 <= pid < n_pts and keep[pid]:
+                d[kp] = int(old2new[pid])
+        new_obs[f] = d
+    map3d['obs'] = new_obs
+
+    np.save(map_path, map3d)
+
+    # Re-exportar la nube centrada (mismo criterio que track_sfm)
+    pts = map3d['points']
+    valid = np.isfinite(pts).all(axis=1)
+    median_pt = np.median(pts[valid], axis=0) if valid.any() else np.zeros(3)
+    centered = pts[valid] - median_pt
+    cam_centers = [-(P[:3, :3].T @ P[:3, 3:]).ravel() - median_pt for P in map3d['poses'].values()]
+    export_ply(os.path.join(out_dir, TRACKED_CLOUD_FILE), centered, map3d['colors'][valid], cam_centers)
+    print(f"[BA] map_state.npy + {TRACKED_CLOUD_FILE} actualizados.")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Global Bundle Adjustment sobre el mapa SfM")
+    parser.add_argument("--out", required=True, help="Output dir con sfm_data.pkl y map_state.npy.")
+    parser.add_argument("--max-reproj", type=float, default=4.0,
+                        help="Descartar puntos con error medio de reproyección > este valor (px, default 4).")
+    parser.add_argument("--max-nfev", type=int, default=300,
+                        help="Máx. evaluaciones del optimizador (default 300).")
+    args = parser.parse_args()
+    run_bundle_adjustment(args.out, max_reproj_err=args.max_reproj, max_nfev=args.max_nfev)