I have got a calibrated multi-camera system. Both the internal (focal length, distortion, etc) and external (poses) camera parameters have been estimated using HALCON-based program. Now, the goal is to write a C++ program, to read the camera parameters, in particular the distortion coefficients (k1, k2, k3, p1, p2) estimated by HALCON, and use them to undistort images using OpenCV. Unfortunately, so far, I was unable to succeed: the same distortion coefficients used in HALCON and OpenCV generate very different undistorted images! I guess, the problem is the way HALCON and OpenCV are interpreting the distortion coefficients, or even the way they perform undistortion.
Here is my HALCON code to read the distortion coefficients and use them to undistort a test image:
* Read the internal camera calibratino parameters ('area_scan_polynomial' model)
read_cam_par('Calibration.dat', CamParam)
* Estimate camera parameters without distortion: set all distortion coefficients to [k1, k2, k3, p1, p2] = [0, 0, 0, 0, 0]
change_radial_distortion_cam_par ('fixed', CamParam, [0, 0, 0, 0, 0], CamParamOut)
* Estimate camera matrix of distortion-free parameters (used for OpenCV)
cam_par_to_cam_mat(CamParamOut, CamMatrix, ImageWidth, ImageHeight)
* Generate map to use for undistortion.
* Note the use of type 'coord_map_sub_pix' which is of type 'vector_field_absolute',
* i.e. the values are 2D absolute coordinates of the corresponding undistorted pixel location
gen_radial_distortion_map(Map, CamParam, CamParamOut, 'coord_map_sub_pix')
* Read a test image and undistort it using the estimate map
read_image (Image, 'test.jpg')
map_image(Image, Map, ImageRectified)
I'm trying to do exactly the same in OpenCV, using the following code:
Mat view , rview, mapx, mapy;
// Read the same test image as in HALCON
view = imread("test.jpg");
// Get the image size
const Size viewSize = view.size();
// Generate map to use for undistortion
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
cameraMatrix, viewSize, CV_16SC2, mapx, mapy);
// Undistort the image using the estimated map
remap(view, rview, mapx, mapy, INTER_LINEAR);
The variable "cameraMatrix" in OpenCV equals the variable "CamMatrix" in HALCON. The distortion coefficients "distCoeffs" in OpenCV are taken from HALCON (k1, k2, k3, p1, p2) and rearranged following the documentation in this way:
distCoeffs = (Mat_<double>(5, 1) << k1, k2, p2, p1, k3)
Notice that k3 is provided as the fifth parameters, and p2 and p1 are swapped. According to the HALCON documentation (https://www.mvtec.com/doc/halcon/12/en/calibrate_cameras.html , see the function calibrate_cameras) the undistorted coordinate in the image plane (u, v) are computed from the distorted (u', v') as:
u = u' + u' (k1 r^2 + k2 r^4 + k3 r^6) + p1 (r^2 + 2 u'^2) + 2 p2 u' v'
v = v' + v' (k1 r^2 + k2 r^4 + k3 r^6) + p2 (r^2 + 2 v'^2) + 2 p1 u' v'
having r = sqrt(u'^2 + v'^2)
While in OpenCV (https://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html , see the function initUndistortRectifyMap) the same undistorted coordinates are estimated similarly, only p1 and p2 are swapped.
Apparently both OpenCV and HALCON project pixels into the image plane similarly. That is, having pixel (x, y) the corresponding image plane coordinates are computed as:
u' = x - cx / fx
v' = y - cy / fy
These can be of course back-projected to re-obtain the corresponding pixel coordinates:
x = u' * fx + cx
y = v' * fy + cy
According to the documentation, it seems all should work as expected. However, I don't get it why the HALCON and OpenCV-based codes still output very different results. I noticed that to produce similar undistorted results (yet not exactly the same) as in HALCON, I had to scale down (factor of ~100 !) the distortion coefficients in OpenCV. In fact, I noticed that HALCON estimates HUGE distortion coefficiens. As an example, to produce visible changes in the undistorted image, I had to set in HALCON k1=1000, while in OpenCV k1=1 already changed the image visibly. For some distortion coefficients I even had to invert (with a minus) the values to obtain undistorted results going in a similar direction...
I digged-in a little more the HALCON undistortion code, and tried to estimate manually the undistorted coordinates (u, v) following the documentation, which should correspond to the one found in "Map". I did this to make sure "Map" is really estimated the way it is specified in the documentation/the way I understand it. However, even here I obtained very different results, compared to those estimated in "Map"... To do the test I used the following code:
* Get the camera parameters from the calibration
get_cam_par_data (CamParam, 'k1', k1)
get_cam_par_data (CamParam, 'k2', k2)
get_cam_par_data (CamParam, 'k3', k3)
get_cam_par_data (CamParam, 'p1', p1)
get_cam_par_data (CamParam, 'p2', p2)
get_cam_par_data (CamParam, 'cx', cx)
get_cam_par_data (CamParam, 'cy', cy)
get_cam_par_data (CamParam, 'image_width', width)
get_cam_par_data (CamParam, 'image_height', height)
* Estimate the camera matrix, to read the focal length in pixel
cam_par_to_cam_mat(CamParamOut, CamMatrix, width, height)
* Extract the focal lenths in pixel from the estimated camera matrix (see above)
fx_px := CamMatrix[0]
fy_px := CamMatrix[4]
* Pick a pixel coordinate (I tried different values) in the image domain
x := 350
y := 450
* Convert into image plane coordinates
u_1 := (x - cx) / fx_px
v_1 := (y - cy) / fy_px
* Estimate the undistorted location u_2 and v_2
r2 := u_1 * u_1 + v_1 * v_1
u_2 := u_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + 2 * p1 * u_1 * v_1 + p2 * (r2 + 2 * u_1 * u_1)
v_2 := v_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + 2 * p2 * u_1 * v_1 + p1 * (r2 + 2 * v_1 * v_1)
* Back to pixel coordinate
x_1 := u_2 * fx_px + cx
y_1 := v_2 * fy_px + cy
* Compare the values with the value in Map (estimated as before). G_found and G_est should match!
G_found := [y_1, x_1]
get_grayval(Map, y, x, G_est)
I tried concentrating on few distortion coefficients at a time, i.e. only k1 > 0, the others are set to 0. However in most cases, (with few exceptions when x = cx, y = cy) the undistorted coordinates exceed the image size or even become negative.
Is this not the way HALCON estimates the undistortion map coordinates? Am I missing something? How should these distortion coefficients be converted to have OpenCV produce the exact same undistorted resulting images? Any help would be highly appreciated!
Due to some software restriction, using exclusively OpenCV for calibration and undistortion is arguable, however unfortunately for me not an acceptable solution.
I was able to find an answer to my own question. In short, the answer is YES. Yes, it is possible to convert from HALCON to OpenCV distortion parameter and viceversa. The reason is that, HALCON and OpenCV apparently estimate the same underlying model. I did several successfull tests to confirm this and I would like to share my insights. Below, the formulas I computed to convert each distortion parameters from HALCON to OpenCV:
k1_opencv = k1_halcon * fmm * fmm;
k2_opencv = k2_halcon * fmm * fmm * fmm * fmm;
k3_opencv = k3_halcon * fmm * fmm * fmm * fmm * fmm * fmm;
p1_opencv = p2_halcon * fmm; // Notice: swap
p2_opencv = p1_halcon * fmm;
Note that fmm is the focal length in millimeters as estimated in e.g. HALCON. The correct HALCON code to estimate the same values estimated in the map is:
get_cam_par_data (CamParam, 'sx', Sx)
get_cam_par_data (CamParam, 'sy', Sy)
* Convert into image plane coordinates
u_1 := (x - cx) * sx
v_1 := (y - cy) * sy
* Estimate the undistorted location u_2 and v_2
r2 := u_1 * u_1 + v_1 * v_1
u_2 := u_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + 2 * p2 * u_1 * v_1 + p1 * (r2 + 2 * u_1 * u_1)
v_2 := v_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + 2 * p1 * u_1 * v_1 + p2 * (r2 + 2 * v_1 * v_1)
* Back to pixel coordinate
x_1 := u_2 / sx + cx
y_1 := v_2 / sy + cy
* Compare coordinates. NOTICE: we get the values from the distortion map
* going from UNdistorted to DIstorted coordinates!!!
G_found := [y_1, x_1]
gen_radial_distortion_map(MapUD, CamParamOut, CamParam, 'coord_map_sub_pix')
get_grayval(MapUD, y, x, G_est)
With respect to the initial code I posted in my question, the coordinates are transformed using the pixel size in millimeters sx and sy, rather than the focal length. Another difference is that we compare the estimated coordinates with the values from MapUD, where MapUD(UNdistorted coordinate) := DIstorted coordinate. The estimated coordinates and the coordinates in the map correspond up to round-off errors and conditions at the image boundaries.
OpenCV instead does the following (perfectly conform to the documentation!):
float u = (x - cx) / fpx;
float v = (y - cy) / fpx;
float x_1 = u;
float y_1 = v;
float r2 = x_1 * x_1 + y_1 * y_1;
float x_2 = x_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + 2 * p1 * x_1 * y_1 + p2 * (r2 + 2 * x_1 * x_1);
float y_2 = y_1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + p1 * (r2 + 2 * y_1 * y_1) + 2 * p2 * x_1 * y_1;
float map_x_est = x_2 * fpx + cx;
float map_y_est = y_2 * fpx + cy;
// Compare coordinates
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
cameraMatrix, viewSize, CV_32FC1, mapx, mapy);
float map_x = mapx.at<float>(y, x);
float map_y = mapy.at<float>(y, x);
In the above code, the values map_x correspond to map_x_est, and map_y correspond to map_y_est up to round-off errors. If we use the same cameraMatrix as is in HALCON, and convert the distortion coefficients distCoeffs using the formula stated above, we clearly see that the mapped values in the OpenCV variables map_x and map_y correspond to those found in MapUD from HALCON. I tested this by outputting the mapped values one-by-one (in the entire domain) for different distortion parameters both in HALCON and in OpenCV obtaining the same result up to small errors < 0.01 pixels.
Additional Info: MVTec sent me some HALCON code to compare manually estimated coordinates with the one in the Map. Notice, that with respect to my solution, they go the other way round, i.e. from DIstorted to UNdistorted. The code does not work for me in many cases. Feel free to try it and let me know:
dev_close_window ()
dev_update_off ()
Width:=1600
Height:=1200
dev_open_window_fit_size(0, 0, Width, Height, -1, -1, WindowHandle)
gen_cam_par_area_scan_polynomial (0.008, 0, 0, 0, 0, 10, 5.2e-006, 5.2e-006, Width/2, Height/2, Width, Height, CamParam)
change_radial_distortion_cam_par ('fixed', CamParam, [0, 0, 0, 0, 0], CamParamOut)
gen_radial_distortion_map(Map, CamParam, CamParamOut, 'coord_map_sub_pix')
get_cam_par_data (CamParam, 'k1', k1)
get_cam_par_data (CamParam, 'k2', k2)
get_cam_par_data (CamParam, 'k3', k3)
get_cam_par_data (CamParam, 'p1', p1)
get_cam_par_data (CamParam, 'p2', p2)
get_cam_par_data (CamParam, 'cx', cx)
get_cam_par_data (CamParam, 'cy', cy)
get_cam_par_data (CamParam, 'sx', Sx)
get_cam_par_data (CamParam, 'sy', Sy)
* Select a valid point
Row := 86
Col := 89
get_grayval(Map, Row, Col, G_map)
get_domain (Map, Domain)
test_region_point (Domain, Row, Col, IsInside)
if (IsInside)
* Check calculation
GRow:=G_map[0]
GCol:=G_map[1]
U_1 := (GCol - cx) * Sx
V_1 := (GRow - cy) * Sy
R_2 := U_1 * U_1 + V_1 * V_1
U_2 := U_1 * (1 + k1 * R_2 + k2 * R_2 * R_2 + k3 * R_2 * R_2 * R_2) + p1 * (R_2 + 2 * U_1 * U_1) + 2 * p2 * U_1 * V_1
V_2 := V_1 * (1 + k1 * R_2 + k2 * R_2 * R_2 + k3 * R_2 * R_2 * R_2) + p2 * (R_2 + 2 * V_1 * V_1) + 2 * p1 * U_1 * V_1
Col_calc := U_2 / Sx + cx
Row_calc := V_2 / Sy + cy
G_input:=[Row, Col]
G_calc:=[Row_calc, Col_calc]
G_diff:=G_calc-G_input
dev_inspect_ctrl ([G_input, G_calc, G_diff])
stop()
else
* Point is outside domain of distortion map
stop()
endif
dev_close_inspect_ctrl ([G_input, G_calc, G_diff])
dev_clear_window ()
dev_update_on ()
disp_message (WindowHandle, 'No more lines to execute...', 'window', 12, 12, 'black', 'true')