Merge pull request #193 from braden6521/sofast_fixed_cal_update

Sofast fixed cal update

opencsp/app/sofast/lib/BlobIndex.py

@@ -6,6 +6,7 @@
 
 from opencsp.common.lib.geometry.Vxy import Vxy
 from opencsp.common.lib.geometry.LoopXY import LoopXY
+from opencsp.common.lib.render.lib.AbstractPlotHandler import AbstractPlotHandler
 from opencsp.common.lib.tool import log_tools as lt
 
 
@@ -19,7 +20,30 @@ class Step(Enum):
     LEFT_OR_UP = -1
 
 
-class BlobIndex:
+class DebugBlobIndex:
+    """Debug class for BlobIndex
+
+    Parameters
+    ----------
+        debug_active : bool
+            Flag to turn on debugging, default false
+        figures : list[Figure]
+            Container to hold debug figures
+        bg_image : ndarray
+            2d ndarray, by default None. Background image (i.e. the image with the blobs present)
+            to plot in debugging plots. If None, background image is not plotted.
+        name : str
+            Name of current instance to prepend to plot titles
+    """
+
+    def __init__(self):
+        self.debug_active: bool = False
+        self.figures: list = []
+        self.bg_image: np.ndarray = None
+        self.name: str = ''
+
+
+class BlobIndex(AbstractPlotHandler):
     """Class containing blob indexing algorithms to assign indices to blobs in a rough grid pattern.
     X/Y axes correspond to image axes; +x is to right, +y is down. Class takes in points (in units
     of pixels) that have been previously found with a blob detector and attempts to assign all found
@@ -51,6 +75,8 @@ def __init__(self, points: Vxy, x_min: int, x_max: int, y_min: int, y_max: int)
         x_min/x_max/y_min/y_max : int
             Expected min/max of blob indices in x/y directions
         """
+        super().__init__()
+
         self._points = points
 
         self._num_pts = len(points)
@@ -70,14 +96,18 @@ def __init__(self, points: Vxy, x_min: int, x_max: int, y_min: int, y_max: int)
         """Ratio of point distances: (perpendicular to axis) / (along axis) used to search for points. Default 3.0"""
         self.apply_filter: bool = False
         """To filter bad points (experimental, not implemented yet). Default False"""
+        self.debug: DebugBlobIndex = DebugBlobIndex()
+        """BlobIndex debug object"""
+        self.shape_yx_data_mat: tuple[int, int] = (y_max - y_min + 1, x_max - x_min + 1)
+        """The yx shape of the internal data matrix that holds the point pixel locations and xy indices"""
 
         self._offset_x = -x_min  # index
         self._offset_y = -y_min  # index
         idx_x_vec = np.arange(x_min, x_max + 1)  # index
         idx_y_vec = np.arange(y_min, y_max + 1)  # index
         self._idx_x_mat, self._idx_y_mat = np.meshgrid(idx_x_vec, idx_y_vec)  # index
-        self._points_mat = np.zeros((y_max - y_min + 1, x_max - x_min + 1, 2)) * np.nan  # pixels
-        self._point_indices_mat = np.zeros((y_max - y_min + 1, x_max - x_min + 1)) * np.nan  # index
+        self._points_mat = np.zeros(self.shape_yx_data_mat + (2,)) * np.nan  # pixels
+        self._point_indices_mat = np.zeros(self.shape_yx_data_mat) * np.nan  # index
 
     def _get_assigned_point_indices(self) -> np.ndarray[int]:
         """Returns found point indices"""
