matrix depends on chip resolutions (e. g. binning)

1 files changed, 13 insertions, 5 deletions

diff --git a/camera_calib.c b/camera_calib.c
index 78362bd..804c40d 100644
--- a/camera_calib.c
+++ b/camera_calib.c
@@ -492,7 +492,7 @@ static void init_undistort_map_scalar(struct cam_calib_data_t *data) {
 	float x_corr, y_corr;
 	struct lense_undistort_coord_t *map;
 	const int scale_fact = (1 << (data->calib_map_scale_fact));
-	
+	float x_resolution_scaling, y_resolution_scaling;		// camera_matrix needs to be scaled depending on the camera resolution
 	
 	fov_x_start = data->fov_x_start;
 	fov_x_end = data->fov_x_end;
@@ -505,10 +505,18 @@ static void init_undistort_map_scalar(struct cam_calib_data_t *data) {
 	p2 = data->dist_coeff.p2;
 	k3 = data->dist_coeff.k3;
 	
-	fx = data->camera_matrix.a11;
-	fy = data->camera_matrix.a22;
-	cx = data->camera_matrix.a13;
-	cy = data->camera_matrix.a23;
+	/* 
+	 * While the distortion coefficients are the same regardless of the camera resolution 
+	 * used, the camera matrix coefficients should be scaled along with the current 
+	 * resolution from the calibrated resolution. (see OpenCV documentation)
+	 */
+	x_resolution_scaling = (fov_x_end - fov_x_start + 1) / data->tot_width;
+	y_resolution_scaling = (fov_y_end - fov_y_start + 1) / data->tot_height;
+	
+	fx = data->camera_matrix.a11/x_resolution_scaling;
+	fy = data->camera_matrix.a22/y_resolution_scaling;
+	cx = data->camera_matrix.a13/x_resolution_scaling;
+	cy = data->camera_matrix.a23/y_resolution_scaling;
 	
 	map = data->calib_map;