diff --git a/extract_information_v1.py b/extract_information_v1.py new file mode 100644 index 00000000..b3b603d5 --- /dev/null +++ b/extract_information_v1.py @@ -0,0 +1,161 @@ +import os +import torch +import numpy as np +import pandas as pd +from tkinter import filedialog, Tk +import json +import matplotlib.pyplot as plt +from PIL import Image +import gc + +# Importaciones específicas de VGGT +from vggt.models.vggt import VGGT +from vggt.utils.load_fn import load_and_preprocess_images +from vggt.utils.pose_enc import pose_encoding_to_extri_intri + +# --- CONFIGURACIÓN --- +BATCH_SIZE = 10 # Ajustado para evitar OOM +# --------------------- + +def select_folder(prompt): + root = Tk() + root.withdraw() + return filedialog.askdirectory(title=prompt) + +def extract_camera_parameters(extrinsic, intrinsic): + """ + Descompone las matrices de VGGT en parámetros interpretables. + Convención: Extrínseca [R|t] transforma de Mundo -> Cámara. + """ + # 1. Intrínsecos + fx = intrinsic[0, 0] + fy = intrinsic[1, 1] + cx = intrinsic[0, 2] + cy = intrinsic[1, 2] + focal_length = (fx + fy) / 2.0 + + # 2. Extrínsecos + R_cw = extrinsic[:3, :3] # Rotación Mundo -> Cámara + t_cw = extrinsic[:3, 3] # Translación Mundo -> Cámara + + # Calcular Pose (Cámara -> Mundo) + # Posición de la cámara C = -R_cw^T * t_cw + R_wc = R_cw.T + camera_center = -np.dot(R_wc, t_cw) + + return { + "focal_length": float(focal_length), + "principal_point": [float(cx), float(cy)], + "intrinsic_matrix": intrinsic.tolist(), + "camera_position": camera_center.tolist(), + "rotation_matrix_wc": R_wc.tolist() # Guardamos la rotación de la pose + } + +def save_depth_map(depth_tensor, output_path): + depth = depth_tensor + 1e-6 + inverse_depth = 1.0 / depth + vmax = np.percentile(inverse_depth, 95) + vmin = np.percentile(inverse_depth, 5) + inverse_depth_normalized = (inverse_depth - vmin) / (vmax - vmin + 1e-8) + inverse_depth_normalized = np.clip(inverse_depth_normalized, 0, 1) + + cmap = plt.get_cmap("turbo") + color_depth = (cmap(inverse_depth_normalized)[..., :3] * 255).astype(np.uint8) + Image.fromarray(color_depth).save(output_path, format="JPEG", quality=85) + +def process_batch(model, batch_files, input_folder, output_folder, depth_out_dir, device, dtype): + image_paths = [os.path.join(input_folder, f) for f in batch_files] + + try: + images_tensor = load_and_preprocess_images(image_paths).to(device) + if images_tensor.ndim == 4: + images_tensor = images_tensor.unsqueeze(0) + except Exception as e: + print(f"Error cargando batch: {e}") + return [] + + with torch.no_grad(): + # Usando la sintaxis moderna de autocast para evitar warnings + with torch.amp.autocast('cuda', dtype=dtype): + predictions = model(images_tensor) + + pose_enc = predictions["pose_enc"] + img_size_hw = images_tensor.shape[-2:] + extrinsics, intrinsics = pose_encoding_to_extri_intri(pose_enc, img_size_hw) + + extrinsics = extrinsics.squeeze(0).cpu().numpy().astype(np.float64) # Mayor precisión + intrinsics = intrinsics.squeeze(0).cpu().numpy().astype(np.float64) + + depths_np = None + if "depth" in predictions: + depths_tensor = predictions["depth"] + depths_np = depths_tensor.squeeze(0).squeeze(-1).cpu().numpy() + + batch_records = [] + + for i, img_name in enumerate(batch_files): + params = extract_camera_parameters(extrinsics[i], intrinsics[i]) + + depth_filename = "" + if depths_np is not None: + depth_filename = f"depth_{os.path.splitext(img_name)[0]}.jpeg" + depth_path = os.path.join(depth_out_dir, depth_filename) + save_depth_map(depths_np[i], depth_path) + + record = { + "image_name": img_name, + "depth_map_file": depth_filename, + "f": params["focal_length"], + "cx": params["principal_point"][0], + "cy": params["principal_point"][1], + "tx": params["camera_position"][0], + "ty": params["camera_position"][1], + "tz": params["camera_position"][2], + "intrinsic_matrix": json.dumps(params["intrinsic_matrix"]), + "rotation_matrix_wc": json.dumps(params["rotation_matrix_wc"]) + } + batch_records.append(record) + + del images_tensor, predictions, pose_enc, extrinsics, intrinsics + if depths_np is not None: del depths_tensor # Corrección de variable + torch.cuda.empty_cache() + + return batch_records + +def process_images_vx(input_folder, output_folder): + device = "cuda" if torch.cuda.is_available() else "cpu" + dtype = torch.bfloat16 if torch.cuda.is_available() and torch.cuda.get_device_capability()[0] >= 8 else torch.float16 + + print(f"Cargando modelo en {device}...") + model = VGGT.from_pretrained("facebook/VGGT-1B").to(device) + model.eval() + + image_files = sorted([f for f in os.listdir(input_folder) if f.lower().endswith(('png', 'jpg', 'jpeg'))]) + total_images = len(image_files) + print(f"Encontradas {total_images} imágenes.") + + depth_out_dir = os.path.join(output_folder, "depth_maps") + os.makedirs(depth_out_dir, exist_ok=True) + + all_records = [] + + for i in range(0, total_images, BATCH_SIZE): + batch_files = image_files[i : i + BATCH_SIZE] + print(f"Procesando {i}/{total_images}...") + records = process_batch(model, batch_files, input_folder, output_folder, depth_out_dir, device, dtype) + all_records.extend(records) + gc.collect() + + df = pd.DataFrame(all_records) + csv_path = os.path.join(output_folder, "vggt_camera_data.csv") + df.to_csv(csv_path, index=False) + print(f"Hecho. CSV en: {csv_path}") + +if __name__ == "__main__": + print("Selecciona carpeta de entrada...") + in_dir = select_folder("Entrada") + if in_dir: + print("Selecciona carpeta de salida...") + out_dir = select_folder("Salida") + if out_dir: + process_images_vx(in_dir, out_dir) \ No newline at end of file diff --git a/extract_information_v2.py b/extract_information_v2.py new file mode 100644 index 00000000..1f4f907b --- /dev/null +++ b/extract_information_v2.py @@ -0,0 +1,185 @@ +import os +import torch +import numpy as np +import pandas as pd +from tkinter import filedialog, Tk +import sys + +# Importaciones de VGGT (asegúrate de ejecutar esto desde la raíz del proyecto vggt) +try: + from vggt.models.vggt import VGGT + from vggt.utils.load_fn import load_and_preprocess_images + from vggt.utils.pose_enc import pose_encoding_to_extri_intri +except ImportError: + print("Error: No se encuentran los módulos de VGGT. Asegúrate de ejecutar este script desde la carpeta raíz 'vggt-data-augmentation'.") + sys.exit(1) + +def select_folder(prompt): + """Abre una ventana para seleccionar carpeta""" + root = Tk() + root.withdraw() # Ocultar la ventana principal de Tkinter + root.attributes('-topmost', True) # Forzar la ventana al frente + folder_path = filedialog.askdirectory(title=prompt) + root.destroy() + return folder_path + +def extract_information_vx(): + # --- 1. Selección de Carpetas --- + print("Por favor, selecciona la carpeta con las IMÁGENES de entrada...") + image_folder = select_folder("Selecciona la carpeta con las IMÁGENES") + + if not image_folder: + print("No se seleccionó carpeta de entrada. Cancelando.") + return + + print("Por favor, selecciona la carpeta donde guardar el CSV de salida...") + output_folder = select_folder("Selecciona la carpeta de SALIDA (para guardar el CSV)") + + if not output_folder: + print("No se seleccionó carpeta de salida. Cancelando.") + return + + output_csv = os.path.join(output_folder, "camera_data_vx.csv") + + # --- 2. Configuración del Modelo --- + device = "cuda" if torch.cuda.is_available() else "cpu" + dtype = torch.bfloat16 if (torch.cuda.is_available() and torch.cuda.get_device_capability()[0] >= 8) else torch.float16 + print(f"Usando dispositivo: {device}") + + print("Cargando modelo VGGT...") + try: + model = VGGT.from_pretrained("facebook/VGGT-1B").to(device) + except Exception as e: + print(f"Nota: Carga automática falló ({e}), intentando carga manual...") + model = VGGT() + _URL = "https://huggingface.co/facebook/VGGT-1B/resolve/main/model.pt" + model.load_state_dict(torch.hub.load_state_dict_from_url(_URL)) + model = model.to(device) + model.eval() + + # --- 3. Procesamiento --- + valid_exts = ('.png', '.jpg', '.jpeg') + image_files = sorted([os.path.join(image_folder, f) for f in os.listdir(image_folder) + if f.lower().endswith(valid_exts)]) + + if not image_files: + print(f"Error: No se encontraron imágenes válidas en {image_folder}") + return + + print(f"Procesando {len(image_files)} imágenes...") + + # Cargar imágenes + images_tensor = load_and_preprocess_images(image_files).to(device) + + # Inferencia + with torch.no_grad(): + with torch.cuda.amp.autocast(dtype=dtype): + if images_tensor.ndim == 4: + images_input = images_tensor.unsqueeze(0) + else: + images_input = images_tensor + + predictions = model(images_input) + pose_enc = predictions["pose_enc"] + + # Descodificar poses + extrinsics, intrinsics = pose_encoding_to_extri_intri(pose_enc, images_tensor.shape[-2:]) + extrinsics = extrinsics.squeeze(0).float().cpu().numpy() + intrinsics = intrinsics.squeeze(0).float().cpu().numpy() + + # --- 4. Guardar Datos --- + data_records = [] + for i, img_path in enumerate(image_files): + K = intrinsics[i] + E = extrinsics[i] # [R|t] (Cámara <- Mundo) + + R = E[:3, :3] + t = E[:3, 3] + + # Cálculo de posición real en el mundo: C = -R^T * t + camera_center_world = -np.dot(R.T, t) + + # Rotación para visualización (Mundo <- Cámara) + # Esta es la orientación de la cámara en el mundo + R_wc = R.T + + # Calcular Ángulos de Euler (Yaw, Pitch, Roll) a partir de R_wc + # Asumimos convención XYZ o similar. Para cámaras suele ser útil Pitch, Yaw, Roll. + # Una implementación robusta de rotación a Euler (ZYX convention: Z=Yaw, Y=Pitch, X=Roll) + import math + sy = math.sqrt(R_wc[0, 0] * R_wc[0, 0] + R_wc[1, 0] * R_wc[1, 0]) + singular = sy < 1e-6 + if not singular: + x_rot = math.atan2(R_wc[2, 1], R_wc[2, 2]) + y_rot = math.atan2(-R_wc[2, 0], sy) + z_rot = math.atan2(R_wc[1, 0], R_wc[0, 0]) + else: + x_rot = math.atan2(-R_wc[1, 2], R_wc[1, 1]) + y_rot = math.atan2(-R_wc[2, 0], sy) + z_rot = 0 + + # Convertir a grados + roll = np.degrees(x_rot) + pitch = np.degrees(y_rot) + yaw = np.degrees(z_rot) + + # Altura relativa + # Para datos de UAV/Drones (VGGT suele alinear Z con la vista), + # si la cámara mira hacia abajo (Z+), la altura/altitud varía en el eje Z. + # Asumimos que moverse en -Z es subir. + height_rel = -camera_center_world[2] + + # --- 5. Guardar Mapa de Profundidad --- + depth_rel_path = "" + if "depth" in predictions: + # Obtener el mapa de profundidad para este índice + # depth_tensor shape: B, S, H, W, 1 + # predictions["depth"][0, i, :, :, 0] + d_map = predictions["depth"][0, i, :, :, 0].float().cpu().numpy() + + # Normalizar para visualización (Inverse Depth suele verse mejor) + d_map = d_map + 1e-6 + inv_depth = 1.0 / d_map + vmax = np.percentile(inv_depth, 95) + vmin = np.percentile(inv_depth, 5) + norm_depth = (inv_depth - vmin) / (vmax - vmin + 1e-8) + norm_depth = np.clip(norm_depth, 0, 1) + + # Colorear con mapa de calor (Turbo es bueno para profundidad) + import matplotlib.pyplot as plt + cmap = plt.get_cmap("turbo") + color_depth = (cmap(norm_depth)[..., :3] * 255).astype(np.uint8) + + # Guardar imagen + from PIL import Image + depth_filename = f"depth_{os.path.basename(img_path).split('.')[0]}.png" + depth_save_path = os.path.join(output_folder, "depth_maps", depth_filename) + os.makedirs(os.path.dirname(depth_save_path), exist_ok=True) + + Image.fromarray(color_depth).save(depth_save_path) + depth_rel_path = depth_filename + + data_records.append({ + "image_name": os.path.basename(img_path).split('.')[0], # Nombre sin extensión para referencia más limpia + "full_path": img_path, + "depth_map_path": depth_rel_path, + "focal_x": K[0, 0], + "focal_y": K[1, 1], + "principal_x": K[0, 2], + "principal_y": K[1, 2], + "pos_x": camera_center_world[0], + "pos_y": camera_center_world[1], + "pos_z": camera_center_world[2], + "height": height_rel, + "roll": roll, + "pitch": pitch, + "yaw": yaw, + "R_world_flat": R_wc.flatten().tolist() + }) + + df = pd.DataFrame(data_records) + df.to_csv(output_csv, index=False) + print(f"¡Éxito! Datos guardados en: {output_csv}") + +if __name__ == "__main__": + extract_information_vx() \ No newline at end of file diff --git a/requirements_da.txt b/requirements_da.txt new file mode 100644 index 00000000..4698bdec --- /dev/null +++ b/requirements_da.txt @@ -0,0 +1,12 @@ +# PyTorch with native CUDA 13.0 support +--index-url https://download.pytorch.org/whl/cu130 +torch +torchvision +torchaudio + +# Core dependencies - updated for Python 3.13 +numpy>=2.0.0 +Pillow +huggingface_hub +einops +safetensors \ No newline at end of file diff --git a/view_cluster_v1.py b/view_cluster_v1.py new file mode 100644 index 00000000..0b8485b0 --- /dev/null +++ b/view_cluster_v1.py @@ -0,0 +1,169 @@ +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import json +import cv2 +from tkinter import filedialog, Tk, simpledialog + +# --- CONFIGURACIÓN --- +DEFAULT_K = 100 +# --------------------- + +def select_file(prompt): + root = Tk() + root.withdraw() + file_path = filedialog.askopenfilename(title=prompt, filetypes=[("CSV Files", "*.csv")]) + root.destroy() + return file_path + +def get_k_input(total): + root = Tk() + root.withdraw() + val = min(DEFAULT_K, total) + k = simpledialog.askinteger("Configuración", f"Total imágenes: {total}\nClusters a mostrar:", + parent=root, minvalue=1, maxvalue=total, initialvalue=val) + root.destroy() + return k if k else val + +def convert_opencv_to_plot(points): + """ + Transforma coordenadas del sistema OpenCV (cámara) al sistema Plot/Mapa (gráfico). + OpenCV: X=Right, Y=Down, Z=Forward + Plot: X=Right, Y=Forward, Z=Up + + Mapeo: + Plot_X = CV_X + Plot_Y = CV_Z (Profundidad se vuelve 'Norte/Adelante' en el mapa) + Plot_Z = -CV_Y (Abajo se vuelve negativo de 'Arriba') + """ + # Si entra un solo punto (3,) convertir a (1,3) + if points.ndim == 1: + points = points.reshape(1, -1) + + x = points[:, 0] + y = points[:, 1] + z = points[:, 2] + + new_points = np.stack([x, z, -y], axis=1) + return new_points + +def create_camera_frustum(R, C, scale=0.5): + """ + 1. Genera el frustum en coordenadas locales de cámara (OpenCV). + 2. Transforma al mundo (OpenCV). + 3. (La conversión a Plot se hace después). + """ + w = scale + h = scale * 0.75 + z = scale * 1.5 # Profundidad del cono + + # Frustum local (0=Centro óptico) + # Z+ es hacia adelante en OpenCV + local_frustum = np.array([ + [0, 0, 0], + [-w, -h, z], [w, -h, z], + [w, h, z], [-w, h, z] + ]).T + + # Transformar al mundo usando R y C extraídos del CSV + # P_world = R_wc * P_local + C + world_frustum_cv = (R @ local_frustum).T + C + return world_frustum_cv + +def find_representative_cameras(df, k): + """Agrupa cámaras cercanas para limpiar la visualización.""" + positions = df[['tx', 'ty', 'tz']].values.astype(np.float32) + if len(positions) <= k: return df.index.tolist() + + print(f"Agrupando {len(positions)} cámaras en {k} representantes...") + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.2) + _, labels, centers = cv2.kmeans(positions, k, None, criteria, 10, cv2.KMEANS_PP_CENTERS) + + rep_indices = [] + for i in range(k): + idx = np.where(labels.flatten() == i)[0] + if len(idx) == 0: continue + # Elegir cámara real más cercana al centro del cluster + dists = np.linalg.norm(positions[idx] - centers[i], axis=1) + rep_indices.append(idx[np.argmin(dists)]) + return rep_indices + +def plot_cameras_vx(csv_path): + print(f"Cargando: {csv_path}") + try: + df = pd.read_csv(csv_path) + except Exception as e: + print(f"Error leyendo CSV: {e}") + return + + # Clustering + indices = find_representative_cameras(df, get_k_input(len(df))) + df_vis = df.iloc[indices].reset_index(drop=True) + + # Calcular escala de la escena para tamaño de conos + all_pos_cv = df[['tx', 'ty', 'tz']].values + scene_span = np.linalg.norm(all_pos_cv.max(0) - all_pos_cv.min(0)) + scale = scene_span * 0.05 if scene_span > 0 else 0.1 + + fig = plt.figure(figsize=(14, 10)) + ax = fig.add_subplot(111, projection='3d') + + positions_plot = [] + colors = plt.cm.jet(np.linspace(0, 1, len(df_vis))) + + print("Generando visualización (Transformando ejes CV -> Plot)...") + + for i, row in df_vis.iterrows(): + # 1. Recuperar datos (Sistema OpenCV) + C_cv = np.array([row['tx'], row['ty'], row['tz']]) + try: + R_cv = np.array(json.loads(row['rotation_matrix_wc'])) + except: continue + + # 2. Generar Geometría del Frustum (Sistema OpenCV) + frustum_cv = create_camera_frustum(R_cv, C_cv, scale=scale) + + # 3. CONVERSIÓN DE COORDENADAS (Crucial para corregir orientación) + # Transformamos tanto el centro como los vértices del frustum + C_plot = convert_opencv_to_plot(C_cv).flatten() + frustum_plot = convert_opencv_to_plot(frustum_cv) + + positions_plot.append(C_plot) + + # 4. Dibujar + verts = frustum_plot + # Caras laterales + sides = [[verts[0], verts[1], verts[2]], [verts[0], verts[2], verts[3]], + [verts[0], verts[3], verts[4]], [verts[0], verts[4], verts[1]]] + # Base (Plano de imagen) + base = [[verts[1], verts[2], verts[3], verts[4]]] + + ax.add_collection3d(Poly3DCollection(sides, facecolors=colors[i], alpha=0.15, edgecolors='k', linewidths=0.3)) + # La base más oscura indica hacia dónde mira la cámara + ax.add_collection3d(Poly3DCollection(base, facecolors=colors[i], alpha=0.5, edgecolors='k', linewidths=0.5)) + ax.scatter(C_plot[0], C_plot[1], C_plot[2], color=colors[i], s=15) + + # Ajuste de Ejes + p = np.array(positions_plot) + if len(p) > 0: + mid = (p.max(0) + p.min(0)) / 2 + rng = (p.max(0) - p.min(0)).max() / 2 + ax.set_xlim(mid[0]-rng, mid[0]+rng) + ax.set_ylim(mid[1]-rng, mid[1]+rng) + ax.set_zlim(mid[2]-rng, mid[2]+rng) + + ax.set_title('Visualización de Poses (Coordenadas Mapa: Z=Arriba)') + ax.set_xlabel('X (Lateral)') + ax.set_ylabel('Y (Adelante/Norte)') + ax.set_zlabel('Z (Altura)') + + # Vista inicial + ax.view_init(elev=30, azim=-45) + plt.tight_layout() + plt.show() + +if __name__ == "__main__": + f = select_file("Selecciona vggt_camera_data.csv") + if f: plot_cameras_vx(f) \ No newline at end of file diff --git a/view_cluster_v2.py b/view_cluster_v2.py new file mode 100644 index 00000000..e52c0af7 --- /dev/null +++ b/view_cluster_v2.py @@ -0,0 +1,220 @@ +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import ast +import cv2 +from tkinter import filedialog, Tk, simpledialog +import sys +import os + +def select_file(prompt): + """Abre una ventana para seleccionar el archivo CSV""" + root = Tk() + root.withdraw() + root.attributes('-topmost', True) + file_path = filedialog.askopenfilename( + title=prompt, + filetypes=[("CSV files", "*.csv"), ("All files", "*.*")] + ) + root.destroy() + return file_path + +def ask_cluster_number(): + """Pide al usuario el número de clusters mediante una ventana""" + root = Tk() + root.withdraw() + root.attributes('-topmost', True) + num = simpledialog.askinteger( + "Configuración de Clusters", + "Introduce el número de clusters para agrupar las cámaras:", + parent=root, + minvalue=1, + maxvalue=50, + initialvalue=5 + ) + root.destroy() + return num + +def convert_opencv_to_plot(points): + """ + Transforma coordenadas del sistema OpenCV (cámara) al sistema Plot/Mapa (gráfico). + + MODO UAV/DRONE (Nadir): + Asumimos que el sistema de coordenadas del mundo (Frame 1) tiene: + - Z+ apuntando hacia el suelo (Vista de la cámara). + - Y+ apuntando hacia "Abajo" de la imagen (Sur/Atrás). + - X+ apuntando a la Derecha. + + Queremos transformar a un sistema de mapa (NED/ENU modificado): + - Plot Z+ = Altura (Hacia arriba). + - Plot Y+ = Norte/Adelante. + - Plot X+ = Derecha. + + Mapeo: + Plot_X = CV_X + Plot_Y = -CV_Y (CV Y es Back -> Plot Y es Forward) + Plot_Z = -CV_Z (CV Z es Down -> Plot Z es Up) + """ + if points.ndim == 1: + x, y, z = points + return np.array([x, -y, -z]) + + x = points[:, 0] + y = points[:, 1] + z = points[:, 2] + + new_points = np.stack([x, -y, -z], axis=1) + return new_points + +def plot_camera(ax, position, rotation_matrix, scale=0.5, color='blue', label=None): + """ + Dibuja la cámara: Pirámide (Frustum) + Flecha de dirección (Vector) + Aplica conversión de coordenadas de OpenCV (Y-down) a Plot (Z-up). + """ + # 1. DIBUJAR FRUSTUM (PIRÁMIDE) + w = scale + h = scale * 0.75 + z = scale * 0.8 + + # Vértices locales (Punta en 0,0,0) + # Z+ es hacia adelante en el frame de la cámara + local_verts = np.array([ + [0, 0, 0], # Ojo + [w, h, z], [-w, h, z], [-w, -h, z], [w, -h, z] # Base + ]).T + + # Transformar al mundo (OpenCV): R * P_local + C + world_verts = np.dot(rotation_matrix, local_verts) + position.reshape(3, 1) + + # CONVERTIR AL SISTEMA DEL GRÁFICO (Z-Up) + plot_verts = convert_opencv_to_plot(world_verts.T).T + plot_pos = convert_opencv_to_plot(position) + + # Caras de la pirámide + # Los índices son sobre las columnas de plot_verts + verts_indices = [ + [0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1], # Lados + [1, 2, 3, 4] # Tapa trasera + ] + poly_3d = [[tuple(plot_verts[:, i]) for i in indices] for indices in verts_indices] + + # Agregar la pirámide semitransparente + ax.add_collection3d(Poly3DCollection(poly_3d, facecolors=color, linewidths=0.5, edgecolors='k', alpha=0.3)) + + # 2. DIBUJAR VECTOR DE DIRECCIÓN (FLECHA) + # El eje Z local es [0, 0, 1]. + # Vector dirección en Mundo CV = R * [0,0,1] + direction_cv = rotation_matrix @ np.array([0, 0, 1]) + + # Convertir vector a Plot usando la función helper + direction_plot = convert_opencv_to_plot(direction_cv) + + # Dibujar flecha (quiver) + arrow_len = scale * 1.5 + ax.quiver( + plot_pos[0], plot_pos[1], plot_pos[2], # Origen + direction_plot[0], direction_plot[1], direction_plot[2], # Vector + length=arrow_len, color=color, linewidth=1.5, arrow_length_ratio=0.3 + ) + + # 3. DIBUJAR CENTRO Y ETIQUETA + ax.scatter(plot_pos[0], plot_pos[1], plot_pos[2], color='black', s=10) + if label: + ax.text(plot_pos[0], plot_pos[1], plot_pos[2], label, fontsize=8) + +def view_cluster_vx(): + # --- 1. Selección de Archivo --- + print("Abriendo ventana para seleccionar archivo CSV...") + csv_path = select_file("Selecciona el archivo 'camera_data_vx.csv'") + + if not csv_path: + print("Cancelado por el usuario.") + return + + # --- 2. Selección de Clusters --- + num_clusters = ask_cluster_number() + if num_clusters is None: + print("No se introdujo número. Usando por defecto: 5") + num_clusters = 5 + + # Cargar datos + print(f"Cargando datos de {os.path.basename(csv_path)}...") + df = pd.read_csv(csv_path) + positions = df[['pos_x', 'pos_y', 'pos_z']].values.astype(np.float32) + + # Parsear rotaciones + rotations = [] + for r_str in df['R_world_flat']: + r_list = ast.literal_eval(r_str) if isinstance(r_str, str) else r_str + rotations.append(np.array(r_list).reshape(3, 3)) + + # --- 3. Agrupamiento (Clustering) --- + if len(positions) < num_clusters: + num_clusters = len(positions) + + # Usamos KMeans para agrupar por POSICIÓN espacial + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.2) + _, labels, centers = cv2.kmeans(positions, num_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS) + labels = labels.flatten() + + # --- 4. Visualización --- + fig = plt.figure(figsize=(14, 10)) + ax = fig.add_subplot(111, projection='3d') + + # Mapa de colores distinto para cada cluster + cmap = plt.get_cmap("tab20") + + # Calcular escala visual basada en el tamaño de la escena + scene_span = np.max(np.ptp(positions, axis=0)) + # Ajustamos la escala para que las cámaras no se vean ni muy grandes ni muy chicas + cam_scale = scene_span * 0.08 if scene_span > 0 else 0.1 + + print(f"Graficando {num_clusters} grupos representativos (sin cámaras individuales)...") + + # Iterar por cada cluster para dibujar SOLO el representante + for k in range(num_clusters): + # Encontrar cámaras que pertenecen a este cluster + cluster_indices = np.where(labels == k)[0] + if len(cluster_indices) == 0: + continue + + # Tomar la rotación de la cámara más cercana al centro del cluster + # Esto asegura que la orientación del cono sea realista (la del dron en esa zona) + cluster_center = centers[k] + dists = np.linalg.norm(positions[cluster_indices] - cluster_center, axis=1) + closest_idx = cluster_indices[np.argmin(dists)] + rep_rotation = rotations[closest_idx] + + # Asignar color + color = cmap(k / max(num_clusters, 1)) + + # Graficar cono representativo en el centro del cluster + # Usamos una escala un poco mayor para destacar que es un grupo + plot_camera(ax, cluster_center, rep_rotation, scale=cam_scale*2.0, color=color, label=f"Cluster {k}") + + # (Las marcas 'X' de los centros se han eliminado implícitamente al no incluirlas) + + # Etiquetas y Título + ax.set_xlabel('X (Lateral)') + ax.set_ylabel('Y (Profundidad)') + ax.set_zlabel('Z (Altura)') + ax.set_title(f'Visualización de Cámaras: {num_clusters} Clusters\n(Las líneas indican la dirección de enfoque)') + + # --- 5. Ajuste de Aspect Ratio (Crucial para ver bien la dirección) --- + # Matplotlib 3D por defecto deforma los ejes. Esto fuerza una escala 1:1:1 + limits = np.array([ax.get_xlim3d(), ax.get_ylim3d(), ax.get_zlim3d()]) + origin = np.mean(limits, axis=1) + radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) + + ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) + ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) + ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) + + plt.legend() + print("Mostrando gráfico...") + plt.show() + +if __name__ == "__main__": + view_cluster_vx() \ No newline at end of file diff --git a/view_cluster_v3.py b/view_cluster_v3.py new file mode 100644 index 00000000..38b8425e --- /dev/null +++ b/view_cluster_v3.py @@ -0,0 +1,257 @@ +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import ast +import cv2 +from tkinter import filedialog, Tk, simpledialog +import sys +import os + +# --- UTILIDADES --- + +def select_file(prompt): + """Abre una ventana para seleccionar el archivo CSV""" + root = Tk() + root.withdraw() + root.attributes('-topmost', True) + file_path = filedialog.askopenfilename( + title=prompt, + filetypes=[("CSV files", "*.csv"), ("All files", "*.*")] + ) + root.destroy() + return file_path + +def ask_cluster_number(): + """Pide al usuario el número de clusters mediante una ventana""" + root = Tk() + root.withdraw() + root.attributes('-topmost', True) + num = simpledialog.askinteger( + "Configuración de Clusters", + "Introduce el número de clusters para agrupar las cámaras:", + parent=root, + minvalue=1, + maxvalue=50, + initialvalue=5 + ) + root.destroy() + return num + +def convert_opencv_to_plot(points): + """ + Transforma coordenadas del sistema OpenCV (cámara) al sistema Plot/Mapa (gráfico). + + MODO UAV/DRONE (Nadir): + Asumimos que el sistema de coordenadas del mundo (Frame 1) tiene: + - Z+ apuntando hacia el suelo (Vista de la cámara). + - Y+ apuntando hacia "Abajo" de la imagen (Sur/Atrás). + - X+ apuntando a la Derecha. + + Queremos transformar a un sistema de mapa (NED/ENU modificado): + - Plot Z+ = Altura (Hacia arriba). + - Plot Y+ = Norte/Adelante. + - Plot X+ = Derecha. + + Mapeo: + Plot_X = CV_X + Plot_Y = -CV_Y (CV Y es Back -> Plot Y es Forward) + Plot_Z = -CV_Z (CV Z es Down -> Plot Z es Up) + """ + if points.ndim == 1: + x, y, z = points + return np.array([x, -y, -z]) + + x = points[:, 0] + y = points[:, 1] + z = points[:, 2] + + new_points = np.stack([x, -y, -z], axis=1) + return new_points + +def plot_camera(ax, position, rotation_matrix, scale=0.5, color='blue', label=None): + """ + Dibuja la cámara: Pirámide (Frustum) + Flecha de dirección (Vector) + Aplica conversión de coordenadas de OpenCV (Y-down) a Plot (Z-up). + """ + # 1. DIBUJAR FRUSTUM (PIRÁMIDE) + w = scale + h = scale * 0.75 + z = scale * 0.8 + + # Vértices locales (Punta en 0,0,0) + # Z+ es hacia adelante en el frame de la cámara + local_verts = np.array([ + [0, 0, 0], # Ojo + [w, h, z], [-w, h, z], [-w, -h, z], [w, -h, z] # Base + ]).T + + # Transformar al mundo (OpenCV): R * P_local + C + world_verts = np.dot(rotation_matrix, local_verts) + position.reshape(3, 1) + + # CONVERTIR AL SISTEMA DEL GRÁFICO (Z-Up) + plot_verts = convert_opencv_to_plot(world_verts.T).T + plot_pos = convert_opencv_to_plot(position) + + # Caras de la pirámide + # Los índices son sobre las columnas de plot_verts + verts_indices = [ + [0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 4, 1], # Lados + [1, 2, 3, 4] # Tapa trasera + ] + poly_3d = [[tuple(plot_verts[:, i]) for i in indices] for indices in verts_indices] + + # Agregar la pirámide semitransparente + ax.add_collection3d(Poly3DCollection(poly_3d, facecolors=color, linewidths=0.5, edgecolors='k', alpha=0.3)) + + # 2. DIBUJAR VECTOR DE DIRECCIÓN (FLECHA) + # El eje Z local es [0, 0, 1]. + # Vector dirección en Mundo CV = R * [0,0,1] + direction_cv = rotation_matrix @ np.array([0, 0, 1]) + + # Convertir vector a Plot usando la función helper + direction_plot = convert_opencv_to_plot(direction_cv) + + # Dibujar flecha (quiver) + arrow_len = scale * 1.5 + ax.quiver( + plot_pos[0], plot_pos[1], plot_pos[2], # Origen + direction_plot[0], direction_plot[1], direction_plot[2], # Vector + length=arrow_len, color=color, linewidth=1.5, arrow_length_ratio=0.3 + ) + + # 3. DIBUJAR CENTRO Y ETIQUETA + ax.scatter(plot_pos[0], plot_pos[1], plot_pos[2], color='black', s=10) + if label: + ax.text(plot_pos[0], plot_pos[1], plot_pos[2], label, fontsize=8) + +# --- VISUALIZACIÓN COMPLETA (V3) --- + +def view_analysis_v3(): + # --- 1. Selección de Archivo --- + print("Abriendo ventana para seleccionar archivo CSV...") + csv_path = select_file("Selecciona el archivo 'camera_data_vx.csv'") + + if not csv_path: + print("Cancelado por el usuario.") + return + + # --- 2. Selección de Clusters --- + num_clusters = ask_cluster_number() + if num_clusters is None: + print("No se introdujo número. Usando por defecto: 5") + num_clusters = 5 + + # Cargar datos + print(f"Cargando datos de {os.path.basename(csv_path)}...") + df = pd.read_csv(csv_path) + + # Extraer posiciones (OpenCV World Coords) + positions = df[['pos_x', 'pos_y', 'pos_z']].values.astype(np.float32) + + # Extraer alturas (si existen, sino calcular desde Z) + if 'height' in df.columns: + heights = df['height'].values + else: + # Fallback para CSVs antiguos: altura = -Z (asumiendo nadir) + heights = -positions[:, 2] + + # Parsear rotaciones + rotations = [] + for r_str in df['R_world_flat']: + r_list = ast.literal_eval(r_str) if isinstance(r_str, str) else r_str + rotations.append(np.array(r_list).reshape(3, 3)) + + # --- 3. Agrupamiento (Clustering) --- + if len(positions) < num_clusters: + num_clusters = len(positions) + + # Usamos KMeans para agrupar por POSICIÓN espacial + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.2) + _, labels, centers = cv2.kmeans(positions, num_clusters, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS) + labels = labels.flatten() + + # --- 4. Configuración de Gráficos (2 Subplots) --- + fig = plt.figure(figsize=(14, 6)) + fig.suptitle(f"Análisis de información de imágenes extraidas por VGGT", fontsize=16) + + # Mapa de colores + cmap = plt.get_cmap("tab20") + + # ------------------------------------------------------------------------- + # GRÁFICO 1: VISUALIZACIÓN 3D DE CLUSTERS (REPRESENTATIVOS) + # ------------------------------------------------------------------------- + ax1 = fig.add_subplot(121, projection='3d') + ax1.set_title("1. Clusters 3D (Representantes)") + + # Escala visual + scene_span = np.max(np.ptp(positions, axis=0)) + cam_scale = scene_span * 0.08 if scene_span > 0 else 0.1 + + print(f"Graficando {num_clusters} grupos representativos...") + + # Iterar por cada cluster para dibujar SOLO el representante + colors_per_cluster = [cmap(k / max(num_clusters, 1)) for k in range(num_clusters)] + + for k in range(num_clusters): + cluster_indices = np.where(labels == k)[0] + if len(cluster_indices) == 0: continue + + cluster_center = centers[k] + dists = np.linalg.norm(positions[cluster_indices] - cluster_center, axis=1) + closest_idx = cluster_indices[np.argmin(dists)] + rep_rotation = rotations[closest_idx] + + plot_camera(ax1, cluster_center, rep_rotation, scale=cam_scale*2.0, color=colors_per_cluster[k], label=f"C{k}") + + # Etiquetas Ejes 3D + ax1.set_xlabel('X (Lateral)') + ax1.set_ylabel('Y (Profundidad)') + ax1.set_zlabel('Z (Altura relativa)') + + # Ajuste de Aspect Ratio 3D + limits = np.array([ax1.get_xlim3d(), ax1.get_ylim3d(), ax1.get_zlim3d()]) + origin = np.mean(limits, axis=1) + radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) + ax1.set_xlim3d([origin[0] - radius, origin[0] + radius]) + ax1.set_ylim3d([origin[1] - radius, origin[1] + radius]) + ax1.set_zlim3d([origin[2] - radius, origin[2] + radius]) + + # ------------------------------------------------------------------------- + # GRÁFICO 2: ANÁLISIS DE ALTURA RELATIVA + # ------------------------------------------------------------------------- + ax2 = fig.add_subplot(122) + ax2.set_title("2. Análisis de altura de vuelo relativa (Z)") + + # Estadísticas + mean_h = np.mean(heights) + std_h = np.std(heights) + + # Scatter de alturas por imagen (eje X = índice imagen) + # Coloreamos por cluster para correlacionar + x_indices = np.arange(len(heights)) + point_colors = [colors_per_cluster[l] for l in labels] + + ax2.scatter(x_indices, heights, c=point_colors, alpha=0.7, label='Imágenes') + + # Líneas de referencia + ax2.axhline(mean_h, color='green', linestyle='-', linewidth=2, label=f'Promedio: {mean_h:.2f}m') + ax2.axhline(mean_h + std_h, color='red', linestyle='--', alpha=0.5, label=f'+1 STD: {mean_h+std_h:.2f}m') + ax2.axhline(mean_h - std_h, color='red', linestyle='--', alpha=0.5, label=f'-1 STD: {mean_h-std_h:.2f}m') + + # Relleno de desviación estándar + ax2.fill_between(x_indices, mean_h - std_h, mean_h + std_h, color='gray', alpha=0.1) + + ax2.set_xlabel("Índice de Imagen") + ax2.set_ylabel("Altura relativa (m)") + ax2.legend(loc='best', fontsize='small') + ax2.grid(True, linestyle=':', alpha=0.6) + + # Finalizar + plt.tight_layout() + print("Mostrando dashboard de análisis...") + plt.show() + +if __name__ == "__main__": + view_analysis_v3()