imicpe 1.0.7__tar.gz → 1.0.8.1__tar.gz

{imicpe-1.0.7 → imicpe-1.0.8.1}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: imicpe
-Version: 1.0.7
+Version: 1.0.8.1
 Summary: Toolbox for Maths,Signal,Image Teaching @ CPE
 Author-email: Marion Foare <marion.foare@cpe.fr>, Eric Van Reeth <eric.vanreeth@cpe.fr>, Arthur Gautheron <arthur.gautheron@cpe.fr>
 License: MIT License

{imicpe-1.0.7 → imicpe-1.0.8.1}/pyproject.toml

@@ -14,7 +14,7 @@ mypkg = ["*.txt", "*.mat", "*.npy"]
 #[tool.setuptools_scm]
 
 [project]
-version="1.0.7"
+version="1.0.8.1"
 name = "imicpe"
 authors = [
   { name="Marion Foare", email="marion.foare@cpe.fr" },

imicpe-1.0.8.1/src/imicpe/_version.py

@@ -0,0 +1 @@
+__version__="1.0.8.1"

imicpe-1.0.8.1/src/imicpe/reco3D/__init__.py

@@ -0,0 +1,4 @@
+from .vue3D import vue3D
+from .angles import ang2rot, rot2ang
+from .evalTrajecto import evalTrajecto
+from .calculPose import calculePose3D2D

imicpe-1.0.8.1/src/imicpe/reco3D/angles.py

@@ -0,0 +1,117 @@
+import numpy as np
+
+def ang2rot(ang):
+    """
+    
+    Calcule la matrice de rotation 3x3 à partir de 3 angles (Euler Z–X–Y)
+    (ONERA/DTIM)
+    
+    Parameters
+    ----------
+    ang : array_like of shape (3,)
+        Angles des rotations autour des axes z, x, y (en radians)
+    
+    Returns
+    -------
+    R : ndarray of shape (3,3)
+        Matrice de rotation correspondante
+    """
+
+    ang = np.array(ang, dtype=float).flatten()
+    if ang.size != 3:
+        raise ValueError("ang doit être un vecteur de 3 éléments (z, x, y)")
+
+    cz, sz = np.cos(ang[0]), np.sin(ang[0])
+    cx, sx = np.cos(ang[1]), np.sin(ang[1])
+    cy, sy = np.cos(ang[2]), np.sin(ang[2])
+
+    # === Codage Euler z-x-y ===
+    Rz = np.array([
+        [cz,  sz,  0],
+        [-sz, cz,  0],
+        [0,   0,   1]
+    ])
+
+    Rx = np.array([
+        [1,  0,   0],
+        [0,  cx,  sx],
+        [0, -sx,  cx]
+    ])
+
+    Ry = np.array([
+        [cy,  0, -sy],
+        [0,   1,  0],
+        [sy,  0,  cy]
+    ])
+
+    R = Rz @ Rx @ Ry
+    return R
+
+def rot2ang(matR):
+    """
+    Conversion d'une matrice de rotation 3x3 en angles d'Euler (z, x, y)
+    Codage : Euler Z–X–Y
+    Retourne : ang (3x2) — deux solutions possibles
+    (ONERA/DTIM)
+    
+    Parameters
+    ----------
+    R : ndarray (3,3)
+        Matrice de rotation correspondante
+
+    Returns
+    -------
+    ang : array_like (3,2)
+        Angles des rotations autour des axes z, x, y (en radians)
+    """
+
+    # Sécurité : s'assurer que matR est un array numpy 3x3
+    matR = np.array(matR, dtype=float).reshape(3, 3)
+
+    # Cas général
+    if abs(matR[2, 1]) != 1:
+        ang = np.zeros((3, 2))  # (3x2) comme MATLAB après transposition finale
+
+        # rotation angle around x axis
+        ang[1, 0] = np.arcsin(-matR[2, 1])
+        ang[1, 1] = np.sign(ang[1, 0]) * np.pi - ang[1, 0]
+
+        # rotation angle around y axis (colonne 3 MATLAB → ligne 2 ici avant transpose)
+        ang[2, 0] = np.arctan2(matR[2, 0] / np.cos(ang[1, 0]),
+                            matR[2, 2] / np.cos(ang[1, 0]))
+        ang[2, 1] = np.arctan2(matR[2, 0] / np.cos(ang[1, 1]),
+                            matR[2, 2] / np.cos(ang[1, 1]))
+
+        # rotation angle around z axis (colonne 1 MATLAB → ligne 0 ici avant transpose)
+        ang[0, 0] = np.arctan2(matR[0, 1] / np.cos(ang[1, 0]),
+                            matR[1, 1] / np.cos(ang[1, 0]))
+        ang[0, 1] = np.arctan2(matR[0, 1] / np.cos(ang[1, 1]),
+                            matR[1, 1] / np.cos(ang[1, 1]))
+
+        return ang  # déjà (3x2) comme après transpose MATLAB
+
+    # Cas dégénéré : matR[3,2] = ±1 → gimbal lock
+    else:
+        ang = np.zeros(3)
+        # variable "roll" inexistante en MATLAB original → non utilisée ici
+        # on garde simplement les 3 angles comme définis par le cas particulier
+
+        if matR[2, 1] == 1:
+            ang[1] = -np.pi / 2
+            ang[2] = np.arctan2(-matR[0, 2], -matR[1, 2]) - ang[0]
+            if ang[2] > np.pi:
+                ang[2] -= 2 * np.pi
+            elif ang[2] < -np.pi:
+                ang[2] += 2 * np.pi
+
+        else:  # matR[2,1] == -1
+            ang[1] = np.pi / 2
+            ang[2] = ang[0] - np.arctan2(matR[0, 2], matR[1, 2])
+            if ang[2] > np.pi:
+                ang[2] -= 2 * np.pi
+            elif ang[2] < -np.pi:
+                ang[2] += 2 * np.pi
+
+        # Le MATLAB retourne ang = ang'
+        return ang.reshape(3, 1)
+  

imicpe-1.0.8.1/src/imicpe/reco3D/calculPose.py

@@ -0,0 +1,284 @@
+from .angles import ang2rot, rot2ang
+import numpy as np
+from scipy.linalg import svd
+from scipy.optimize import least_squares
+# ===============================================
+# === Fonction principale : affinageRT ==========
+# ===============================================
+
+def affinageRT(anginit, Tinit, matK, pos3D, pos2D):
+    """
+    Affine rotation et translation par minimisation de l'erreur de reprojection
+    Entrées :
+      anginit : angles initiaux (3x1)
+      Tinit : translation initiale (3x1)
+      matK : matrice intrinsèque 3x3
+      pos3D : points 3D (Nx3)
+      pos2D : points 2D (Nx2)
+    Sortie :
+      angfin : angles affinés (3x1)
+      Tfin : translation affinée (3x1)
+      rmsInit : RMS initial
+      rmsFin : RMS final
+    """
+
+    # ===============================================
+    # === Fonctions de rotation =====================
+    # ===============================================
+
+    # def ang2rot(ang):
+    #     """Conversion angles (3x1) → matrice de rotation 3x3 (ordre Rx*Ry*Rz typique MATLAB)"""
+    #     cx, cy, cz = np.cos(ang)
+    #     sx, sy, sz = np.sin(ang)
+
+    #     Rx = np.array([[1, 0, 0],
+    #                 [0, cx, -sx],
+    #                 [0, sx,  cx]])
+    #     Ry = np.array([[ cy, 0, sy],
+    #                 [  0, 1,  0],
+    #                 [-sy, 0, cy]])
+    #     Rz = np.array([[cz, -sz, 0],
+    #                 [sz,  cz, 0],
+    #                 [ 0,   0, 1]])
+
+    #     # MATLAB fait généralement R = Rz * Ry * Rx
+    #     R = Rz @ Ry @ Rx
+    #     return R
+
+
+    # ===============================================
+    # === Fonction de projection ====================
+    # ===============================================
+
+    def fct_proj_ck(d, X, K):
+        """
+        Projection 3D → 2D avec paramètres caméra.
+        Équivalent MATLAB de fct_proj_ck
+        Entrées :
+        d : vecteur [T; ang] de taille (6,)
+        X : points 3D (3xN)
+        K : matrice intrinsèque (3x3)
+        Sortie :
+        x : projections 2D (2xN)
+        """
+        alx = K[0, 0]
+        aly = K[1, 1]
+        x0 = K[0, 2]
+        y0 = K[1, 2]
+
+        R = ang2rot(d[3:6])
+        T = d[0:3].reshape(3, 1)
+
+        # Projection
+        D = R @ (X - T)  # même que R*(X - repmat(T,1,N)) en MATLAB
+        x1 = alx * D[0, :] / D[2, :] + x0
+        y1 = aly * D[1, :] / D[2, :] + y0
+
+        x = np.vstack((x1, y1))
+        return x
+
+
+    # ===============================================
+    # === Fonction d'évaluation =====================
+    # ===============================================
+
+    def eval_fct_proj_ck(d, xref, X, K):
+        """
+        Évalue la fonction de reprojection et retourne les résidus
+        Entrées :
+        d : [T; ang] (6,)
+        xref : 2xN (positions 2D de référence)
+        X : 3xN (points 3D)
+        K : matrice intrinsèque
+        Sortie :
+        res : vecteur des résidus (2N,)
+        x : projections (2xN)
+        """
+        x = fct_proj_ck(d, X, K)
+        res = x - xref
+        res = res.flatten(order='F')  # même ordre colonne que MATLAB
+        return res, x
+
+    nbPts = pos3D.shape[0]
+    if nbPts != pos2D.shape[0]:
+        print("Erreur : dimensions incompatibles entre pos3D et pos2D")
+        return None, None, None, None
+
+    # --- Calcul du résidu initial ---
+    Rinit = ang2rot(anginit.flatten())
+    Mproj3 = Rinit @ pos3D.T - Rinit @ Tinit.reshape(3, 1).repeat(nbPts, axis=1)
+    aux = matK @ Mproj3
+    mproj3 = aux[:2, :] / aux[2, :]
+    mproj3 = mproj3.T
+
+    residu3 = (mproj3.flatten() - pos2D.flatten())
+    rmsInit = np.sqrt(np.mean(residu3 ** 2))
+
+    # --- Optimisation non linéaire ---
+    dinit = np.concatenate((Tinit.flatten(), anginit.flatten()))
+
+    def fun(d):
+        res, _ = eval_fct_proj_ck(d, pos2D.T, pos3D.T, matK)
+        return res
+
+    res_lsq = least_squares(fun, dinit, method='lm', verbose=0)
+    dfin = res_lsq.x
+
+    # --- Résidu final ---
+    resfin, _ = eval_fct_proj_ck(dfin, pos2D.T, pos3D.T, matK)
+    rmsFin = np.sqrt(np.mean(resfin ** 2))
+
+    Tfin = dfin[0:3].reshape(3, 1)
+    angfin = dfin[3:6].reshape(3, 1)
+
+    # --- Résidu image final (comme MATLAB) ---
+    Rfin = ang2rot(angfin.flatten())
+    Mproj4 = Rfin @ pos3D.T - Rfin @ Tfin.repeat(nbPts, axis=1)
+    aux = matK @ Mproj4
+    mproj4 = aux[:2, :] / aux[2, :]
+    mproj4 = mproj4.T
+
+    residu4 = (mproj4.flatten() - pos2D.flatten())
+    rmsFin = np.sqrt(np.mean(residu4 ** 2))
+    return angfin.squeeze(), Tfin.squeeze(), rmsInit, rmsFin
+
+
+def calculePose3D2D(pts3D, pts2D, matK, nbPts, AFFINAGE=False):    
+    """
+    Calcule la pose d’une caméra (rotation et translation) à partir de correspondances 3D-2D.
+
+    Utilise la méthode DLT (Direct Linear Transform) avec une résolution par SVD pour estimer
+    la matrice de projection, puis extrait la rotation et la translation. Un affinage optionnel
+    permet d'améliorer la précision via une minimisation de l'erreur de reprojection.
+
+    Parameters
+    ----------
+    pts3D : ndarray of shape (nbPts, 3)
+        Coordonnées des points 3D dans le repère monde.
+
+    pts2D : ndarray of shape (nbPts, 2)
+        Coordonnées des points 2D correspondants dans l'image (en pixels).
+
+    matK : ndarray of shape (3, 3)
+        Matrice de calibration intrinsèque de la caméra.
+
+    nbPts : int
+        Nombre de points de correspondance 3D-2D utilisés pour le calcul.
+
+    AFFINAGE : bool, optional
+        Si True, active un affinage de la pose par minimisation de l'erreur de reprojection.
+        Par défaut à False.
+
+    Returns
+    -------
+    ang : ndarray of shape (3,)
+        Angles d'Euler (rx, ry, rz) en radians, représentant la rotation de la caméra.
+
+    T : ndarray of shape (3,)
+        Vecteur de translation de la caméra dans le repère monde.
+
+    rms_reproj : float
+        Erreur RMS (Root Mean Square) de reprojection entre les points 2D projetés et les
+        observations réelles.
+
+    Notes
+    -----
+    - La rotation est estimée à partir de la matrice de projection, et corrigée
+      pour s'assurer qu'elle appartient au groupe SO(3).
+    - Le vecteur d'angles `ang` est sélectionné comme la solution avec la norme minimale.
+    - Si `AFFINAGE` est activé, une fonction interne est utilisée pour optimiser
+      les paramètres `(R, T)` en minimisant l'erreur de reprojection.
+
+    Examples
+    --------
+    >>> ang, T, rms = calculePose3D2D(pts3D, pts2D, matK, len(pts3D), AFFINAGE=True)
+    >>> print("Angles (deg):", np.degrees(ang))
+    >>> print("Translation:", T)
+    >>> print("RMS reprojection error:", rms)
+    """
+    ROTPROJ=False
+    
+    fx = matK[0, 0]
+    fy = matK[1, 1]
+    x0 = matK[0, 2]
+    y0 = matK[1, 2]
+    # --- Constitution du système ---
+    Kinv = np.linalg.inv(matK)
+    posN = Kinv @ np.vstack((pts2D.T, np.ones((1, nbPts))))
+    posN = posN.T
+    M1 = np.hstack([
+        -pts3D, -np.ones((nbPts, 1)),
+        np.zeros((nbPts, 4)),
+        (posN[:, [0]] * pts3D), posN[:, [0]]
+    ])
+
+    M2 = np.hstack([
+        np.zeros((nbPts, 4)),
+        -pts3D, -np.ones((nbPts, 1)),
+        (posN[:, [1]] * pts3D), posN[:, [1]]
+    ])
+
+    M = np.vstack([M1, M2])
+
+    # --- Résolution SVD ---
+    U, S, Vt = svd(M)
+    pest = Vt[-1, :]  # dernière ligne
+    pest = pest / np.linalg.norm(pest[8:11])
+    Pest = pest.reshape(4,3, order='F').T  # comme reshape(...,4,3)' en MATLAB
+
+    if np.linalg.det(Pest[:, :3]) < 0:
+        Pest = -Pest
+
+    # --- R/T et angles ---
+    if ROTPROJ:
+        Ur, Sr, Vr = svd(Pest[:, :3])
+        Rest = Ur @ Vr
+    else:
+        Rest = Pest[:3, :3]
+
+
+    angcandidat = rot2ang(Rest)
+    # Sélection du plus proche angle (ici, le plus faible)
+    imin = np.argmin(np.sum(angcandidat**2, axis=0))
+    ang = angcandidat[:, imin]
+    T = -Rest.T @ Pest[:, 3]
+    # --- Affinage optionnel ---
+    if AFFINAGE:
+        angfin, Tfin, rmsInit, rmsFin = affinageRT(ang, T, matK, pts3D, pts2D)
+        mvtfin = np.concatenate((angfin.flatten(), Tfin.flatten()))
+        ang = angfin
+        T = Tfin
+    else:
+        rmsInit = 0
+        rmsFin = 0
+
+    # --- Erreur de reprojection ---
+    
+    Xw = pts3D.T
+    R = ang2rot(ang.flatten())
+    # Xc = R @ (Xw - T.reshape(3, 1))
+    # xn = Xc[:2, :] / Xc[2, :]
+    # x = np.vstack([fx * xn[0, :] + x0, fy * xn[1, :] + y0]).T
+    # res = (x - pts2D).flatten()
+    # rms_reproj = np.sqrt(np.mean(res**2))
+    # Transformation 3D → caméra
+    Xc = R @ (Xw - np.tile(T.reshape(3, 1), (1, nbPts)))  # (3 x nbPts)
+
+    # Normalisation en coordonnées homogènes
+    xn = Xc[0:2, :] / np.tile(Xc[2, :], (2, 1))  # (2 x nbPts)
+
+    # Projection avec les paramètres intrinsèques
+    x = np.vstack((fx * xn[0, :] + x0, fy * xn[1, :] + y0))  # (2 x nbPts)
+    x = x.T  # (nbPts x 2)
+
+    # Calcul du résidu et RMS
+    res = x.reshape(-1) - pts2D.reshape(-1)
+    rms_reproj = np.sqrt(np.mean(res**2))
+    # --- Affichage résultats ---
+    print(f"ang = [{ang[0]*180/np.pi:.2f}, {ang[1]*180/np.pi:.2f}, {ang[2]*180/np.pi:.2f}] deg")
+    print(f"T = [{T[0]:.4f}, {T[1]:.4f}, {T[2]:.4f}]")
+    if AFFINAGE:
+        print(f"nbPts={nbPts} --- RMS : avant={rmsInit:.4f}, après={rms_reproj:.4f}")
+    else:
+        print(f"nbPts={nbPts} --- RMS={rms_reproj:.4f}")
+    return ang, T, rms_reproj

imicpe-1.0.8.1/src/imicpe/reco3D/evalTrajecto.py

@@ -0,0 +1,167 @@
+def evalTrajecto(repBase, traj_pos, traj_ang, carte=None):
+    """
+    Évalue une trajectoire estimée par rapport à une trajectoire de référence.
+
+    Cette fonction compare une trajectoire estimée (positions et angles) avec une solution
+    de référence stockée dans des fichiers `.npz`. Elle calcule différents scores d'erreur
+    (distance, position, orientation) et affiche une visualisation 3D comparative.
+
+    Parameters
+    ----------
+    repBase : str
+        Chemin vers le dossier contenant les fichiers `solution.npz` et `info_seq.npz`.
+
+    traj_pos : ndarray of shape (3, N)
+        Positions estimées de la trajectoire. Chaque colonne représente une position 3D
+        (X, Y, Z) pour une image donnée.
+
+    traj_ang : ndarray of shape (3, N)
+        Angles d'Euler estimés (rx, ry, rz) pour chaque position. Les angles sont en radians.
+
+    carte : optional
+        Paramètre optionnel (non utilisé dans la fonction actuelle).
+
+    Returns
+    -------
+    score_pos1 : float
+        Pourcentage d'erreur relative entre la distance totale estimée et la distance réelle.
+
+    score_pos2 : float
+        Pourcentage maximal d'erreur relative entre les distances inter-images estimées et
+        celles de la trajectoire de référence.
+
+    score_ang : float
+        Erreur angulaire normalisée (en degrés par mètre), calculée entre la rotation finale
+        estimée et la rotation finale réelle.
+
+    score_posint : list of float
+        Intégrale des erreurs de position (X, Y, Z), calculées comme l'aire sous la courbe
+        des différences de position au cours du temps.
+
+    delta_pos : ndarray of shape (3, N-1)
+        Différences de position entre la solution de référence et la trajectoire estimée
+        pour chaque image (sauf la dernière).
+    
+    Notes
+    -----
+    - Cette fonction utilise la bibliothèque Plotly pour afficher une figure 3D comparant 
+      les trajectoires de référence et estimée.
+    - Les fichiers `solution.npz` doivent contenir les clés ``sol_pos`` et ``sol_ang``.
+    - Le fichier `info_seq.npz` doit contenir la clé ``Kleft``, même si elle n'est pas utilisée
+      dans cette fonction.
+    - Les angles d'Euler sont interprétés dans l'ordre ZYX pour la conversion en matrices de rotation.
+
+    Examples
+    --------
+    >>> score_pos1, score_pos2, score_ang, score_posint, delta_pos = evalTrajecto(
+    ...     repBase='data/test_seq',
+    ...     traj_pos=estimated_positions,
+    ...     traj_ang=estimated_angles
+    ... )
+    """
+
+    import numpy as np
+    import plotly.graph_objects as go
+    import os
+    
+    def ang2rot(angles):
+        """
+        Convert Euler angles (rx, ry, rz) to rotation matrix.
+        Angles are assumed in radians.
+        """
+        rx, ry, rz = angles
+        Rx = np.array([[1, 0, 0],
+                    [0, np.cos(rx), -np.sin(rx)],
+                    [0, np.sin(rx), np.cos(rx)]])
+        Ry = np.array([[np.cos(ry), 0, np.sin(ry)],
+                    [0, 1, 0],
+                    [-np.sin(ry), 0, np.cos(ry)]])
+        Rz = np.array([[np.cos(rz), -np.sin(rz), 0],
+                    [np.sin(rz), np.cos(rz), 0],
+                    [0, 0, 1]])
+        return Rz @ Ry @ Rx
+
+
+    def rodrigues(R):
+        """
+        Convert rotation matrix to rotation vector (axis-angle) or vice versa.
+        """
+        if R.shape == (3,):
+            # Rotation vector -> matrix
+            theta = np.linalg.norm(R)
+            if theta < 1e-12:
+                return np.eye(3)
+            omega = R / theta
+            K = np.array([[0, -omega[2], omega[1]],
+                        [omega[2], 0, -omega[0]],
+                        [-omega[1], omega[0], 0]])
+            return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K)
+        elif R.shape == (3, 3):
+            # Matrix -> rotation vector
+            U, _, Vt = np.linalg.svd(R)
+            R = U @ Vt  # project onto SO(3)
+            theta = np.arccos(np.clip((np.trace(R)-1)/2, -1, 1))
+            if theta < 1e-12:
+                return np.zeros(3)
+            else:
+                return (theta/(2*np.sin(theta))) * np.array([R[2,1]-R[1,2],
+                                                            R[0,2]-R[2,0],
+                                                            R[1,0]-R[0,1]])
+        else:
+            raise ValueError("Input must be 3-vector or 3x3 matrix")
+
+
+    # Chargement des données MATLAB
+    # solution = loadmat(f"{repBase}/solution.mat")
+    # info_seq = loadmat(f"{repBase}/info_seq.mat")
+    solution = np.load(os.path.join(repBase, 'solution.npz'))
+    info_seq = np.load(os.path.join(repBase, 'info_seq.npz'))
+    sol_pos = solution['sol_pos']
+    sol_ang = solution['sol_ang']
+    matK = info_seq['Kleft']
+    
+    nbIma = sol_pos.shape[1]
+    interdist_est = []
+    delta_pos = []
+
+    # Calcul des distances et différences de positions
+    for cpt in range(nbIma-1):
+        interdist_est.append(np.linalg.norm(traj_pos[:,cpt+1]-traj_pos[:,cpt]))
+        delta_pos.append(sol_pos[:,cpt] - traj_pos[:,cpt])
+    
+    interdist_est = np.array(interdist_est)
+    delta_pos = np.array(delta_pos).T  # 3 x (nbIma-1)
+    
+    totaldist_est = np.sum(interdist_est)
+    totaldist = np.sum(np.linalg.norm(np.diff(sol_pos, axis=1), axis=0))
+    interdist = np.linalg.norm(np.diff(sol_pos, axis=1), axis=0)
+
+    score_pos1 = 100 * np.abs(totaldist_est - totaldist) / totaldist
+    score_pos2 = 100 * np.max(np.abs(interdist_est - interdist) / interdist)
+
+    # Calcul de l'erreur angulaire
+    Rest = ang2rot(traj_ang[:, -1])
+    Rtrue = ang2rot(sol_ang[:, -1])
+    Rerr = Rest.T @ Rtrue
+    rodri = rodrigues(Rerr)
+    score_ang = np.linalg.norm(rodri) * 180 / (np.pi * totaldist)
+
+    score_posint = [np.trapezoid(delta_pos[0,:]), np.trapezoid(delta_pos[1,:]), np.trapezoid(delta_pos[2,:])]
+
+    print(f"score_pos1 = {score_pos1:.2f}")
+    print(f"score_pos2 = {score_pos2:.2f}")
+    print(f"score_ang = {score_ang:.2f}")
+    print(f"integral Delta pos X = {score_posint[0]:.2f}")
+    print(f"integral Delta pos Y = {score_posint[1]:.2f}")
+    print(f"integral Delta pos Z = {score_posint[2]:.2f}")
+
+    # ==== Affichage comparatif ====
+    fig = go.Figure()
+    fig.add_trace(go.Scatter3d(x=sol_pos[0,:], y=sol_pos[1,:], z=sol_pos[2,:], mode='lines', line=dict(color='red'), name='ref'))
+    fig.add_trace(go.Scatter3d(x=traj_pos[0,:], y=traj_pos[1,:], z=traj_pos[2,:], mode='lines', line=dict(color='blue'), name='estim'))
+    fig.update_layout(title='Comparaison estimation de la trajectoire 3D (rouge=ref)',
+                      scene=dict(xaxis_title='X', yaxis_title='Y', zaxis_title='Z'),
+                      legend=dict(x=0, y=1))
+    fig.show()
+
+    return score_pos1, score_pos2, score_ang, score_posint, delta_pos

