Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

update undistort to use video camera intrinsics (#2363)

Browse Source
This commit is contained in:
Kirk Scheper
2018-12-03 21:55:01 +01:00
committed by Gautier Hattenberger
parent 9cceab1c49
commit b2667da42e
5 changed files with 22 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop2_undistort_front.xml
+3 -4
View File
@@ -52,10 +52,9 @@
</module>
<module name="cv_undistort_image">
<define name="UNDISTORT_MIN_X_NORMALIZED" value="-2.0"/>
<define name="UNDISTORT_MAX_X_NORMALIZED" value="2.0"/>
<define name="UNDISTORT_DHANE_K" value="1.25"/>
<define name="UNDISTORT_CAMERA" value="front_camera"/>
<define name="UNDISTORT_MIN_X_NORMALIZED" value="-2.0"/>
<define name="UNDISTORT_MAX_X_NORMALIZED" value="2.0"/>
<define name="UNDISTORT_CAMERA" value="front_camera"/>
</module>
<module name="video_rtp_stream">
conf/modules/cv_undistort_image.xml
+5 -12
View File
@@ -12,15 +12,9 @@
</description>
<define name="UNDISTORT_MIN_X_NORMALIZED" value="-2.0" description="Minimal normalized x-coordinate to be used for the undistortion"/>
<define name="UNDISTORT_MAX_X_NORMALIZED" value="2.0" description="Maximal normalized x-coordinate to be used for the undistortion"/>
<define name="UNDISTORT_DHANE_K" value="1.25" description="The single parameter of the Dhane invertible (un)distortion model."/>
<define name="UNDISTORT_FPS" value="0" description="The (maximum) frequency to run the calculations at. If zero, it will max out at the camera frame rate"/>
<define name="UNDISTORT_CAMERA" value="bottom_camera|front_camera" description="The V4L2 camera device that is used for the calculations"/>
<define name="UNDISTORT_CENTER_RATIO" value="1.0" description="If smaller than 1 only generate pixels for the center_ratio times the min_x to max_x interval. This makes undistortion quicker, but for a smaller FOV."/>
<define name="UNDISTORT_FOCAL_X" value="189.69" description="Focal length x-axis in pixels."/>
<define name="UNDISTORT_FOCAL_Y" value="188.60" description="Focal length y-axis in pixels."/>
<define name="UNDISTORT_CENTER_X" value="165.04" description="Image x-coordinate of the camera principal axis."/>
<define name="UNDISTORT_CENTER_Y" value="118.44" description="Image y-coordinate of the camera principal axis."/>
</doc>
<settings>
@@ -28,13 +22,12 @@
<dl_settings NAME="Undistort">
<dl_setting var="min_x_normalized" min="-4.0" step="0.1" max="0.0" shortname="min_x_n" param="UNDISTORT_MIN_X_NORMALIZED"/>
<dl_setting var="max_x_normalized" min="0.1" step="0.1" max="4.0" shortname="max_x_n" param="UNDISTORT_MAX_X_NORMALIZED"/>
<dl_setting var="dhane_k" min="1.0" step="0.01" max="2.5" shortname="dhane_k" param="UNDISTORT_DHANE_K"/>
<dl_setting var="camera_intrinsics.Dhane_k" min="1.0" step="0.01" max="2.5" shortname="dhane_k" param="UNDISTORT_DHANE_K"/>
<dl_setting var="center_ratio" min="0.05" step="0.01" max="1.0" shortname="center_ratio" param="UNDISTORT_CENTER_RATIO"/>
<dl_setting var="focal_x" min="0.0" step="0.05" max="1024.0" shortname="focal_x" param="UNDISTORT_FOCAL_X"/>
<dl_setting var="center_x" min="0.0" step="0.05" max="1024.0" shortname="center_x" param="UNDISTORT_CENTER_X"/>
<dl_setting var="focal_y" min="0.0" step="0.05" max="1024.0" shortname="focal_y" param="UNDISTORT_FOCAL_Y"/>
<dl_setting var="center_y" min="0.0" step="0.05" max="1024.0" shortname="center_y" param="UNDISTORT_CENTER_Y"/>
<dl_setting var="camera_intrinsics.focal_x" min="0.0" step="0.05" max="1024.0" shortname="focal_x" param="UNDISTORT_FOCAL_X"/>
<dl_setting var="camera_intrinsics.center_x" min="0.0" step="0.05" max="1024.0" shortname="center_x" param="UNDISTORT_CENTER_X"/>
<dl_setting var="camera_intrinsics.focal_y" min="0.0" step="0.05" max="1024.0" shortname="focal_y" param="UNDISTORT_FOCAL_Y"/>
<dl_setting var="camera_intrinsics.center_y" min="0.0" step="0.05" max="1024.0" shortname="center_y" param="UNDISTORT_CENTER_Y"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/modules/computer_vision/undistort_image.c
+12 -46
View File
@@ -25,44 +25,15 @@ PRINT_CONFIG_VAR(UNDISTORT_MIN_X_NORMALIZED)
#endif
PRINT_CONFIG_VAR(UNDISTORT_MAX_X_NORMALIZED)
#ifndef UNDISTORT_DHANE_K
#define UNDISTORT_DHANE_K 1.25f ///< Maximal normalized coordinate that will be shown in the undistorted image
#endif
PRINT_CONFIG_VAR(UNDISTORT_DHANE_K)
#ifndef UNDISTORT_CENTER_RATIO
#define UNDISTORT_CENTER_RATIO 1.00f ///< Maximal normalized coordinate that will be shown in the undistorted image
#endif
PRINT_CONFIG_VAR(UNDISTORT_CENTER_RATIO)
#ifndef UNDISTORT_FOCAL_X
#define UNDISTORT_FOCAL_X 189.69f ///< Focal length in pixels for x-axis. Default value for Bebop 2.
#endif
PRINT_CONFIG_VAR(UNDISTORT_FOCAL_X)
#ifndef UNDISTORT_CENTER_X
#define UNDISTORT_CENTER_X 165.04f ///< Pixel x-coordinate of the optical center. Default value for Bebop 2.
#endif
PRINT_CONFIG_VAR(UNDISTORT_CENTER_X)
#ifndef UNDISTORT_FOCAL_Y
#define UNDISTORT_FOCAL_Y 188.60f ///< Focal length in pixels for y-axis. Default value for Bebop 2.
#endif
PRINT_CONFIG_VAR(UNDISTORT_FOCAL_Y)
#ifndef UNDISTORT_CENTER_Y
#define UNDISTORT_CENTER_Y 118.44f ///< Pixel y-coordinate of the optical center. Default value for Bebop 2.
#endif
PRINT_CONFIG_VAR(UNDISTORT_CENTER_Y)
float min_x_normalized;
float max_x_normalized;
float dhane_k;
float center_ratio;
float focal_x;
float center_x;
float focal_y;
float center_y;
struct camera_intrinsics_t camera_intrinsics;
struct video_listener *listener = NULL;
@@ -72,18 +43,17 @@ static float K[9] = {0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f};
// Function
struct image_t *undistort_image_func(struct image_t *img);
struct image_t *undistort_image_func(struct image_t *img)
static struct image_t *undistort_image_func(struct image_t *img)
{
// TODO: These commands could actually only be run when the parameters or image size are changed
float normalized_step = (max_x_normalized - min_x_normalized) / img->w;
float h_w_ratio = img->h / (float) img->w;
float min_y_normalized = h_w_ratio * min_x_normalized;
float max_y_normalized = h_w_ratio * max_x_normalized;
K[0] = focal_x;
K[2] = center_x;
K[4] = focal_y;
K[5] = center_y;
K[0] = camera_intrinsics.focal_x;
K[2] = camera_intrinsics.center_x;
K[4] = camera_intrinsics.focal_y;
K[5] = camera_intrinsics.center_y;
// create an image of the same size:
struct image_t img_distorted;
@@ -116,7 +86,7 @@ struct image_t *undistort_image_func(struct image_t *img)
if(center_ratio == 1.0f ||
(x_n > center_ratio * min_x_normalized && x_n < center_ratio * max_x_normalized && y_n > center_ratio * min_y_normalized && y_n < center_ratio * max_y_normalized)
) {
normalized_coords_to_distorted_pixels(x_n, y_n, &x_pd, &y_pd, dhane_k, K);
normalized_coords_to_distorted_pixels(x_n, y_n, &x_pd, &y_pd, camera_intrinsics.Dhane_k, K);
if(x_pd > 0.0f && y_pd > 0.0f) {
x_pd_ind = (uint32_t) x_pd;
y_pd_ind = (uint32_t) y_pd;
@@ -139,18 +109,14 @@ struct image_t *undistort_image_func(struct image_t *img)
void undistort_image_init(void)
{
// set the calibration matrix
focal_x = UNDISTORT_FOCAL_X;
center_x = UNDISTORT_CENTER_X;
focal_y = UNDISTORT_FOCAL_Y;
center_y = UNDISTORT_CENTER_Y;
K[0] = focal_x;
K[2] = center_x;
K[4] = focal_y;
K[5] = center_y;
camera_intrinsics = UNDISTORT_CAMERA.camera_intrinsics;
K[0] = camera_intrinsics.focal_x;
K[2] = camera_intrinsics.center_x;
K[4] = camera_intrinsics.focal_y;
K[5] = camera_intrinsics.center_y;
min_x_normalized = UNDISTORT_MIN_X_NORMALIZED;
max_x_normalized = UNDISTORT_MAX_X_NORMALIZED;
center_ratio = UNDISTORT_CENTER_RATIO;
dhane_k = UNDISTORT_DHANE_K;
listener = cv_add_to_device(&UNDISTORT_CAMERA, undistort_image_func, UNDISTORT_FPS);
}
sw/airborne/modules/computer_vision/undistort_image.h
+1 -6
View File
@@ -17,12 +17,7 @@ extern struct video_listener *listener;
// settings:
extern float min_x_normalized;
extern float max_x_normalized;
extern float dhane_k;
extern float center_ratio;
extern float focal_x;
extern float center_x;
extern float focal_y;
extern float center_y;
extern struct camera_intrinsics_t camera_intrinsics;
#endif /* UNDISTORT_MODULE_H */
sw/airborne/peripherals/video_device.h
+1 -1
View File
@@ -48,7 +48,7 @@ struct camera_intrinsics_t {
float focal_y; ///< focal length in the y-direction in pixels
float center_x; ///< center image coordinate in the x-direction
float center_y; ///< center image coordinate in the y-direction
float Dhane_k; //< (un)distortion parameter for a fish-eye lens
float Dhane_k; ///< (un)distortion parameter for a fish-eye lens
};
/** V4L2 device settings */