added the ability to do a self calibrating bundle adjustment

pyCamSet/calibration/camera_calibrator.py

@@ -1,4 +1,4 @@
-
+from copy import copy
 import cv2
 import matplotlib.pyplot as plt
 from multiprocessing import cpu_count
@@ -244,14 +244,10 @@ def run_stereo_calibration(
         threads = threads,
     )
 
+    param_handler.camset = optimised_cams
     # optimised_cams = param_handler.camset
     # outlier_rejection(optimisation.fun.reshape((-1,2)), param_handler)
 
-    param_handler.camset = optimised_cams
-    # optimised_cams.set_calibration_history(
-    #     optimisation_results=optimisation,
-    #     param_handler=param_handler,
-    # )
 
     if save:
         if floc is not None:

pyCamSet/calibration_targets/target_detections.py

@@ -22,12 +22,15 @@ def __init__(self,  keys: np.ndarray|list|None=None, image_points: np.ndarray|li
             keys = np.array(keys)
         if not isinstance(image_points, np.ndarray) and image_points is not None:
             image_points = np.array(image_points)
+
         if keys is not None and image_points is not None:
             assert len(keys) == len(image_points), "Detected keys must be the same length as detected points"
             self.keys = keys
             self.image_points = image_points
             self.has_data = True
             self.data_len = len(keys)
+            if keys.shape[0] == 0:
+                self.has_data = False
         elif keys is None and image_points is None:
             self.has_data = False
         else:
@@ -234,9 +237,12 @@ def add_detection(self, cam_name, im_num, detection: ImageDetection) -> None:
                 keys = detection.keys[..., None]
             else:
                 keys = detection.keys
-            observation = np.concatenate(
-                [np.ones((detection.data_len, 2))*[ind, im_num], keys, detection.image_points]
-                , axis=1)
+            try:
+                observation = np.concatenate(
+                    [np.ones((detection.data_len, 2))*[ind, im_num], keys, detection.image_points]
+                    , axis=1)
+            except:
+                print(detection.image_points)
             self._update_buffer.append(observation)
 
     def _glomp_buffer(self) -> None:

pyCamSet/optimisation/abstract_function_blocks.py

@@ -690,9 +690,6 @@ def make_param_struct(
         key_type.PER_KEY:max_keys,
     }
     #TODO account for the mod function here, modifying the numbers of params
-
-    # find all of the unique instances of link indicators
-    #TODO use memory location of link indicators to compare (is) I think
     unique_link_inds = []
     for fb in function_blocks:
         if not fb.params in unique_link_inds:

pyCamSet/optimisation/optimisation_handling.py

@@ -93,10 +93,13 @@ def run_bundle_adjustment(param_handler: TemplateBundleHandler,
         # tr_solver='lsmr',
         jac= bundle_jac if bundle_jac is not None else "2-point", #pass the function for the jacobian if it exists
         max_nfev=param_handler.problem_opts["max_nfev"],
+        # loss = "cauchy"
         # x_scale='jac',
     )
     end = time.time()
 
+
+
     final_euclid = np.mean(np.linalg.norm(np.reshape(optimisation.fun, (-1, 2)), axis=1))
     logging.info(f'Final Euclidean error: {final_euclid:.2f} px')
     logging.info(f'Optimisation took {end - start: .2f} seconds.')
@@ -108,6 +111,7 @@ def run_bundle_adjustment(param_handler: TemplateBundleHandler,
     camset = param_handler.get_camset(optimisation.x)
     camset.set_calibration_history(optimisation, param_handler)
 
+
     init_err = loss_fn(optimisation.x)
     init_euclid = np.mean(np.linalg.norm(np.reshape(init_err, (-1, 2)), axis=1))
     logging.info(f"Check test with a result of {init_euclid:.2f}")

pyCamSet/optimisation/standard_bundle_handler.py

@@ -8,6 +8,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 from scipy.optimize import least_squares
+import pyvista as pv
 
 from typing import TYPE_CHECKING
 from pyCamSet.optimisation.template_handler import TemplateBundleHandler, DEFAULT_OPTIONS
@@ -228,5 +229,28 @@ def get_camset(self, x, return_pose=False) -> CameraSet | tuple[CameraSet, np.nd
             ch.n_e4x4_flat_INPLACE(p, pn)
 
         return new_cams, ps
+
+    def special_plots(self, x):
+        og_data = self.target.point_data.reshape((-1,3))
+        n_points = self.flat_point_data.shape[0]
+        final_data = x[-3*n_points:].reshape((-1,3))
+        ref_data = self.target.point_data.reshape((-1,3))
+        find_tform =  gu.make_4x4h_tform(*ch.n_estimate_rigid_transform(final_data, ref_data))
+        final_data = gu.h_tform(final_data, find_tform)
+
+        scale = np.mean(np.linalg.norm(og_data, axis=-1)/np.linalg.norm(final_data, axis=-1))
+        final_data *= scale
+
+        diff = (og_data - final_data) * 1000
+        print(f"found a mean difference of {np.mean(np.linalg.norm(diff, axis=-1)):.2f} mm")
+        s = pv.Plotter()
+        s.add_arrows(og_data, diff, mag=0.1)
+        s.add_mesh(pv.PolyData(og_data), color='r')
+        s.add_mesh(pv.PolyData(final_data), color='g')
+        s.show()
+
+
+
+
 
 

pyCamSet/optimisation/template_handler.py

@@ -26,7 +26,7 @@
     'ref_cam':0,
     'ref_pose':0,
     'outliers':'ask',
-    'max_nfev':50,
+    'max_nfev':100,
 }
 class TemplateBundlePrimitive:
     """
@@ -271,6 +271,7 @@ def find_and_exclude_transform_outliers(self, per_im_error):
                     print(f"Outliers detected in iteration {num_loops}.")
                     user_in = input("Do you wish to remove these outlier poses: \n y/n: ")
                 if user_in == 'y':
+                    # max_loc = np.argmax(per_im_error[outlier_inds])
                     self.missing_poses[outlier_inds] = True
                 if user_in == 'n':
                     cyclic_outlier_detection = False

pyCamSet/utils/visualisation.py

@@ -129,7 +129,7 @@ def visualise_calibration(
     detection = param_handler.get_detection()
     cams, poses = param_handler.get_camset(o_results['x'], return_pose=True)
 
-    cluster_plot([o_results['err']], alphas=[0.1])
+    cluster_plot([o_results['err']], alphas=[0.2])
 
     # the coverage for each camera
     n_cams = cams.get_n_cams()