Save transM into CSV file. BA process changed

parallax/bundle_adjustmnet.py

@@ -24,7 +24,7 @@ def __init__(self, model, file_path):
 
     def _parse_csv(self):
         self.df = pd.read_csv(self.file_path)
-        self._remove_duplicates()
+        #self._remove_duplicates()
         self._average_3D_points()
 
         self._set_camera_list()
@@ -47,7 +47,6 @@ def _set_observations(self):
         # Create a mapping from camera IDs to indices
         camera_id_to_index = {str(camera_id): idx for idx, camera_id in enumerate(self.list_cameras)}
 
-
         # Iterate through the DataFrame to collect observations
         for _, row in self.df.iterrows():
             cam0, pt0 = str(row['cam0']), row['pt0']
@@ -82,9 +81,13 @@ def _average_3D_points(self):
         # Merge the averaged columns back into the original DataFrame
         self.df = self.df.merge(grouped, on='ts_local_coords', how='left')
 
+        # Create a mapping of ts_local_coords to index in the averaged points
+        self.df['point_index'] = self.df.groupby('ts_local_coords').ngroup()
+
         # Write the updated DataFrame back to the CSV file
         self.df.to_csv(self.file_path, index=False)
 
+
     def _remove_duplicates(self):
         # Drop duplicate rows based on 'ts_local_coords', 'global_x', 'global_y', 'global_z' columns
         logger.debug(f"Original rows: {self.df.shape[0]}")
@@ -182,22 +185,31 @@ def optimize(self, print_result=True):
         self.opt_points = opt_params[12 * n_cams:].reshape(n_pts, 3)
 
         if print_result:
-            print(f"\nOptimization completed.")
+            print(f"\n************** Optimization completed. **************************")
             # Compute initial residuals
             initial_residuals = self.residuals(initial_params)
             initial_residuals_sum = np.sum(initial_residuals**2)
             average_residual = initial_residuals_sum / len(self.bal_problem.observations)
-            print(f"Before BA, Average residual: {average_residual}")
+            print(f"** Before BA, Average residual of reproj: {np.round(average_residual, 2)} **")
 
             # Compute Optimize residuals
             opt_residuals = self.residuals(opt_params)
             opt_residuals_sum = np.sum(opt_residuals**2)
             average_residual = opt_residuals_sum / len(self.bal_problem.observations)
-            print(f"After  BA, Average residual: {average_residual}")
+            print(f"** After  BA, Average residual of reproj: {np.round(average_residual, 2)} **")
+            print(f"******************************************************************")
 
             logger.debug(f"Optimized camera parameters: {self.opt_camera_params}")
 
             for i in range(len(self.bal_problem.points)):
                 logger.debug(f"\nPoint {i}")
                 logger.debug(f"org : {self.bal_problem.points[i]}")
-                logger.debug(f"opt : {self.opt_points[i]}")
+                logger.debug(f"opt : {self.opt_points[i]}")
+
+        # Map optimized points to the original DataFrame rows
+        opt_points_df = pd.DataFrame(self.opt_points, columns=['opt_x', 'opt_y', 'opt_z'])
+        self.bal_problem.df = self.bal_problem.df.join(opt_points_df, on='point_index', rsuffix='_opt')
+
+        # Save the updated DataFrame to the CSV file
+        self.bal_problem.df.to_csv(self.bal_problem.file_path, index=False)
+        logger.info(f"Optimized points saved to {self.bal_problem.file_path}")

parallax/coords_transformation.py

@@ -64,8 +64,9 @@ def func(self, x, measured_pts, global_pts, reflect_z=False):
     def avg_error(self, x, measured_pts, global_pts, reflect_z=False):
         """Calculates the total error for the optimization."""
         error_values = self.func(x, measured_pts, global_pts, reflect_z)
-        ave_error = np.sum(error_values**2)/len(error_values)
-        return ave_error
+        mean_squared_error = np.mean(error_values**2)
+        average_error = np.sqrt(mean_squared_error)
+        return average_error
 
     def fit_params(self, measured_pts, global_pts):
         """Fits parameters to minimize the error defined in func"""
@@ -87,12 +88,14 @@ def fit_params(self, measured_pts, global_pts):
         if avg_error1 < avg_error2:
             rez = res1[0]
             R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=False)
+            avg_err = avg_error1
         else:
             rez = res2[0]
             R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=True)
+            avg_err = avg_error1
 
         #origin = rez[3:]
         origin = rez[3:6]
         scale = rez[6:]
         #return origin, R  # translation vector and rotation matrix
-        return origin, R, scale  # translation vector, rotation matrix, and scaling factors
+        return origin, R, scale, avg_err  # translation vector, rotation matrix, and scaling factors