@@ -238,7 +268,6 @@ def _find_nearest_in_direction(
             mask = np.logical_and(unassigned_deltas.y > 0, unassigned_deltas.y > (2 * np.abs(unassigned_deltas.x)))
             idx_x_out = idx_x
             idx_y_out = idx_y + 1
-        # Down
         elif direction == 'down':
             mask = np.logical_and(unassigned_deltas.y < 0, -unassigned_deltas.y > (2 * np.abs(unassigned_deltas.x)))
             idx_x_out = idx_x
@@ -406,10 +435,42 @@ def run(self, pt_known: Vxy, x_known: int, y_known: int) -> None:
         x/y_known : int
             XY indies of known points
         """
+        # Plot all input blobs
+        if self.debug.debug_active:
+            fig = plt.figure()
+            self._register_plot(fig)
+            if self.debug.bg_image is not None:
+                plt.imshow(self.debug.bg_image)
+            self.plot_all_points()
+            plt.title('1: All found dots in image' + self.debug.name)
+            self.debug.figures.append(fig)
+
         # Assign center point
         self._assign_center(pt_known, x_known, y_known)
+
+        # Plot assigned known center point
+        if self.debug.debug_active:
+            fig = plt.figure()
+            self._register_plot(fig)
+            if self.debug.bg_image is not None:
+                plt.imshow(self.debug.bg_image)
+            self.plot_assigned_points_labels(labels=True)
+            plt.title('2: Locations of known, center dot' + self.debug.name)
+            self.debug.figures.append(fig)
+
         # Find 3x3 core point block
         self._find_3x3_center_block(x_known, y_known)
+
+        # Plot 3x3 core block
+        if self.debug.debug_active:
+            fig = plt.figure()
+            self._register_plot(fig)
+            if self.debug.bg_image is not None:
+                plt.imshow(self.debug.bg_image)
+            self.plot_assigned_points_labels(labels=True)
+            plt.title('3: Locations of 3x3 center block' + self.debug.name)
+            self.debug.figures.append(fig)
+
         # Extend rows
         prev_num_unassigned = self._num_unassigned()
         for idx in range(self.max_num_iters):
@@ -429,8 +490,18 @@ def run(self, pt_known: Vxy, x_known: int, y_known: int) -> None:
                 break
             prev_num_unassigned = cur_num_unassigned
 
-    def plot_points_labels(self, labels: bool = False) -> None:
-        """Plots points and labels
+        # Plot all found points
+        if self.debug.debug_active:
+            fig = plt.figure()
+            self._register_plot(fig)
+            if self.debug.bg_image is not None:
+                plt.imshow(self.debug.bg_image)
+            self.plot_points_connections()
+            plt.title('4: Row assignments' + self.debug.name)
+            self.debug.figures.append(fig)
+
+    def plot_assigned_points_labels(self, labels: bool = False) -> None:
+        """Plots all assigned points with [optionally] labels
 
         Parameters
         ----------
@@ -444,6 +515,10 @@ def plot_points_labels(self, labels: bool = False) -> None:
             ):
                 plt.text(*pt.data, f'({x:.0f}, {y:.0f})')
 
+    def plot_all_points(self) -> None:
+        """Plots all points"""
+        plt.scatter(*self._points.data, color='red')
+
     def plot_points_connections(self, labels: bool = False) -> None:
         """Plots points and connections for rows/collumns
 
@@ -497,6 +572,22 @@ def get_data(self) -> tuple[Vxy, Vxy]:
         points = Vxy((x_pts[mask_assigned], y_pts[mask_assigned]))
         return points, indices
 
+    def pts_index_to_mat_index(self, pts_index: Vxy) -> tuple[np.ndarray, np.ndarray]:
+        """Returns corresponding matrix indices (see self.get_data_mat) given point
+        indices (assigned x/y indies of points.)
+
+        Parameters
+        ----------
+        pts_index : Vxy
+            Assigned point xy indices, length N.
+
+        Returns
+        -------
+        x, y
+            Length N 1d arrays of corresponding data matrix indices (see self.get_data_mat)
+        """
+        return pts_index.x - self._offset_x, pts_index.y - self._offset_y
+
     def get_data_in_region(self, loop: LoopXY) -> tuple[Vxy, Vxy]:
         """Returns found points and indices within given region
 

opencsp/app/sofast/lib/CalibrateSofastFixedDots.py

@@ -140,7 +140,7 @@ def __init__(
         self._dot_image_points_indices: Vxy
         self._dot_image_points_indices_x: ndarray
         self._dot_image_points_indices_y: ndarray
-        self._dot_points_xyz_mat = np.ndarray((x_max - x_min + 1, y_max - y_min + 1, 3)) * np.nan
+        self._dot_points_xyz_mat = np.ndarray((y_max - y_min + 1, x_max - x_min + 1, 3)) * np.nan
         self._num_dots: int
         self._rots_cams: list[Rotation] = []
         self._vecs_cams: list[Vxyz] = []

opencsp/app/sofast/lib/DefinitionEnsemble.py

@@ -4,9 +4,11 @@
 from copy import deepcopy
 import json
 
+import matplotlib.pyplot as plt
 import numpy as np
 from scipy.spatial.transform import Rotation
 
+from opencsp.app.sofast.lib.DefinitionFacet import DefinitionFacet
 from opencsp.common.lib.geometry.Vxyz import Vxyz
 from opencsp.common.lib.tool import hdf5_tools
 
@@ -164,6 +166,14 @@ def load_from_hdf(cls, file: str, prefix: str = '') -> 'DefinitionEnsemble':
         v_centroid_ensemble = Vxyz(data['v_centroid_ensemble'])
         return cls(v_facet_locations, r_facet_ensemble, ensemble_perimeter, v_centroid_ensemble)
 
+    def plot_facet_corners_xy_proj(self, facets: list[DefinitionFacet]) -> None:
+        """Plots the xy projection of all facet corners given accompanying facet definitions"""
+        for idx_facet, (facet, R, T) in enumerate(zip(facets, self.r_facet_ensemble, self.v_facet_locations)):
+            facet: DefinitionFacet
+            corners_cur_facet = facet.v_facet_corners
+            corners_cur_ensemble: Vxyz = corners_cur_facet.rotate(R) + T
+            plt.scatter(corners_cur_ensemble.x, corners_cur_ensemble.y, label=f'Facet {idx_facet:d}')
+
 
 def _Vxyz_to_dict(V: Vxyz) -> dict:
     d = {'x': V.x.tolist(), 'y': V.y.tolist(), 'z': V.z.tolist()}

opencsp/app/sofast/lib/ParamsOpticGeometry.py

@@ -8,10 +8,19 @@ class ParamsOpticGeometry(hdf5_tools.HDF5_IO_Abstract):
     """Parameter dataclass for processing optic geometry"""
 
     perimeter_refine_axial_search_dist: float = 50.0
+    """The length of the search box (along the search direction) to use when finding optic
+    perimeter. Units pixels. Default 50.0"""
     perimeter_refine_perpendicular_search_dist: float = 50.0
+    """The half-width of the search box (perpendicular to the search direction) to use when finding
+    optic perimeter. Units pixels. Default 50.0"""
     facet_corns_refine_step_length: float = 10.0
+    """The length of the search box (along the search direction) to use when refining facet corner
+    locations (when processing a facet ensemble). Units pixels. Default 10.0"""
     facet_corns_refine_perpendicular_search_dist: float = 10.0
+    """The half-width of the search box (perpendicular to the search direction) to use when
+    refining facet corner locations (when processing a facet ensemble). Units pixels. Default 10.0"""
     facet_corns_refine_frac_keep: float = 0.5
+    """The fraction of pixels to consider within search box when finding optic edges. Default 0.5"""
 
     def save_to_hdf(self, file: str, prefix: str = ''):
         """Saves data to given HDF5 file. Data is stored in PREFIX + ParamsOpticGeometry/...