Ena končnih verzij, vključuje opcijo izvoz v DICOM datoteko in predporavnavo glede na višino CT slike. Dodal preverjanje podatkov in opozorila. Segmentacije izvožene iz Eclipsa je potrebno ročno pretvoriti v modele.

SeekTransformModule/SeekTransformModule.py

@@ -5,8 +5,11 @@ import re
 from scipy.spatial.distance import cdist
 from scipy.spatial.transform import Rotation as R
 import slicer
+import slicer.util
 import itertools
+import DICOMLib
 from DICOMLib import DICOMUtils
+import DicomRtImportExportPlugin
 from collections import deque, Counter
 import vtk
 from slicer.ScriptedLoadableModule import *
@@ -14,6 +17,11 @@ import qt
 import matplotlib.pyplot as plt
 import csv
 import time
+import logging
+import matplotlib
+matplotlib.use('Agg')  # << to dodaš ZGORAJ, da omogoči PNG zapis brez GUI
+from mpl_toolkits.mplot3d import Axes3D  
+
 
 #exec(open("C:/Users/lkomar/Documents/Prostata/FirstTryRegister.py").read())
 
@@ -57,12 +65,21 @@ class SeekTransformModuleWidget(ScriptedLoadableModuleWidget):
         self.layout.addWidget(self.translationCheckBox)
 
         self.markersCheckBox = qt.QCheckBox("Place control points for detected markers")
-        self.markersCheckBox.setChecked(True)
+        self.markersCheckBox.setChecked(False)
         self.layout.addWidget(self.markersCheckBox)
         
-        self.writefileCheckBox = qt.QCheckBox("Write distances to csv file")
+        self.writefileCheckBox = qt.QCheckBox("Write data to csv file")
         self.writefileCheckBox.setChecked(True)
         self.layout.addWidget(self.writefileCheckBox)
+        
+        self.tableCheckBox = qt.QCheckBox("Find top of the table and match height")
+        self.tableCheckBox.setChecked(True)
+        self.layout.addWidget(self.tableCheckBox)
+        
+        self.DicomCheckBox = qt.QCheckBox("Save transformed DICOM")
+        self.DicomCheckBox.setChecked(True)
+        self.layout.addWidget(self.DicomCheckBox)
+
 
         # Load button
         self.applyButton = qt.QPushButton("Find markers and transform")
@@ -76,6 +93,15 @@ class SeekTransformModuleWidget(ScriptedLoadableModuleWidget):
         self.layout.addStretch(1)
 
     def onApplyButton(self):
+                # Nastavi globalni logger
+        log_file_path = os.path.join("C:/Users/lkomar/Documents/Prostata", "seektransform_log.txt")
+        logging.basicConfig(filename=log_file_path, level=logging.INFO, format='%(asctime)s - %(message)s')
+        
+        try:
+            logging.info("▶️ onApplyButton pressed.")
+        except Exception as e:
+            print("❌ Logging setup failed:", e)
+        
         logic = MyTransformModuleLogic()
         selectedMethod = self.rotationMethodComboBox.currentText  # izberi metodo izračuna rotacije
 
@@ -85,9 +111,11 @@ class SeekTransformModuleWidget(ScriptedLoadableModuleWidget):
         applyScaling = self.scalingCheckBox.isChecked()
         applyMarkers = self.markersCheckBox.isChecked()
         writefilecheck = self.writefileCheckBox.isChecked()
+        tablefind = self.tableCheckBox.isChecked()
+        saveasdicom = self.DicomCheckBox.isChecked()
 
         # Pokliči logiko z izbranimi nastavitvami
-        logic.run(selectedMethod, applyRotation, applyTranslation, applyScaling, applyMarkers, writefilecheck)
+        logic.run(selectedMethod, applyRotation, applyTranslation, applyScaling, applyMarkers, writefilecheck, tablefind, saveasdicom)
 
 
 class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