imicpe-1.0.8.1/src/imicpe/reco3D/vue3D.py

@@ -0,0 +1,216 @@
+import numpy as np
+import plotly.graph_objects as go
+
+def vue3D(fig, pos3D, matK, sizeim, attitude, position, camColor, operation):
+    """
+    Affiche une vue 3D interactive de nuages de points et de caméras avec Plotly.
+
+    Parameters
+    ----------
+    fig : plotly.graph_objects.Figure
+        Objet figure Plotly dans lequel la scène 3D est affichée ou mise à jour.
+    pos3D : ndarray
+        Coordonnées 3D des points dans le repère monde (array de forme (3, N)).
+    matK : ndarray
+        Matrice des paramètres intrinsèques de la caméra (3x3).
+    sizeim : tuple of int
+        Dimensions de l'image (hauteur, largeur), utilisées pour le champ de vision.
+    attitude : ndarray
+        Angles d'attitude de la caméra (angles d'Euler ou matrice de rotation).
+    position : ndarray
+        Position de la caméra dans le repère monde (vecteur de taille 3).
+    camColor : str or tuple
+        Couleur utilisée pour représenter la caméra dans la scène.
+    operation : {'create', 'addcam', 'addpts'}
+        Type d'opération à effectuer :
+            - 'create' : crée une nouvelle scène 3D,
+            - 'addcam' : ajoute une caméra à la scène,
+            - 'addpts' : ajoute un nuage de points à la scène.
+            
+    Returns
+    -------
+    fig : plotly.graph_objects.Figure
+        Objet figure Plotly dans lequel la scène 3D est affichée ou mise à jour.
+        
+    Notes
+    -----
+    Cette fonction permet de visualiser des scènes 3D de manière interactive avec Plotly.
+    Elle peut être utilisée pour représenter des caméras virtuelles, des trajectoires,
+    ou des nuages de points 3D dans un repère monde, avec une gestion dynamique
+    des éléments affichés selon l'opération demandée.
+
+    Example
+    -------
+    >>> from imicpe.reco3D import vue3D
+    >>> import plotly.graph_objects as go
+    >>> fig = go.Figure()
+    >>> vue3D(fig, pos3D, matK, (480, 640), attitude, position, 'blue', 'create')
+    """
+
+    # """
+    # 3D view function for displaying point clouds and cameras.
+    
+    # :param fig: The figure handler
+    # :param pos3D: 3D positions of the points in world coordinates (3xN array)
+    # :param matK: Intrinsic camera parameters
+    # :param sizeim: Image size (height, width)
+    # :param attitude: Camera attitude angles (Euler angles)
+    # :param position: Camera position in world coordinates
+    # :param camColor: Camera display color
+    # :param operation: Display operation, 'create', 'addcam', or 'addpts'
+    # """
+    seuil_dist = 30  # threshold for deleting points that are too far away
+    # Check for correct number of arguments
+    if fig is not None and type(fig) is not go.Figure:
+        print('First argument must be a Plotly figure')
+        return
+    
+    if len(attitude) == 0 or len(position) == 0:
+        print('Attitude and position should be provided')
+        return
+    
+    if (pos3D.shape[0] > 3):
+        pos3D=pos3D.T
+        print('warning : le tableau pos3D doit etre 3xN')
+        
+    # Prepare params
+    params = {
+        'fig': fig,
+        'pt3DStyle': 'blue',
+        'camColor': camColor,
+        'holdon': False if operation == 'create' else True,
+        'tail': 1,
+    }
+    
+    nb_points = pos3D.shape[1]
+    
+    
+    # Calculate distance between camera and 3D points
+    if len(position) == 0:
+        tensP = attitude
+        vec = pos3D - np.repeat(tensP[:, 3].reshape(3, 1), nb_points, axis=1)
+        dist = np.linalg.norm(vec, axis=0)
+    else:
+        Rloc = _euler_to_rot(attitude, 'zxy')
+        tensP = np.hstack([Rloc, -np.dot(Rloc, position.reshape(-1, 1))])
+        vec = pos3D - np.repeat(position.reshape(3, 1), nb_points, axis=1)
+        dist = np.linalg.norm(vec, axis=0)
+    
+    # Filter points that are too far away
+    indOK = np.where(dist < seuil_dist)[0]
+    pos3D_filtered = pos3D[:, indOK]
+    if fig is None:
+        # Create the plotly figure
+        fig = go.Figure()
+
+    # Operation: 'addpts' means just add 3D points
+    if operation == 'addpts':
+        fig.add_trace(go.Scatter3d(
+            x=pos3D_filtered[0, :],
+            y=pos3D_filtered[1, :],
+            z=pos3D_filtered[2, :],
+            mode='markers',
+            marker=dict(color='red', size=5)
+        ))
+
+    # Operation: 'addcam' means add the camera only
+    elif operation == 'addcam':
+        _plot_camera(tensP, matK, sizeim, fig, camColor)
+
+    # Operation: 'create' means both camera and 3D points
+    else:
+        fig.add_trace(go.Scatter3d(
+            x=pos3D_filtered[0, :],
+            y=pos3D_filtered[1, :],
+            z=pos3D_filtered[2, :],
+            mode='markers',
+            marker=dict(color='blue', size=5)
+        ))
+        _plot_camera(tensP, matK, sizeim, fig, camColor)
+
+    # Set figure layout
+    fig.update_layout(
+        scene=dict(
+            xaxis_title='X',
+            yaxis_title='Y',
+            zaxis_title='Z',
+        ),
+        title=f"3D View",
+        width=800,
+        height=800,
+    )
+
+    return fig
+
+def _plot_camera(tensP, matK, sizeim, fig, camColor):
+    """
+    Plots the camera as a cone shape in the 3D space.
+    """
+    if len(tensP.shape) < 3:
+        tensP = tensP[:,:,np.newaxis]
+    nb_cam = tensP.shape[2]
+
+    imasize =  [sizeim[1], sizeim[0]]#np.fliplr(sizeim).squeeze()  # Flip size to [width, height]
+    for cptC in range(nb_cam):
+        current_tensP = tensP[:, :, cptC]
+        cone_vertices = np.zeros((3, 5))
+        vtmp = np.array([
+            [1, imasize[0], imasize[0], 1],
+            [1, 1, imasize[1], imasize[1]],
+            [1, 1, 1, 1]
+        ])
+
+        matK_inv = np.linalg.inv(matK)
+        cone_vertices[:, 1:] = np.dot(matK_inv, vtmp) * 2  # Use tail length as scaling factor
+    
+    
+        
+        cVWorld = np.dot(current_tensP[:, :3].T, (cone_vertices - current_tensP[:, 3].reshape(3, 1)))
+
+        # Add the cone edges to the plot
+        for i in range(4):
+            fig.add_trace(go.Scatter3d(
+                x=[cVWorld[0, 0], cVWorld[0, i + 1]],
+                y=[cVWorld[1, 0], cVWorld[1, i + 1]],
+                z=[cVWorld[2, 0], cVWorld[2, i + 1]],
+                mode='lines',
+                line=dict(color=camColor, width=2),
+                showlegend=False  # Exclude from legend
+            ))
+    
+            fig.add_trace(go.Scatter3d(
+                x=[cVWorld[0, i + 1], cVWorld[0, (i + 1) % 4 + 1]],  # Cycle through the vertices
+                y=[cVWorld[1, i + 1], cVWorld[1, (i + 1) % 4 + 1]],
+                z=[cVWorld[2, i + 1], cVWorld[2, (i + 1) % 4 + 1]],
+                mode='lines',
+                line=dict(color=camColor, width=2),
+                showlegend=False  # Exclude from legend
+            ))
+
+def _euler_to_rot(euler_angles, euler_order='xyz'):
+    """
+    Convert Euler angles to rotation matrix using the specified order.
+    """
+    cx, cy, cz = np.cos(np.radians(euler_angles.squeeze()))
+    sx, sy, sz = np.sin(np.radians(euler_angles.squeeze()))
+
+    R_x = np.array([[1, 0, 0],
+                    [0, cx, -sx],
+                    [0, sx, cx]])
+
+    R_y = np.array([[cy, 0, sy],
+                    [0, 1, 0],
+                    [-sy, 0, cy]])
+
+    R_z = np.array([[cz, -sz, 0],
+                    [sz, cz, 0],
+                    [0, 0, 1]])
+
+    if euler_order == 'xyz':
+        return np.dot(R_z, np.dot(R_y, R_x))
+    elif euler_order == 'zyx':
+        return np.dot(R_x, np.dot(R_y, R_z))
+    # Other orders can be added as needed
+
+    return np.eye(3)  # Default to identity if no valid order is found
+

{imicpe-1.0.7 → imicpe-1.0.8.1}/src/imicpe.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: imicpe
-Version: 1.0.7
+Version: 1.0.8.1
 Summary: Toolbox for Maths,Signal,Image Teaching @ CPE
 Author-email: Marion Foare <marion.foare@cpe.fr>, Eric Van Reeth <eric.vanreeth@cpe.fr>, Arthur Gautheron <arthur.gautheron@cpe.fr>
 License: MIT License

{imicpe-1.0.7 → imicpe-1.0.8.1}/src/imicpe.egg-info/SOURCES.txt

@@ -21,4 +21,9 @@ src/imicpe/optim/metrics.py
 src/imicpe/optim/operators.py
 src/imicpe/optim/pnnDataset.py
 src/imicpe/optim/pnnTrainer.py
-src/imicpe/optim/pnnUtils.py
+src/imicpe/optim/pnnUtils.py
+src/imicpe/reco3D/__init__.py
+src/imicpe/reco3D/angles.py
+src/imicpe/reco3D/calculPose.py
+src/imicpe/reco3D/evalTrajecto.py
+src/imicpe/reco3D/vue3D.py

imicpe-1.0.7/src/imicpe/_version.py

@@ -1 +0,0 @@
-__version__="1.0.7"