generate_undistort_map_radtan.m

function [] = generate_undistort_map_radtan(cinfo, sequence_name, batch_size)
%GENERATE_UNDISTORT_MAP_RADTAN generates a mapping from distorted to 
%undistorted pixels in a given image with radtan camera distortion, given 
%the camera intrinsics.
%
% Syntax: GENERATE_UNDISTORT_MAP_RADTAN(cinfo, sequence_name, batch_size)
%
% Inputs:
%    cinfo         - camera info struct, generated by extract_ros_data.
%    sequence_name - string to be concatenated to the output name.
%    batch_size    - OPTIONAL number of points to undistort at once.
%                    Unclear if increasing this will speed up the program.
%
% See also EXTRACT_ROS_DATA
%
% Author: Alex Zihao Zhu, University of Pennsylvania
% Email: alexzhu(at)seas.upenn.edu
% Copyright 2018 University of Pennsylvania 
% Alex Zihao Zhu, Nikolay Atanasov, Kostas Daniilidis
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS, CONTRIBUTORS, AND THE 
% TRUSTEES OF THE UNIVERSITY OF PENNSYLVANIA "AS IS" AND ANY EXPRESS OR 
% IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
% OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
% IN NO EVENT SHALL THE COPYRIGHT OWNER, CONTRIBUTORS OR THE TRUSTEES OF 
% THE UNIVERSITY OF PENNSYLVANIA BE LIABLE FOR ANY DIRECT, INDIRECT, 
% INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
% NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 
% THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 
% THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

output_name = ['undistort_map_', sequence_name];
fprintf('Generating undistort maps and saving in %s. This may take some time.\n', ...
    output_name)

if nargin < 3
    batch_size = 1;
end

K = reshape(cinfo.K, 3, 3);
D = cinfo.D;
rows = cinfo.height;
cols = cinfo.width;
cameraParams = cameraParameters(...
    'IntrinsicMatrix', K, ...
    'RadialDistortion', D(1:2), ...
    'TangentialDistortion', D(3:4));

undistort_map_x = zeros(rows, cols);
undistort_map_y = zeros(rows, cols);

[X, Y] = meshgrid(0:cols-1, 0:rows-1);
X = X(:);
Y = Y(:);

n_pts = length(X);
undistorted_points = zeros(n_pts, 2);

last_iter = 1;
fprintf('Processing %d points\n', n_pts);
for i=1:batch_size:n_pts
    end_val = min(n_pts, i+batch_size-1);
    undistorted_points(i:end_val, :) = undistortPoints([X(i:end_val) Y(i:end_val)], cameraParams);
    if i - last_iter >= 1000
        fprintf('Processed %d points\n', i);
        last_iter = i;
    end
end

% undistortedPoints = undistortPoints([X Y], cameraParams);
addpath('../EventFeatureTracking/Tracker/')
inds = sub2indc(X+1, Y+1, [rows, cols]);
undistort_map_x(inds) = undistorted_points(:, 1);
undistort_map_y(inds) = undistorted_points(:, 2);

save(output_name, 'undistort_map_x', 'undistort_map_y', '-v7.3');
end