@@ -96,9 +124,12 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
     """
     
     
-    def run(self, selectedMethod, applyRotation, applyTranslation, applyScaling, applymarkers, writefilecheck):
+    def run(self, selectedMethod, applyRotation, applyTranslation, applyScaling, applymarkers, writefilecheck, tablefind, save_as_dicom):
         start_time = time.time()
         print("Calculating...")
+        #slicer.util.delayDisplay(f"Starting", 1000)
+
+        
         
         def group_points(points, threshold):
             # Function to group points that are close to each other
@@ -326,7 +357,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
 
             return moving, R, t
 
-        def match_points(cbct_points, ct_points, auto_weights=True, fallback_if_worse=True, normalize_lengths=True, normalize_angles=False, min_distance=5):
+        def match_points(cbct_points, ct_points, auto_weights=False, fallback_if_worse=False, normalize_lengths=True, normalize_angles=False, min_distance=5, w_order=1.0):
 
             def side_lengths(points):
                 lengths = [
@@ -349,7 +380,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                 norm = np.linalg.norm(vec)
                 return [v / norm for v in vec] if norm > 0 else vec
 
-            def permutation_score(perm, ct_lengths, ct_angles, w_len, w_ang):
+            def permutation_score(perm, ct_lengths, ct_angles, w_len, w_ang, penalty_angle_thresh=np.deg2rad(10)):
                 perm_lengths = side_lengths(perm)
                 perm_angles = triangle_angles(perm)
 
@@ -365,10 +396,34 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
 
                 score_len = sum(abs(a - b) for a, b in zip(lengths_1, lengths_2))
                 score_ang = sum(abs(a - b) for a, b in zip(angles_1, angles_2))
-
-                return w_len * score_len + w_ang * score_ang
-
+                
+                order_penalty = order_mismatch_penalty(ct_points, perm, axis='z')
+                return w_len * score_len + w_ang * score_ang + order_penalty + w_order * order_penalty
+                
+            def smart_sort_cbct_points(cbct_points, z_threshold=5.0):
+                """
+                Sortira točke tako, da poskusi najprej po Z. Če so razlike po Z manjše
+                od praga, sortira po (Y, X), sicer sortira po Z.
+                """
+                z_values = [pt[2] for pt in cbct_points]
+                z_range = max(z_values) - min(z_values)
+
+                if z_range < z_threshold:
+                    # Sortiraj po Y, nato X (če so točke v isti ravnini po Z)
+                    return sorted(cbct_points, key=lambda pt: (pt[1], pt[0]))
+                else:
+                    # Sortiraj po Z, nato Y, nato X
+                    return sorted(cbct_points, key=lambda pt: (pt[2], pt[1], pt[0]))
+            
+            def order_mismatch_penalty(ct_points, perm, axis='z'):
+                axis_idx = {'x': 0, 'y': 1, 'z': 2}[axis]
+                ct_sorted = np.argsort([pt[axis_idx] for pt in ct_points])  
+                perm_sorted = np.argsort([pt[axis_idx] for pt in perm])
+                return sum(1 for a, b in zip(ct_sorted, perm_sorted) if a != b)
+            
+            
             cbct_points = list(cbct_points)
+            print("CBCT points:", cbct_points)
             ct_lengths = side_lengths(np.array(ct_points))
             ct_angles = triangle_angles(np.array(ct_points))
 
@@ -379,30 +434,37 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                 weight_length = (1 - var_len / total_var)
                 weight_angle = (1 - var_ang / total_var)
             else:
-                weight_length = 0.5
-                weight_angle = 0.5
+                weight_length = 0.8
+                weight_angle = 0.2
 
-            best_perm = None
+            cbct_sorted = smart_sort_cbct_points(cbct_points)
+            original_score = permutation_score(np.array(cbct_sorted), ct_lengths, ct_angles, weight_length, weight_angle)
+
+            # Če je ta rezultat dovolj dober, uporabi
             best_score = float('inf')
-            #print(f"CT points: {ct_points}")
+            best_perm = None
+
+            if original_score < float('inf'):  # lahko dodaš prag če želiš
+                best_score = original_score
+                best_perm = np.array(cbct_sorted)
+
+            # Nato preveri vse permutacije (vključno s prvotnim vrstnim redom, če fallback_if_worse=True)
             for perm in itertools.permutations(cbct_points):
                 perm = np.array(perm)
                 score = permutation_score(perm, ct_lengths, ct_angles, weight_length, weight_angle)
-
-                #print(f"Perm: {perm.tolist()} -> Score: {score:.4f}")
-
                 if score < best_score:
                     best_score = score
                     best_perm = perm
+                    print(f"New best permutation found with perm: {perm}")
 
             #print("CT centroid:", np.mean(ct_points, axis=0))
             #print("CBCT centroid (best perm):", np.mean(best_perm, axis=0))
             
             if fallback_if_worse:
-                original_score = permutation_score(np.array(cbct_points), ct_lengths, ct_angles, weight_length, weight_angle)
-                #print("Original score: ", original_score)
+                #original_score = permutation_score(np.array(cbct_points), ct_lengths, ct_angles, weight_length, weight_angle)
+                print("Original score: ", original_score)
                 if original_score <= best_score:
-                    #print("Fallback to original points due to worse score of the permutation.")
+                    print("Fallback to original points due to worse score of the permutation.")
                     return list(cbct_points)
 
             return list(best_perm)
@@ -475,20 +537,8 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                 cumulative_matrices[key] = np.dot(cumulative_matrices[key], affine_matrix)
 
             return transform
-        
-        def matrix_to_array(vtk_transform):
-            """
-            Converts a vtkTransform to a 4x4 numpy array.
-            """
-            vtk_matrix = vtk.vtkMatrix4x4()
-            vtk_transform.GetMatrix(vtk_matrix)
-            np_matrix = np.zeros((4, 4))
-            for i in range(4):
-                for j in range(4):
-                    np_matrix[i, j] = vtk_matrix.GetElement(i, j)
-            return np_matrix
-        
-        def save_transform_matrix(matrix, study_name, cbct_volume_name, ct_table_found):
+
+        def save_transform_matrix(matrix, study_name, cbct_volume_name):
             """
             Appends the given 4x4 matrix to a text file under the given study folder.
             """
@@ -497,17 +547,13 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             os.makedirs(study_folder, exist_ok=True)  # Create folders if they don't exist
             safe_cbct_name = re.sub(r'[<>:"/\\|?*]', '_', cbct_volume_name)
             # Preveri ali je CT miza najdena
-            if ct_table_found:
-                safe_cbct_name += "_MizaPoravnava"
-            else:
-                safe_cbct_name += "_NIMizaPoravnana"
             filename = os.path.join(study_folder, f"{safe_cbct_name}.txt")
-            with open(filename, "a") as f:
+            with open(filename, "w") as f:
                 #f.write("Transformacija:\n")
                 for row in matrix:
                     f.write(" ".join(f"{elem:.6f}" for elem in row) + "\n")
                 f.write("\n")  # Dodaj prazen vrstico med transformacijami
-            print(f"Transform matrix saved to {filename}")
+            #print(f"Transform matrix saved to {filename}")
                 
         def detect_points_region_growing(volume_name, yesCbct, create_marker, intensity_threshold=3000, x_min=90, x_max=380, y_min=190, y_max=380, z_min=50, z_max=140, max_distance=9, centroid_merge_threshold=5):
             volume_node = find_volume_node_by_partial_name(volume_name)
@@ -650,7 +696,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             if candidate_y_values:
                 most_common_y, _ = Counter(candidate_y_values).most_common(1)[0]
                 table_top_y = most_common_y
-                print(f"✅ Rob mize (najpogostejši Y): {table_top_y}, pojavitev: {candidate_y_values.count(table_top_y)}×")
+                print(f"✅ Rob mize (najpogostejši Y): {table_top_y}, pojavitev: {candidate_y_values.count(table_top_y)}/11")
 
             
             """ # --- Poišči skok navzdol pod prag (od spodaj navzgor) ---
@@ -688,7 +734,6 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
 
             return table_ras[1], table_top_y
 
