DLR CalDe
DLR CalLab
Application of the calibration results for common computer vision tasks
In order to make it clear the camera model used by DLR CalLab we next briefly demonstrate in Matlab (R) code some common computer vision tasks like undistortion, pixel reprojection, and stereo rays intersection.
Image point undistortion
On the basis of both the camera calibration files and some particular image coordinates it is possible to undistort these distorted coordinates as follows:
function undistorted = undistort(distorted,A,k1,k2) % starting out, we temporarily suppose the original distorted image% points as undistorted, distort them and compute the residuals,% which shall be subtracted from the temporal undistorted in order% their distorted to equal the original distorted image points. und_tmp = distorted; % initsdistortion_error = [1;1]; i=1; optim_done="0; while (~optim_done) u = und_tmp(1) - A(1,3);v = und_tmp(2) - A(2,3);y = v/A(2,2);x2 = ( (u-A(1,2)*y) / A(1,1) )^2;y2 = y^2;dis_tmp = und_tmp + [k1*u.*(x2+y2) + k2*u.*(x2+y2).^2; ...k1*v.*(x2+y2) + k2*v.*(x2+y2).^2];distortion_error(:,i) = dis_tmp - distorted;und_tmp = und_tmp - distortion_error(:,i); if (sum(distortion_error(:,i).^2)<1e-2) % dis_error < 1e-1 pixels optim_done=1;res_dis_error=sqrt(sum(distortion_error(:,i).^2))else i=i+1 % 4 iterations should sufficeend endundistorted = und_tmp;
function undistorted = undistort(distorted,A,k1,k2)
% starting out, we temporarily suppose the original distorted image% points as undistorted, distort them and compute the residuals,% which shall be subtracted from the temporal undistorted in order% their distorted to equal the original distorted image points.
und_tmp = distorted;
% initsdistortion_error = [1;1]; i=1; optim_done="0;
while (~optim_done)
u = und_tmp(1) - A(1,3);v = und_tmp(2) - A(2,3);y = v/A(2,2);x2 = ( (u-A(1,2)*y) / A(1,1) )^2;y2 = y^2;dis_tmp = und_tmp + [k1*u.*(x2+y2) + k2*u.*(x2+y2).^2; ...k1*v.*(x2+y2) + k2*v.*(x2+y2).^2];distortion_error(:,i) = dis_tmp - distorted;und_tmp = und_tmp - distortion_error(:,i);
if (sum(distortion_error(:,i).^2)<1e-2) % dis_error < 1e-1 pixels optim_done=1;res_dis_error=sqrt(sum(distortion_error(:,i).^2))else i=i+1 % 4 iterations should sufficeend
endundistorted = und_tmp;
or rather faster, by using the jacobian between the undistorted normalized coordinates and the residual error in the distorted coordinates in the image. Plus, if you proceed vector-wise, it is much faster (obrigado a Diego):
function [undistorted,n_it] = undistort_(distorted,A,k1,k2) % from image pixels to pinhole-centered normalized distancesxd_normalized = distorted(1,:) - A(1,3);yd_normalized = distorted(2,:) - A(2,3);yd_normalized = yd_normalized/A(2,2);xd_normalized = ( (xd_normalized-A(1,2)*yd_normalized) / A(1,1) );xd_normalized2 = xd_normalized.^2;yd_normalized2 = yd_normalized.^2;% we use them as temporary desired undistorted normalized valuesxu_normalized = xd_normalized;yu_normalized = yd_normalized; % initsdistortion_error = [1;1]; i=1; optim_done="0; while (~optim_done) % radial distortionr2 = (xu_normalized.^2+yu_normalized.^2);d_r = (1+k1*r2+k2*r2.^2); % distance in normalized coos between desired and actual distorted coosdx_normalized = xu_normalized.*d_r - xd_normalized;dy_normalized = yu_normalized.*d_r - yd_normalized;% Goal: minimize them by refining the undistorted coos.% Jacobian of the latter distances w.r.t. the desired undistorted normalized coos:dD1dx = d_r + xu_normalized.^2 .* (2*k1+4*k2*r2);dD1dy = xu_normalized.*yu_normalized.*(2*k1+4*k2*r2);dD2dx = dD1dy;dD2dy = d_r + yu_normalized.^2 .* (2*k1 + 4*k2*r2);% Linear equation:% dxy_normalized + J * delta_xy = [0;0] => delta_xy = -J^(-1)*dxy_normalized% J^(-1) = 1/det(J) * [dD2dy -dD1dy; -dD2dx dD1dx]det_J = dD2dy.*dD1dx - dD1dy.*dD2dx;delta_x = -( dD2dy.*dx_normalized - dD1dy.*dy_normalized)./det_J;delta_y = -(-dD2dx.*dx_normalized + dD1dx.*dy_normalized)./det_J;% Modifies the undistorted normalized coos.xu_normalized = xu_normalized + delta_x;yu_normalized = yu_normalized + delta_y; % This is only necessary for the termination condition:r2 = (xu_normalized.^2+yu_normalized.^2); d_r = (1+k1*r2+k2*r2.^2);redistorted(1,:) = A(1,1) .* xu_normalized.*d_r + A(1,2) .* yu_normalized.*d_r + A(1,3);redistorted(2,:) = A(2,2) .* yu_normalized.*d_r + A(2,3); % We stop if the (biggest if vector-wise) pixel error is bigger than 0.1.if (max((distorted(1,:)-redistorted(1,:)).^2 + ... (distorted(2,:)-redistorted(2,:)).^2) < 1e-2) % max dis_error < 1e-1 pixels optim_done=1; else i=i+1; % 2-3 iterations should suffice end end % Final undistorted coos in the image.n_it=i;undistorted(1,:) = A(1,1) .* xu_normalized + A(1,2) .* yu_normalized + A(1,3);undistorted(2,:) = A(2,2) .* yu_normalized + A(2,3);
function [undistorted,n_it] = undistort_(distorted,A,k1,k2)
% from image pixels to pinhole-centered normalized distancesxd_normalized = distorted(1,:) - A(1,3);yd_normalized = distorted(2,:) - A(2,3);yd_normalized = yd_normalized/A(2,2);xd_normalized = ( (xd_normalized-A(1,2)*yd_normalized) / A(1,1) );xd_normalized2 = xd_normalized.^2;yd_normalized2 = yd_normalized.^2;% we use them as temporary desired undistorted normalized valuesxu_normalized = xd_normalized;yu_normalized = yd_normalized;
% radial distortionr2 = (xu_normalized.^2+yu_normalized.^2);d_r = (1+k1*r2+k2*r2.^2);
% distance in normalized coos between desired and actual distorted coosdx_normalized = xu_normalized.*d_r - xd_normalized;dy_normalized = yu_normalized.*d_r - yd_normalized;% Goal: minimize them by refining the undistorted coos.% Jacobian of the latter distances w.r.t. the desired undistorted normalized coos:dD1dx = d_r + xu_normalized.^2 .* (2*k1+4*k2*r2);dD1dy = xu_normalized.*yu_normalized.*(2*k1+4*k2*r2);dD2dx = dD1dy;dD2dy = d_r + yu_normalized.^2 .* (2*k1 + 4*k2*r2);% Linear equation:% dxy_normalized + J * delta_xy = [0;0] => delta_xy = -J^(-1)*dxy_normalized% J^(-1) = 1/det(J) * [dD2dy -dD1dy; -dD2dx dD1dx]det_J = dD2dy.*dD1dx - dD1dy.*dD2dx;delta_x = -( dD2dy.*dx_normalized - dD1dy.*dy_normalized)./det_J;delta_y = -(-dD2dx.*dx_normalized + dD1dx.*dy_normalized)./det_J;% Modifies the undistorted normalized coos.xu_normalized = xu_normalized + delta_x;yu_normalized = yu_normalized + delta_y;
% This is only necessary for the termination condition:r2 = (xu_normalized.^2+yu_normalized.^2); d_r = (1+k1*r2+k2*r2.^2);redistorted(1,:) = A(1,1) .* xu_normalized.*d_r + A(1,2) .* yu_normalized.*d_r + A(1,3);redistorted(2,:) = A(2,2) .* yu_normalized.*d_r + A(2,3);
% We stop if the (biggest if vector-wise) pixel error is bigger than 0.1.if (max((distorted(1,:)-redistorted(1,:)).^2 + ... (distorted(2,:)-redistorted(2,:)).^2) < 1e-2) % max dis_error < 1e-1 pixels
optim_done=1;
else
i=i+1; % 2-3 iterations should suffice
end
% Final undistorted coos in the image.n_it=i;undistorted(1,:) = A(1,1) .* xu_normalized + A(1,2) .* yu_normalized + A(1,3);undistorted(2,:) = A(2,2) .* yu_normalized + A(2,3);
Only now we are able to reproject undistorted image points to camera rays and also for instance perform stereo triangulation.
Stereo triangulation from image points
function point_camera = triangulate(und_upper, und_lower, A, A_stereo, T_cam1_cam2) % get normalized coordinates for camera #1u = und_upper(1) - A(1,3);v = und_upper(2) - A(2,3);y_upper = v/A(2,2);x_upper = ( (u-A(1,2)*y_upper) / A(1,1) ); % get normalized coordinates for camera #2u = und_lower(1) - A_stereo(1,3);v = und_lower(2) - A_stereo(2,3);y_lower = v/A_stereo(2,2);x_lower = ( (u-A_stereo(1,2)*y_lower) / A_stereo(1,1) ); % linear least squares solution for triangulation% with the rigid body motion constraint T_cam1_cam2% (in the form: M * x = m)% M = [[x_upper;y_upper;1] -T_cam1_cam2(1:3,1:3)*[x_lower;y_lower;1]];m = T_cam1_cam2(1:3,4);% SVD solution[U,S,V]=svd(M);m_=U'*m;s = size(S);m_(1:s(2))./sum(S)';sol = V*ans; %distance_to_camera = sum(sol)/2distance_to_camera_1 = sqrt(sum(([x_upper;y_upper;1]*sol(1)).^2))distance_to_camera_2 = sqrt(sum(([x_lower;y_lower;1]*sol(2)).^2)) % [[x_upper;y_upper;1]*sol(1);1]% T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1]) % admittedly this solution ain't any optimal but approximated...point_camera = ( [[x_upper;y_upper;1]*sol(1); 1] + ...T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1]) ) / 2point_camera(4)=1; error = ([[x_upper;y_upper;1]*sol(1);1] -(T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1])))euclidean_error = sqrt(sum(error(1:3).^2))if euclidean_error>1e-2disp('Left and right rays do not really cut (within one centimeter)')returnend
function point_camera = triangulate(und_upper, und_lower, A, A_stereo, T_cam1_cam2)
% get normalized coordinates for camera #1u = und_upper(1) - A(1,3);v = und_upper(2) - A(2,3);y_upper = v/A(2,2);x_upper = ( (u-A(1,2)*y_upper) / A(1,1) );
% get normalized coordinates for camera #2u = und_lower(1) - A_stereo(1,3);v = und_lower(2) - A_stereo(2,3);y_lower = v/A_stereo(2,2);x_lower = ( (u-A_stereo(1,2)*y_lower) / A_stereo(1,1) );
% linear least squares solution for triangulation% with the rigid body motion constraint T_cam1_cam2% (in the form: M * x = m)% M = [[x_upper;y_upper;1] -T_cam1_cam2(1:3,1:3)*[x_lower;y_lower;1]];m = T_cam1_cam2(1:3,4);% SVD solution[U,S,V]=svd(M);m_=U'*m;s = size(S);m_(1:s(2))./sum(S)';sol = V*ans;
%distance_to_camera = sum(sol)/2distance_to_camera_1 = sqrt(sum(([x_upper;y_upper;1]*sol(1)).^2))distance_to_camera_2 = sqrt(sum(([x_lower;y_lower;1]*sol(2)).^2))
% [[x_upper;y_upper;1]*sol(1);1]% T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1])
% admittedly this solution ain't any optimal but approximated...point_camera = ( [[x_upper;y_upper;1]*sol(1); 1] + ...T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1]) ) / 2point_camera(4)=1;
error = ([[x_upper;y_upper;1]*sol(1);1] -(T_cam1_cam2*([[x_lower;y_lower;1]*sol(2);1])))euclidean_error = sqrt(sum(error(1:3).^2))if euclidean_error>1e-2disp('Left and right rays do not really cut (within one centimeter)')returnend