-        
         def align_cbct_to_ct(volumeNode, scan_type, offset, CT_offset=None, CT_spacing=None):
             """
             Aligns CBCT volume to CT volume based on height offset.
@@ -715,7 +760,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                 CBCT_offset = offset
 
                 # Razlika v mm brez skaliranja na CBCT_spacing
-                alignment_offset_mm = CT_offset - CBCT_offset
+                alignment_offset_mm = CT_offset
 
                 #print(f"CT offset: {CT_offset}, CBCT offset: {CBCT_offset}")
                 #print(f"CT spacing: {CT_spacing} mm/voxel, CBCT spacing: {volumeNode.GetSpacing()[1]} mm/voxel")
@@ -876,9 +921,6 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             """
             Odstrani točko, ki je najnižja po dani osi (default Y-os v RAS prostoru).
             """
-            if len(points) <= 3:
-                return points  # Ni potrebe po brisanju
-
             arr = np.array(points)
             lowest_index = np.argmin(arr[:, axis])
             removed = points.pop(lowest_index)
@@ -890,7 +932,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             file_exists = os.path.isfile(file_path)
 
             with open(file_path, mode='a', newline='') as csvfile:
-                fieldnames = ["Study", "IO", "Fixing", "Scaling", "CentroidAlign", "Rotation", "Translation", "Transform", "FileSave", "Total"]
+                fieldnames = ["Study", "IO", "Fixing", "Table", "Scaling", "CentroidAlign", "Rotation", "Translation", "Transform", "FileSave", "Total"]
                 writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
 
                 if not file_exists:
@@ -906,6 +948,248 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                     return node
             raise RuntimeError(f"❌ Volume with name containing '{partial_name}' not found.")
                 
+        def convert_rtstruct_to_segmentation_nodes(ct_volume_node):
+            shNode = slicer.vtkMRMLSubjectHierarchyNode.GetSubjectHierarchyNode(slicer.mrmlScene)
+            segmentation_nodes = []
+
+            # ✅ Najdi vse SegmentationNode-e, ki imajo segmente
+            for seg_node in slicer.util.getNodesByClass("vtkMRMLSegmentationNode"):
+                num_segments = seg_node.GetSegmentation().GetNumberOfSegments()
+                if num_segments == 0:
+                    continue
+
+                # Če še nima referenceVolume, jo nastavimo
+                if not seg_node.GetNodeReferenceID("referenceVolume"):
+                    seg_node.SetReferenceImageGeometryParameterFromVolumeNode(ct_volume_node)
+                    seg_node.SetNodeReferenceID("referenceVolume", ct_volume_node.GetID())
+                    print(f"🔗 Nastavljena referenca na CT za: {seg_node.GetName()}")
+
+                segmentation_nodes.append(seg_node)
+                print(f"📎 Najdena segmentacija z vsebino: {seg_node.GetName()}, segmentov: {num_segments}")
+
+            if segmentation_nodes:
+                return segmentation_nodes
+
+            for item_id in range(shNode.GetNumberOfItems()):
+                modality = shNode.GetItemAttribute(item_id, "DICOM.Modality")
+                if modality != "RTSTRUCT":
+                    continue
+
+                rtstruct_node = shNode.GetItemDataNode(item_id)
+                if not rtstruct_node:
+                    continue
+
+                print(f"📎 Najden RTSTRUCT: {rtstruct_node.GetName()}")
+
+                # Ustvari nov SegmentationNode
+                seg_node = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLSegmentationNode", f"Seg_{rtstruct_node.GetName()}")
+                seg_node.SetReferenceImageGeometryParameterFromVolumeNode(ct_volume_node)
+                seg_node.SetNodeReferenceID("referenceVolume", ct_volume_node.GetID())
+
+
+
+                # Najdi child elemente v SubjectHierarchy (strukture)
+                rtstruct_item_id = shNode.GetItemByDataNode(rtstruct_node)
+                structure_ids = vtk.vtkIdList()
+                shNode.GetItemChildren(rtstruct_item_id, structure_ids)
+
+                segment_count = 0
+
+                for i in range(structure_ids.GetNumberOfIds()):
+                    structure_item_id = structure_ids.GetId(i)
+                    name = shNode.GetItemName(structure_item_id)
+                    associated_node = shNode.GetItemDataNode(structure_item_id)
+                    if associated_node and associated_node.IsA("vtkMRMLModelNode"):
+                        print(f" ➕ Dodajam strukturo: {name}")
+                        slicer.modules.segmentations.logic().ImportModelToSegmentationNode(associated_node, seg_node)
+                        seg_node.GetSegmentation().GetSegment(seg_node.GetSegmentation().GetNumberOfSegments() - 1).SetName(name)
+                        segment_count += 1
+
+                # Če ni bilo nič dodano iz SH: poskusi uvoziti modele iz scene
+                if segment_count == 0:
+                    print("⚠️ RTSTRUCT nima struktur v SubjectHierarchy – poskus uvoza vseh modelov iz scene.")
+                    for model_node in slicer.util.getNodesByClass("vtkMRMLModelNode"):
+                        if "RTSTRUCT" in model_node.GetName().upper() or model_node.GetName().startswith("Model"):
+                            print(f" ➕ [fallback] Uvoz modela: {model_node.GetName()}")
+                            slicer.modules.segmentations.logic().ImportModelToSegmentationNode(model_node, seg_node)
+                            seg_node.GetSegmentation().GetSegment(seg_node.GetSegmentation().GetNumberOfSegments() - 1).SetName(model_node.GetName())
+                
+                print(f"📊 Segmentov v {seg_node.GetName()}: {seg_node.GetSegmentation().GetNumberOfSegments()}")
+                segmentation_nodes.append(seg_node)
+
+            return segmentation_nodes
+
+        def apply_cumulative_transform_to_segmentation(segmentation_node, matrix):
+            transform = vtk.vtkTransform()
+            vtk_matrix = vtk.vtkMatrix4x4()
+            for i in range(4):
+                for j in range(4):
+                    vtk_matrix.SetElement(i, j, matrix[i, j])
+            transform.SetMatrix(vtk_matrix)
+
+            transform_node = slicer.vtkMRMLTransformNode()
+            slicer.mrmlScene.AddNode(transform_node)
+            transform_node.SetAndObserveTransformToParent(transform)
+            segmentation_node.SetAndObserveTransformNodeID(transform_node.GetID())
+
+            slicer.vtkSlicerTransformLogic().hardenTransform(segmentation_node)
+            slicer.mrmlScene.RemoveNode(transform_node)
+
+        def convert_all_models_to_segmentation(reference_volume_name: str, prefix: str = "Imported_"):
+            """
+            Pretvori vse modele (ModelNode) v sceni v enoten vtkMRMLSegmentationNode.
+
+            📥 VHODI:
+            ----------
+            reference_volume_name : str
+                Ime obstoječega CT volumna (npr. "CT_1"), ki določa geometrijo za segmentacijo.
+                Ta volumen mora biti že naložen v sceni.
+
+            prefix : str
+                Predpona za ime novega segmentation noda (npr. "Imported_").
+                Ime novega noda bo nekaj kot: "Imported_Segmentation".
+
+            📤 IZHOD:
+            ----------
+            segmentation_node : vtkMRMLSegmentationNode
+                Nov nod, ki vsebuje en segment za vsak najden model v sceni.
+                Ta segmentacijski nod je pripravljen za transformacijo in DICOM export (vsebuje BinaryLabelmap).
+            """
+            import slicer
+
+            # Pridobi referenčni volumen (CT)
+            reference_volume = slicer.util.getNode(reference_volume_name)
+
+            # Ustvari nov segmentacijski nod
+            segmentation_node = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLSegmentationNode", prefix + "Segmentation")
+            segmentation_node.SetReferenceImageGeometryParameterFromVolumeNode(reference_volume)
+            segmentation_node.SetNodeReferenceID("referenceVolume", reference_volume.GetID())
+
+            # Najdi vse modele
+            model_nodes = slicer.util.getNodesByClass("vtkMRMLModelNode")
+            #print(f"📦 Najdenih modelov: {len(model_nodes)}")
+            
+            for model_node in model_nodes:
+                name = model_node.GetName()
+                #print(f"🔍 Model: {name}")
+                skip = (
+                    name.lower().startswith("segmentation")
+                    or name.lower().startswith("surface")
+                    or name.lower() in ["red volume slice", "green volume slice", "yellow volume slice"]
+                    or "rtstruct" in name.lower()
+                )
+                if skip:
+                    continue
+
+
+                # Uvozi model kot segment
+                success = slicer.modules.segmentations.logic().ImportModelToSegmentationNode(model_node, segmentation_node)
+                if not success:
+                    #print(f"✅ Model '{name}' uvožen kot segment.")
+                    print(f"❌ Napaka pri uvozu modela: {name}")
+                    
+
+            # Ustvari BinaryLabelmap reprezentacijo (nujno za DICOM export)
+            created = segmentation_node.GetSegmentation().CreateRepresentation("BinaryLabelmap")
+            if created:
+                print("✅ BinaryLabelmap reprezentacija uspešno ustvarjena.")
+                segmentation_node.GetSegmentation().SetMasterRepresentationName("BinaryLabelmap")
+            #else:
+                #print("❌ Pretvorba v BinaryLabelmap ni uspela.")
+
+            return segmentation_node
+
+        def apply_cumulative_transform_to_volume(volume_node, matrix):
+            transform = vtk.vtkTransform()
+            vtk_matrix = vtk.vtkMatrix4x4()
+            for i in range(4):
+                for j in range(4):
+                    vtk_matrix.SetElement(i, j, matrix[i, j])
+            transform.SetMatrix(vtk_matrix)
+
+            transform_node = slicer.vtkMRMLTransformNode()
+            slicer.mrmlScene.AddNode(transform_node)
+            transform_node.SetAndObserveTransformToParent(transform)
+            volume_node.SetAndObserveTransformNodeID(transform_node.GetID())
+
+            slicer.vtkSlicerTransformLogic().hardenTransform(volume_node)
+            slicer.mrmlScene.RemoveNode(transform_node)
+
+        def triangle_similarity(p1, p2):
+            """
+            Primerja dva trikotnika (vsak definiran s 3 točkami v 3D) na podlagi:
+            - razmerij dolžin
+            - razlik med koti
+
+            :param p1: seznam treh točk (npr. ct_points)
+            :param p2: seznam treh točk (npr. cbct_points)
+            :return: dict z razliko dolžin, razliko kotov, povprečno napako
+            """
+            def side_lengths(pts):
+                a = np.linalg.norm(pts[1] - pts[0])
+                b = np.linalg.norm(pts[2] - pts[1])
+                c = np.linalg.norm(pts[0] - pts[2])
+                return np.array([a, b, c])
+
+            def angles(pts):
+                # uporabimo kosinusni izrek
+                a, b, c = side_lengths(pts)
+                angle_A = np.arccos((b**2 + c**2 - a**2) / (2*b*c))
+                angle_B = np.arccos((a**2 + c**2 - b**2) / (2*a*c))
+                angle_C = np.pi - angle_A - angle_B
+                return np.degrees([angle_A, angle_B, angle_C])
+
+            l1 = side_lengths(p1)
+            l2 = side_lengths(p2)
+            a1 = angles(p1)
+            a2 = angles(p2)
+
+            length_diff = np.abs(l1 - l2)
+            angle_diff = np.abs(a1 - a2)
+
+            return {
+                "side_length_diff_mm": length_diff,
+                "angle_diff_deg": angle_diff,
+                "mean_length_error_mm": np.mean(length_diff),
+                "mean_angle_error_deg": np.mean(angle_diff),
+                "TSR": 1 / (1 + np.mean(length_diff) + np.mean(angle_diff) / 10)
+            }
+
+        def save_triangle_visualization(ct_pts, cbct_pts, outpath):
+            fig = plt.figure(figsize=(10, 8))
+            ax = fig.add_subplot(111, projection='3d')
+
+            ct_closed = np.vstack([ct_pts, ct_pts[0]])
+            cbct_closed = np.vstack([cbct_pts, cbct_pts[0]])
+
+            ax.plot(ct_closed[:, 0], ct_closed[:, 1], ct_closed[:, 2], 'b-', label='CT trikotnik')
+            ax.scatter(ct_pts[:, 0], ct_pts[:, 1], ct_pts[:, 2], c='blue')
+
+            ax.plot(cbct_closed[:, 0], cbct_closed[:, 1], cbct_closed[:, 2], 'r--', label='CBCT trikotnik')
+            ax.scatter(cbct_pts[:, 0], cbct_pts[:, 1], cbct_pts[:, 2], c='red')
+
+            ct_centroid = np.mean(ct_pts, axis=0)
+            cbct_centroid = np.mean(cbct_pts, axis=0)
+            ax.scatter(*ct_centroid, c='blue', marker='x', s=60)
+            ax.scatter(*cbct_centroid, c='red', marker='x', s=60)
+
+            
+            ax.set_title("Trikotnik markerjev: CT (modro) vs CBCT (rdeče)")
+            ax.set_xlabel('X (mm)')
+            ax.set_ylabel('Y (mm)')
+            ax.set_zlabel('Z (mm)')
+            ax.legend()
+            ax.view_init(elev=20, azim=30)
+            plt.tight_layout()
+
+            try:
+                fig.savefig(outpath)
+                print(f"🖼 Triangle visualization saved to: {outpath}")
+            except Exception as e:
+                print(f"❌ Failed to save triangle visualization: {e}")
+            finally:
+                plt.close(fig)
+            
         # Globalni seznami za končno statistiko
         prostate_size_est = []
         ctcbct_distance = []
@@ -917,6 +1201,9 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
         studyItems = vtk.vtkIdList()
         shNode.GetItemChildren(shNode.GetSceneItemID(), studyItems)
         
+        #slicer.util.delayDisplay(f"[DEBUG] Starting the loops", 1000)
+
+        
         for i in range(studyItems.GetNumberOfIds()):
             study_start_time = time.time()
             start_io = time.time()
@@ -998,6 +1285,21 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                     # Detekcija točk v volumnu
                     ustvari_marker = not yesCbct  # Ustvari markerje pred poravnavo na mizo
                     grouped_points = detect_points_region_growing(volumeName, yesCbct, ustvari_marker, intensity_threshold=3000)
+                    if grouped_points is None or len(grouped_points) < 3:
+                        print(f"⚠️ Volume {volumeName} doesn't have enough points for registration. Points: {len(grouped_points)}")
+                        continue
+                    if not yesCbct:
+                        
+                        # loči koordinate in intenzitete
+                        coords_only = [pt for pt, _ in grouped_points]
+                        intensities = [intensity for _, intensity in grouped_points]
+
+                        # permutiraj koordinate (npr. zaradi boljšega ujemanja)
+                        coords_sorted = match_points(coords_only, coords_only)
+
+                        # ponovno sestavi pare (točka, intenziteta)
+                        grouped_points = list(zip(coords_sorted, intensities))
+                        
                     #print(f"Populating volume_points_dict with key ('{scan_type}', '{volumeName}')")
                     volume_points_dict[(scan_type, volumeName)] = grouped_points
                     #print(volume_points_dict)  # Check if the key is correctly added
@@ -1005,31 +1307,47 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             # Če imamo oba tipa volumna (CBCT in CT) **znotraj istega studyja**
             end_io = time.time()
             if cbct_list and ct_list:
+                fixing = fixing_end = 0
+                table1_time = table1end_time = 0
+                table2_time = table2end_time = 0
+                start_scaling = end_scaling = 0
+                start_align = end_align = 0
+                start_rotation = end_rotation = 0
+                start_translation = end_translation = 0
+                start_transform = end_transform = 0
+                study_start_time = study_end_time = 0
+                
                 ct_volume_name = ct_list[0]  # Uporabi prvi CT kot referenco
                 ct_volume_Node = find_volume_node_by_partial_name(ct_volume_name)
                 print(f"\nProcessing CT: {ct_volume_name}")
                 yesCbct = False
-                makemarkerscheck = True                               
-                result = find_table_top_z(ct_volume_name, writefilecheck, makemarkerscheck, yesCbct)
-                if result is not None:
-                    mm_offset, pixel_offset = result
-                    ct_table_found = True
-                    #print(f"✔️ Poravnava z višino mize: ΔY = {mm_offset:.2f} mm")
-                    # Dodaj ΔY k translaciji ali transformaciji po potrebi
-                else:
-                    print("⚠️ Table top not found – continue without correction on Y axis.")
-                    mm_offset = 0.0  # ali None, če želiš eksplicitno ignorirati
-                    ct_table_found = False
-                CT_offset, CT_spacing = align_cbct_to_ct(ct_volume_Node, "CT", mm_offset)                                    
+                if(tablefind):
+                    table1_time = time.time()
+                    makemarkerscheck = True                               
+                    result = find_table_top_z(ct_volume_name, writefilecheck, makemarkerscheck, yesCbct)
+                    if result is not None:
+                        mm_offset, pixel_offset = result
+                        ct_table_found = True
+                        #print(f"✔️ Poravnava z višino mize: ΔY = {mm_offset:.2f} mm")
+                        # Dodaj ΔY k translaciji ali transformaciji po potrebi
+                    else:
+                        print("⚠️ Table top not found – continue without correction on Y axis.")
+                        mm_offset = 0.0  # ali None, če želiš eksplicitno ignorirati
+                        ct_table_found = False
+                    CT_offset, CT_spacing = align_cbct_to_ct(ct_volume_Node, "CT", mm_offset)
+                    table1end_time = time.time()                                    
                      
                 
                 ct_points = [centroid for centroid, _ in volume_points_dict[("CT", ct_volume_name)]]
-                ct_points = remove_lowest_marker(ct_points) #odstrani marker v riti, če obstaja
                 print(f"CT points: {ct_points}")                
                 if len(ct_points) < 3:
                     print(f"CT volume {ct_volume_name} doesn't have enough points for registration. Points: {len(ct_points)}")
                     continue
                 else:
+                    # if len(ct_points) == 4:
+                    #     ct_points = remove_lowest_marker(ct_points) #odstrani marker v riti, če obstaja
+                    
+                    
                     for cbct_volume_name in cbct_list:
                         print(f"\nProcessing CBCT Volume: {cbct_volume_name}")                        
                         yesCbct = True
@@ -1041,31 +1359,43 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                             cumulative_matrices[key] = np.eye(4)
                         
                         fixing = time.time()
-                        makemarkerscheck = False  # Ustvari CBCT miza markerje pred poravnavo
-                        if(ct_table_found):
-                            result = find_table_top_z(cbct_volume_name, writefilecheck, makemarkerscheck, yesCbct) #!!!!!!!!!!!!!???????????? ct_volume_name
-                            if result is not None:
-                                mm_offset, pixel_offset = result
-                                #print(f"✔️ Poravnava z višino mize: ΔY = {mm_offset:.2f} mm")
-                                skupni_offset = align_cbct_to_ct(cbct_volume_node, scan_type, mm_offset, CT_offset, CT_spacing) #poravna CBCT in sporoči skupni offset                                
-                                
-                                table_shift_matrix = np.eye(4)
-                                table_shift_matrix[1, 3] = skupni_offset  # Premik po Y
-
-                                # Pomnoži obstoječo kumulativno matriko
-                                key = (studyName, cbct_volume_name)
-                                if key not in cumulative_matrices:
-                                    cumulative_matrices[key] = np.eye(4)
-
-                                cumulative_matrices[key] = np.dot(cumulative_matrices[key], table_shift_matrix)
-                                
-                            else:
-                                print("⚠️ Table top not found – continue without correction on Y axis.")
-                                mm_offset = 0.0             
+                        if(tablefind):
+                            table2_time = time.time()
+                            makemarkerscheck = False  # Ustvari CBCT miza markerje pred poravnavo
+                            if(ct_table_found):
+                                result = find_table_top_z(cbct_volume_name, writefilecheck, makemarkerscheck, yesCbct) #!!!!!!!!!!!!!???????????? ct_volume_name
+                                if result is not None:
+                                    mm_offset, pixel_offset = result
+                                    print(f"CT offset: {CT_offset}, cbct offset: {mm_offset}")
+                                    
+                                    #ne rabimo več CT_offset, le še cbct offset. 
+                                    skupni_offset = align_cbct_to_ct(cbct_volume_node, scan_type, mm_offset, CT_offset, CT_spacing) #poravna CBCT in sporoči skupni offset                                
+                                    
+                                    table_shift_matrix = np.eye(4)
+                                    table_shift_matrix[1, 3] = skupni_offset  # Premik po Y
+
+                                    # Pomnoži obstoječo kumulativno matriko
+                                    key = (studyName, cbct_volume_name)
+                                    if key not in cumulative_matrices:
+                                        cumulative_matrices[key] = np.eye(4)
+
+                                    cumulative_matrices[key] = np.dot(cumulative_matrices[key], table_shift_matrix)
+                                    
+                                else:
+                                    print("⚠️ Table top not found – continue without correction on Y axis.")
+                                    mm_offset = 0.0
+                            table2end_time = time.time()            
                                             
                         cbct_points = [centroid for centroid, _ in volume_points_dict[("CBCT", cbct_volume_name)]] #zastareli podatki
                         cbct_points_array = np.array(cbct_points)  # Pretvorba v numpy array
                         
+                        if len(cbct_points) != len(ct_points):
+                            if(len(cbct_points) + 1 == len(ct_points)):
+                                ct_points = remove_lowest_marker(ct_points) #odstrani marker v riti, če obstaja
+                            else:
+                                print(f"Neujemajoče število točk! CBCT: {len(cbct_points)}, CT: {len(ct_points)}")
+                                continue  
+                        
                         # print_orientation(ct_volume_name)
                         # print_orientation(cbct_volume_name)
                         
@@ -1102,10 +1432,14 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                             
                             # Sortiramo točke po Z-koordinati (ali X/Y, če raje uporabljaš drugo os)
                             cbct_points_sorted = cbct_points_array[np.argsort(cbct_points_array[:, 2])]
-
-                            # Razdalje med CT in CBCT (SORTIRANE točke!)
-                            d_ct_cbct = np.linalg.norm(cbct_points_sorted - ct_points, axis=1)
-                            distances_ct_cbct.append(d_ct_cbct)
+                            
+                            # Razdalje med CT in CBCT (SORTIRANE točke!) 
+                            if cbct_points_sorted.shape[0] != len(ct_points):
+                                print(f"⚠️ Število točk CBCT ({cbct_points_sorted.shape[0]}) != CT ({len(ct_points)}), preskakujem izračun.")
+                            else:
+                                d_ct_cbct = np.linalg.norm(cbct_points_sorted - ct_points, axis=1)
+                                distances_ct_cbct.append(d_ct_cbct)
+                            
 
                             # Razdalje med točkami znotraj SORTIRANIH cbct_points
                             d_ab = np.linalg.norm(cbct_points_sorted[0] - cbct_points_sorted[1])
@@ -1136,11 +1470,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                             scaling_factors = compute_scaling_stddev(cbct_points, ct_points)
                             #print("Scaling factors: ", scaling_factors)
                             cbct_points = compute_scaling(cbct_points, scaling_factors)
-                            
-                        
-                        
-                            
-                            
+    
                         end_scaling = time.time()
                         start_align = time.time()
                         initial_error = np.mean(np.linalg.norm(np.array(cbct_points) - np.array(ct_points), axis=1))
@@ -1193,16 +1523,27 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                         
                         
                         combined_matrix = np.eye(4)
-                        
-                        if scaling_factors is not None:
-                            scaling_matrix = np.diag([scaling_factors[0], scaling_factors[1], scaling_factors[2], 1.0])
-                            combined_matrix = np.dot(scaling_matrix, combined_matrix)
-                        
-                        if chosen_translation_vector is not None:
-                            combined_matrix[:3, 3] = chosen_translation_vector + transvector # glavna poravnava, sistematični y shift in groba poravnava
+
+                        # 1. Rotacija
                         if chosen_rotation_matrix is not None:
                             combined_matrix[:3, :3] = chosen_rotation_matrix
+
+                        # 2. Skaliranje
+                        if scaling_factors is not None:
+                            # Pomnoži rotacijo s skalirnim faktorjem po vsaki osi
+                            scaled_rotation = combined_matrix[:3, :3] * scaling_factors  # broadcasting vsake vrstice
+                            combined_matrix[:3, :3] = scaled_rotation
+
+                        # 3. Translacija
+                        if chosen_translation_vector is not None:
+                            combined_matrix[:3, 3] = chosen_translation_vector + transvector  # združena translacija
+
                         
+                        #preverjanje determinante
+                        rot_part = combined_matrix[:3, :3]
+                        det = np.linalg.det(rot_part)
+                        if not np.isclose(det, 1.0, atol=0.01):
+                            print(f"⚠️ Neortogonalna rotacija! Determinanta: {det}")
                         
                         cumulative_matrices[(studyName, cbct_volume_name)] = np.dot(cumulative_matrices[(studyName, cbct_volume_name)], combined_matrix)
 
@@ -1226,7 +1567,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                         ustvari_marker = False
                         cbct_points = [centroid for centroid, _ in detect_points_region_growing(cbct_volume_name, yesCbct, ustvari_marker, intensity_threshold=3000)]
                         #print("Markerji po transformaciji:\n", cbct_points, ct_points)
-                        
+                        cbct_points = match_points(cbct_points, ct_points)
                         #popravek v x osi
                         delta_x_list = [ct[0] - cbct[0] for ct, cbct in zip(ct_points, cbct_points)]
                         mean_delta_x = np.mean(delta_x_list)
@@ -1248,6 +1589,18 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                  
                     
                         chosen_rotation_matrix = np.eye(3) #tokrat brez rotacije
+                        
+                        #### TEST ROTACIJA ########
+                        angle_deg = 0
+                        angle_rad = np.deg2rad(angle_deg)
+
+                        chosen_rotation_matrix = np.array([
+                            [np.cos(angle_rad), -np.sin(angle_rad), 0],
+                            [np.sin(angle_rad),  np.cos(angle_rad), 0],
+                            [0,                 0,                 1]
+                        ])
+                        ###KONEC TESTA###
+                        
                         vtk_transform = create_vtk_transform(chosen_rotation_matrix, fine_shift, studyName, cbct_volume_name) #Tukaj se tudi izpiše transformacijska matrika
 
                         # 🔄 Pripni transformacijo
@@ -1263,14 +1616,70 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                         slicer.mrmlScene.RemoveNode(transform_node)
 
                         
-                        ustvari_marker = True
-                        cbct_points = [centroid for centroid, _ in detect_points_region_growing(cbct_volume_name, yesCbct, ustvari_marker, intensity_threshold=3000)]
+                        table_shift_matrix_ct = np.eye(4)
+                        table_shift_matrix_ct[1, 3] = CT_offset  # ali skupni_offset, če je potreben simetričen premik
+                        
+                        cumulative_matrices[(studyName, cbct_volume_name)] = np.dot(
+                            table_shift_matrix,
+                            cumulative_matrices[(studyName, cbct_volume_name)]
+                        )
+                        
+                        #ustvari_marker = True
+                        cbct_points = [centroid for centroid, _ in detect_points_region_growing(cbct_volume_name, yesCbct, applymarkers, intensity_threshold=3000)]
                         print(f"Fine correction shifts: ΔX={fine_shift[0]:.2f} mm, ΔY={fine_shift[1]:.2f} mm, ΔZ={fine_shift[2]:.2f} mm")
 
                         #shrani transformacijsko matriko v datoteko
-                        save_transform_matrix(cumulative_matrices[(studyName, cbct_volume_name)], studyName, cbct_volume_name, ct_table_found)
-                        
+                        save_transform_matrix(cumulative_matrices[(studyName, cbct_volume_name)], studyName, cbct_volume_name)
                         
+                        if save_as_dicom:
+                            # Apply transform to CT
+                            logging.info(f"[{studyName}] Start applying transform to CT volume.")
+                            slicer.util.delayDisplay(f"[DEBUG] Start transform on CT", 1000)
+                            apply_cumulative_transform_to_volume(ct_volume_Node, cumulative_matrices[(studyName, cbct_volume_name)])
+                            
+                            # Convert RTSTRUCT to SegmentationNode if needed
+                            logging.info(f"[{studyName}] Getting segmentation nodes from RTSTRUCT.")
+                            #slicer.util.delayDisplay(f"[DEBUG] Start segmentation conversion", 1000)
+                            #seg_nodes = convert_rtstruct_to_segmentation_nodes(ct_volume_Node)
+                            seg_node = convert_all_models_to_segmentation(ct_volume_name, prefix="Imported_")
+                            #new_seg_node = slicer.util.getNode(new_seg_name)
+                            apply_cumulative_transform_to_segmentation(seg_node, cumulative_matrices[(studyName, cbct_volume_name)])
+
+                            plugin = DicomRtImportExportPlugin.DicomRtImportExportPluginClass()
+                            shNode = slicer.vtkMRMLSubjectHierarchyNode.GetSubjectHierarchyNode(slicer.mrmlScene)
+                            ct_itemID = shNode.GetItemByDataNode(ct_volume_Node)
+                            seg_itemID = shNode.GetItemByDataNode(seg_node)
+
+                            
+                            
+                            cbct_item = shNode.GetItemByDataNode(cbct_volume_node)
+                            cbct_date = shNode.GetItemAttribute(cbct_item, "DICOM.SeriesDate") or "unknownDate"
+
+                            #cbct_date = cbct_volume_node.GetAttribute("DICOM.AcquisitionDate") or "unknownDate"
+                            results_base = os.path.join(os.path.dirname(__file__), "Rezultati")
+                            study_folder = studyName.replace('^', '_')
+                            study_dir = os.path.join(results_base, study_folder)
+                            os.makedirs(study_dir, exist_ok=True)  
+
+                            export_dir = os.path.join(study_dir, f"{cbct_date}_DICOM")
+                            os.makedirs(export_dir, exist_ok=True)
+                            
+                            exportables = []
+                            exportables += plugin.examineForExport(ct_itemID)
+                            exportables += plugin.examineForExport(seg_itemID)
+                            for exp in exportables:
+                                 # Kopiraj podatke iz volumetričnega noda v exportable
+                                if ct_volume_Node.GetAttribute("DICOM.PatientName"):
+                                    exp.setTag("0010,0010", ct_volume_Node.GetAttribute("DICOM.PatientName"))  # PatientName
+                                if ct_volume_Node.GetAttribute("DICOM.PatientID"):
+                                    exp.setTag("0010,0020", ct_volume_Node.GetAttribute("DICOM.PatientID"))    # PatientID
+                                if ct_volume_Node.GetAttribute("DICOM.StudyInstanceUID"):
+                                    exp.setTag("0020,000D", ct_volume_Node.GetAttribute("DICOM.StudyInstanceUID"))  # StudyInstanceUID
+                                exp.directory = export_dir
+                                print(f"[{studyName}] ✅ Export successful")
+                            plugin.export(exportables)
+
+                                            
                         # 📏 Izračun napake
                         errors = [np.linalg.norm(cbct - ct) for cbct, ct in zip(cbct_points, ct_points)]
                         mean_error = np.mean(errors)
@@ -1282,6 +1691,66 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                             diff = np.array(cbct) - np.array(ct)
                             print(f"Specific marker errors {i+1}: ΔX={diff[0]:.2f} mm, ΔY={diff[1]:.2f} mm, ΔZ={diff[2]:.2f} mm")
                         
+                        ct_pts = np.array(ct_points)
+                        cbct_pts = np.array(cbct_points)
+
+                        if len(ct_pts) == 3 and len(cbct_pts) == 3:
+                            sim = triangle_similarity(ct_pts, cbct_pts)
+                            print("\n📐 Triangle similarity analysis:")
+                            print(f"Side length differences (mm): {sim['side_length_diff_mm']}")
+                            print(f"Angle differences (deg): {sim['angle_diff_deg']}")
+                            print(f"Mean length error: {sim['mean_length_error_mm']:.2f} mm")
+                            print(f"Mean angle error: {sim['mean_angle_error_deg']:.2f}°")
+                            print(f"Triangle Similarity Ratio (TSR): {sim['TSR']:.3f}")
+                        else:
+                            print("⚠️ Za primerjavo trikotnikov potrebne točno 3 točke.")
+                        
+                        if writefilecheck:
+
+ 
+                            errorsfile = os.path.join(study_dir, f"{cbct_date}_errors.csv")
+
+                            with open(errorsfile, mode='a', newline='') as file:
+                                writer = csv.writer(file)
+
+                                # Glava za študijo
+                                writer.writerow(["Study", studyName])
+                                writer.writerow(["Total Individual Errors (mm)"])
+                                
+                                for error in errors:
+                                    writer.writerow(["", f"{error:.2f}"])
+
+                                writer.writerow(["Average Error (mm)", f"{mean_error:.2f}"])
+
+                                # Specifične napake markerjev
+                                writer.writerow(["Specific Marker Errors"])
+                                writer.writerow(["Marker", "ΔX (mm)", "ΔY (mm)", "ΔZ (mm)"])
+
+                                for i, (cbct, ct) in enumerate(zip(cbct_points, ct_points)):
+                                    diff = np.array(cbct) - np.array(ct)
+                                    writer.writerow([f"Marker {i+1}", f"{diff[0]:.2f}", f"{diff[1]:.2f}", f"{diff[2]:.2f}"])
+
+                                if len(cbct_points) == 3 and len(ct_points) == 3:
+                                    writer.writerow([])
+                                    writer.writerow(["Triangle Similarity"])
+                                    writer.writerow(["Side Length Diff (mm)", *[f"{v:.2f}" for v in sim["side_length_diff_mm"]]])
+                                    writer.writerow(["Angle Diff (deg)", *[f"{v:.2f}" for v in sim["angle_diff_deg"]]])
+                                    writer.writerow(["Mean Length Error (mm)", f"{sim['mean_length_error_mm']:.2f}"])
+                                    writer.writerow(["Mean Angle Error (deg)", f"{sim['mean_angle_error_deg']:.2f}"])
+                                    writer.writerow(["Triangle Similarity Ratio (TSR)", f"{sim['TSR']:.3f}"])
+                                    writer.writerow([])
+                                 
+                            
+                            graphs_dir = os.path.join(results_base, study_folder)
+                            os.makedirs(graphs_dir, exist_ok=True)
+
+                            pngfile = os.path.join(graphs_dir, f"{cbct_date}_trikotnika.png")
+                            save_triangle_visualization(np.array(ct_points), np.array(cbct_points), pngfile)
+                                
+
+                            #print("Prostate size file written at ", prostate_size_file)
+                        
+                        
                         
                 
             else:
@@ -1292,6 +1761,7 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
             timing_data = {
                 "IO": end_io - start_io,
                 "Fixing": fixing_end - fixing,
+                "Table": ((table1end_time - table1_time)+(table2end_time - table2_time)) if tablefind else 0,
                 "Scaling": end_scaling - start_scaling,
                 "CentroidAlign": end_align - start_align,
                 "Rotation": end_rotation - start_rotation,
@@ -1300,6 +1770,10 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
                 "Total": study_end_time - study_start_time}
             update_timing_csv(timing_data, studyName)
             print(f"Timing data for {studyName}: {timing_data}")
+    
+
+
+
 
         # Izpis globalne statistike
         
@@ -1336,5 +1810,4 @@ class MyTransformModuleLogic(ScriptedLoadableModuleLogic):
         minutes = int(elapsed_time // 60)
         seconds = elapsed_time % 60
 
-        print(f"Execution time: {minutes} minutes and {seconds:.6f} seconds")
-        
+        print(f"Execution time: {minutes} minutes and {seconds:.6f} seconds")