Commit 47ace827 authored by Daniel Bourgault's avatar Daniel Bourgault
Browse files

The cdoe can now deal with GCPs with eleation. Plus the code now comes with m_map.

parent 13a4218a
...@@ -33,20 +33,18 @@ dH = 0.0; ...@@ -33,20 +33,18 @@ dH = 0.0;
dtheta = 20.0; dtheta = 20.0;
% Order of the polynomial correction (0, 1 or 2) % Order of the polynomial correction (0, 1 or 2)
polyOrder = 1; polyOrder = 0;
% To save memory calculation can be done in single precision. % To save memory calculation can be done in single precision.
% For higher precision set the variable 'precision' to 'double'; % For higher precision set the variable 'precision' to 'double';
precision = 'double'; precision = 'double';
% Ground Control Points (GCP). % Ground Control Points (GCP).
% The data must come right after the gcpData = true % The data must come right after the gcpData = true
gcpData = true; gcpData = true;
360 829 -70.561367 47.303783 360 829 -70.561367 47.303783 0
54 719 -70.54500 47.335 54 719 -70.54500 47.335 0
99 661 -70.505 47.375 99 661 -70.505 47.375 0
452 641 -70.435 47.389 452 641 -70.435 47.389 0
429 633 -70.418 47.408 429 633 -70.418 47.408 0
816 644 -70.393 47.368 816 644 -70.393 47.368 0
\ No newline at end of file \ No newline at end of file
...@@ -45,18 +45,18 @@ precision = 'double'; ...@@ -45,18 +45,18 @@ precision = 'double';
% Ground Control Points (GCP). % Ground Control Points (GCP).
gcpData = true; gcpData = true;
2999 226 0.500 0.00 2999 226 0.500 0.00 0
1694 220 1.500 0.00 1694 220 1.500 0.00 0
528 677 2.290 0.50 528 677 2.290 0.50 0
289 1231 2.290 1.00 289 1231 2.290 1.00 0
3819 686 0.000 0.50 3819 686 0.000 0.50 0
4018 1266 0.000 1.00 4018 1266 0.000 1.00 0
2566 1235 0.890 0.99 2566 1235 0.890 0.99 0
3806 2118 0.255 1.56 3806 2118 0.255 1.56 0
4131 1580 0.000 1.23 4131 1580 0.000 1.23 0
521 2548 1.910 1.82 521 2548 1.910 1.82 0
248 2328 2.060 1.71 248 2328 2.060 1.71 0
14 1868 2.290 1.46 14 1868 2.290 1.46 0
......
...@@ -4,7 +4,7 @@ function errGeoFit = g_error_geofit(cv,imgWidth,imgHeight,xp,yp,ic,jc,... ...@@ -4,7 +4,7 @@ function errGeoFit = g_error_geofit(cv,imgWidth,imgHeight,xp,yp,ic,jc,...
hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,... hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,...
dhfov,dlambda,dphi,dH,dtheta,... dhfov,dlambda,dphi,dH,dtheta,...
LON0,LAT0,... LON0,LAT0,...
i_gcp,j_gcp,lon_gcp,lat_gcp,... i_gcp,j_gcp,lon_gcp0,lat_gcp0,h_gcp,...
theOrder,field); theOrder,field);
...@@ -20,7 +20,6 @@ end ...@@ -20,7 +20,6 @@ end
[LON LAT] = g_pix2ll(xp,yp,imgWidth,imgHeight,ic,jc,... [LON LAT] = g_pix2ll(xp,yp,imgWidth,imgHeight,ic,jc,...
hfov,lambda,phi,theta,H,LON0,LAT0,field); hfov,lambda,phi,theta,H,LON0,LAT0,field);
% Calculate the error between ground control points (GCPs) % Calculate the error between ground control points (GCPs)
% and image control points once georectified. % and image control points once georectified.
% %
...@@ -35,6 +34,9 @@ ngcp = length(i_gcp); ...@@ -35,6 +34,9 @@ ngcp = length(i_gcp);
ngcpFinite = 0; ngcpFinite = 0;
errGeoFit = 0.0; errGeoFit = 0.0;
% Project the GCPs onto the water surface given their elevation
[lon_gcp,lat_gcp] = g_proj_GCP(LON0,LAT0,H,lon_gcp0,lat_gcp0,h_gcp,field);
for k = 1:ngcp for k = 1:ngcp
% Calculate the distance (i.e. error) between GCP and rectificed ICPs. % Calculate the distance (i.e. error) between GCP and rectificed ICPs.
...@@ -63,17 +65,3 @@ if abs(phi - phiGuess) > dphi; errGeoFit = inf; end ...@@ -63,17 +65,3 @@ if abs(phi - phiGuess) > dphi; errGeoFit = inf; end
if abs(H - HGuess) > dH; errGeoFit = inf; end if abs(H - HGuess) > dH; errGeoFit = inf; end
if abs(theta - thetaGuess) > dtheta; errGeoFit = inf; end if abs(theta - thetaGuess) > dtheta; errGeoFit = inf; end
%fprintf('\n')
%fprintf('Field of view (fov): %f\n',hfov)
%fprintf('Dip angle (lambda): %f\n',lambda)
%fprintf('Tilt angle (phi): %f\n',phi)
%fprintf('Camera altitude (H): %f\n',H)
%fprintf('View angle from North (theta): %f\n',theta)
%fprintf('RMS error (m): %f\n',errGeoFit)
%fprintf('\n')
%figure(100)
%hold on;
%plot(errGeoFit);
...@@ -61,7 +61,6 @@ vfov = 2*atand(tand(hfov/2)/aspectRatio); ...@@ -61,7 +61,6 @@ vfov = 2*atand(tand(hfov/2)/aspectRatio);
fx = (imgWidth/2)/tand(hfov/2); fx = (imgWidth/2)/tand(hfov/2);
fy = (imgHeight/2)/tand(vfov/2); fy = (imgHeight/2)/tand(vfov/2);
% Subtract the principal point % Subtract the principal point
x_p = xp - x_c + (jc); x_p = xp - x_c + (jc);
y_p = yp - y_c + (ic); y_p = yp - y_c + (ic);
......
function [lon_gcp,lat_gcp] = g_proj_GCP(LON0,LAT0,H,lon_gcp0,lat_gcp0,h_gcp,field)
%G_PROJ_GCP
%
% This function projects the GCPs that have an elevation greater than 0
% onto the water level of zero elevation. This is a trick in order to be
% able to deal with GCPs that have elevation. The principle can be
% illustrated in 1D like this. Consider the position x of a GCP that has an
% elevation h. Then, the projection (xp) of this GCP onto the plane that has
% zero elevation and along a line that joins the camera position of elevation
% H and the GCP is given by:
%
% xp = x * (H - h)/H
%
% INPUTS:
%
% LON0: The longitiude of the camera
% LAT0: The latitude of the camera
% H: The elevation of the camera
% lon_gcp0: The longitudes of the GCPs
% lat_gcp0: The latitudes of the GCPs
% h_gcp: The elevations of the GCPs
%
% OUTPUTS:
% lon_gcp: The projected longitudes of the GCPs
% lat_gcp: The projected latitudes of the GCPs
%
n_gcp = length(h_gcp);
for i = 1:n_gcp
if field == true
% The calculation for the distance between the GCPs and the camera
% is done on an elliptical earth and this takes a long calculation
% with the command m_idist from the m_map package. Therefore, the
% calculation is done only on the GCPs with no elevation, hence
% the 'if' condition here.
if h_gcp(i) > 0
[range,a12,a21] = m_idist(LON0,LAT0,lon_gcp0(i),lat_gcp0(i));
proj_factor = H/(H-h_gcp(i));
new_range = range*proj_factor;
[lon_gcp(i),lat_gcp(i),a21] = m_fdist(LON0,LAT0,a12,new_range);
lon_gcp(i) = lon_gcp(i) - 360;
else
% If there's no elevation, then there's nothing to do.
lon_gcp(i) = lon_gcp0(i);
lat_gcp(i) = lat_gcp0(i);
end
else
% For lab situations where positions are provided in meters rather
% than in latitude longitude the calculation is simpler.
dx = lon_gcp0(i) - LON0;
dy = lat_gcp0(i) - LAT0;
range = sqrt(dx^2 + dy^2);
proj_factor = H/(H-h_gcp(i));
new_range = range*proj_factor;
beta = atan2(dy,dx);
lon_gcp(i) = new_range*cos(beta) + LON0;
lat_gcp(i) = new_range*sin(beta) + LAT0;
end
end
\ No newline at end of file
...@@ -37,12 +37,23 @@ function g_rect ...@@ -37,12 +37,23 @@ function g_rect
% %
% LAT0: Same as LON0 but for the latitude. % LAT0: Same as LON0 but for the latitude.
% %
% lon_gcp: A vector containing the longitude of each ground % lon0_gcp: A vector containing the longitude of each ground
% control points (GCP). For the lab case this is the % control points (GCP). For the lab case this is the
% cartesian coordinate in meters. % cartesian coordinate in meters. These GCPs may have
% elevations.
%
% lat0_gcp: Same as lon_gcp for latitude.
%
% lon_gcp: A vector containing the longitude of each ground
% control points (GCP) projected onto the water level
% i.e. at 0 m of elevation. For the lab case this is
% the cartesian coordinate in meters.
% %
% lat_gcp: Same as lon_gcp for latitude. % lat_gcp: Same as lon_gcp for latitude.
% %
% h_gcp: The elevation in meter of the GCPs. The elevation is
% 0 m if taken at water level.
%
% i_gcp: The horizontal index of the image ground control % i_gcp: The horizontal index of the image ground control
% points. % points.
% %
...@@ -53,11 +64,12 @@ function g_rect ...@@ -53,11 +64,12 @@ function g_rect
% %
% phi: Camera tilt angle [degree]. % phi: Camera tilt angle [degree].
% %
% lambda: Camera dip angle [degree].
%
% H: The camera altitude relative to the water [m]. % H: The camera altitude relative to the water [m].
% %
% theta: View angle clockwise from North [degree]. % theta: View angle clockwise from North [degree].
% %
%
% errGeoFit: The rms error of the georectified image after % errGeoFit: The rms error of the georectified image after
% geometrical transformation [m]. % geometrical transformation [m].
% %
...@@ -71,6 +83,7 @@ function g_rect ...@@ -71,6 +83,7 @@ function g_rect
% and calculations can always be done in double % and calculations can always be done in double
% precision. % precision.
% %
% Delta: The effective resolution of the georectified image.
% %
% Other m-files required: Works best with the m_map package for % Other m-files required: Works best with the m_map package for
% visualization. % visualization.
...@@ -83,7 +96,7 @@ function g_rect ...@@ -83,7 +96,7 @@ function g_rect
% February 2013 % February 2013
% %
% %%
% The minimization is repeated nMinimize times where each time a random % The minimization is repeated nMinimize times where each time a random
% combination of the initial guesses is chosen within the given % combination of the initial guesses is chosen within the given
% uncertainties provided by the user. This is becasue the algorithm often % uncertainties provided by the user. This is becasue the algorithm often
...@@ -91,7 +104,6 @@ function g_rect ...@@ -91,7 +104,6 @@ function g_rect
% that the minimum found is a true minimum within the uncertainties provided. % that the minimum found is a true minimum within the uncertainties provided.
nMinimize = 50; nMinimize = 50;
%% Read the parameter file %% Read the parameter file
% Count the number of header lines before the ground control points (GCPs) % Count the number of header lines before the ground control points (GCPs)
% The GCPs start right after the variable gcpData is set to true. % The GCPs start right after the variable gcpData is set to true.
...@@ -114,12 +126,28 @@ while gcpData == false ...@@ -114,12 +126,28 @@ while gcpData == false
end end
fclose(fid); fclose(fid);
%% Import the GCP data (4 column) at the end of the parameter file %% Import the GCP data at the end of the parameter file
gcp = importdata(inputFname,' ',nHeaderLine) gcp = importdata(inputFname,' ',nHeaderLine);
i_gcp = gcp.data(:,1); i_gcp = gcp.data(:,1);
j_gcp = gcp.data(:,2); j_gcp = gcp.data(:,2);
lon_gcp = gcp.data(:,3); lon_gcp0 = gcp.data(:,3);
lat_gcp = gcp.data(:,4); lat_gcp0 = gcp.data(:,4);
h_gcp = gcp.data(:,5);
%%
% Check if the elevation of the GCPs are not too high and above
% a certain fraction (gamma) of the camera height. If so, stop.
gamma = 0.75;
i_bad = find(h_gcp > gamma*(H+dH));
if length(i_bad) > 0
display([' ']);
display([' WARNING:']);
for i = 1:length(i_bad)
display([' The elevation of GCP #',num2str(i_bad(i)),' is greater than ',num2str(gamma),'*(H+dH).']);
end
display([' FIX AND RERUN.']);
return
end
ngcp = length(i_gcp); ngcp = length(i_gcp);
% Get the image size % Get the image size
...@@ -132,7 +160,6 @@ if precision == 'single' ...@@ -132,7 +160,6 @@ if precision == 'single'
imgHeight = single(imgHeight); imgHeight = single(imgHeight);
end end
%% Display information %% Display information
fprintf('\n') fprintf('\n')
fprintf(' INPUT PARAMETERS\n') fprintf(' INPUT PARAMETERS\n')
...@@ -169,9 +196,12 @@ colormap(gray); ...@@ -169,9 +196,12 @@ colormap(gray);
hold on hold on
for i = 1:ngcp for i = 1:ngcp
plot(i_gcp(i),j_gcp(i),'r.'); plot(i_gcp(i),j_gcp(i),'r.');
text(i_gcp(i),j_gcp(i),num2str(i),'color','r','horizontalalignment','right'); text(i_gcp(i),j_gcp(i),[num2str(i),'(',num2str(h_gcp(i)),')'],...
'color','r',...
'horizontalalignment','right',...
'fontsize',16);
end end
title('Ground Control Points','color','r'); title('Ground Control Points Number (elevation in meters)','color','r');
print -dpng -r300 image1.png print -dpng -r300 image1.png
...@@ -179,11 +209,9 @@ fprintf('\n') ...@@ -179,11 +209,9 @@ fprintf('\n')
ok = input('Is it ok to proceed with the rectification (y/n): ','s'); ok = input('Is it ok to proceed with the rectification (y/n): ','s');
if ok ~= 'y' if ok ~= 'y'
return return
%break
end end
%% %%
nUnknown = 0; nUnknown = 0;
if dhfov > 0.0; nUnknown = nUnknown+1; end if dhfov > 0.0; nUnknown = nUnknown+1; end
if dlambda > 0.0; nUnknown = nUnknown+1; end if dlambda > 0.0; nUnknown = nUnknown+1; end
...@@ -196,7 +224,6 @@ if nUnknown > ngcp ...@@ -196,7 +224,6 @@ if nUnknown > ngcp
fprintf('WARNING: \n'); fprintf('WARNING: \n');
fprintf(' The number of GCPs is < number of unknown parameters.\n'); fprintf(' The number of GCPs is < number of unknown parameters.\n');
fprintf(' Program stopped.\n'); fprintf(' Program stopped.\n');
%break
return return
end end
...@@ -227,7 +254,6 @@ if nUnknown > 0 ...@@ -227,7 +254,6 @@ if nUnknown > 0
%options=optimset('MaxFunEvals',100000,'MaxIter',100000,'TolX',1.d-12,'TolFun',1.d-12); %options=optimset('MaxFunEvals',100000,'MaxIter',100000,'TolX',1.d-12,'TolFun',1.d-12);
options = []; options = [];
% Only feed the minimization algorithm with the GCPs. xp and yp are the % Only feed the minimization algorithm with the GCPs. xp and yp are the
% image coordinate of these GCPs. % image coordinate of these GCPs.
xp = i_gcp; xp = i_gcp;
...@@ -297,10 +323,9 @@ if nUnknown > 0 ...@@ -297,10 +323,9 @@ if nUnknown > 0
hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,... hfovGuess,lambdaGuess,phiGuess,HGuess,thetaGuess,...
dhfov,dlambda,dphi,dH,dtheta,... dhfov,dlambda,dphi,dH,dtheta,...
LON0,LAT0,... LON0,LAT0,...
i_gcp,j_gcp,lon_gcp,lat_gcp,... i_gcp,j_gcp,lon_gcp0,lat_gcp0,h_gcp,...
theOrder,field); theOrder,field);
if errGeoFit < bestErrGeoFit if errGeoFit < bestErrGeoFit
bestErrGeoFit = errGeoFit; bestErrGeoFit = errGeoFit;
cvBest = cv; cvBest = cv;
...@@ -334,9 +359,11 @@ if nUnknown > 0 ...@@ -334,9 +359,11 @@ if nUnknown > 0
if length(theOrder) > 1 if length(theOrder) > 1
fprintf('The rms error after geometrical correction (m): %f\n',bestErrGeoFit); fprintf('The rms error after geometrical correction (m): %f\n',bestErrGeoFit);
end end
end end
% Project the GCP that have elevation.
[lon_gcp,lat_gcp] = g_proj_GCP(LON0,LAT0,H,lon_gcp0,lat_gcp0,h_gcp,field);
%% %%
% Now construct the matrices LON and LAT for the entire image using the % Now construct the matrices LON and LAT for the entire image using the
...@@ -369,7 +396,8 @@ fprintf('Saving rectification file in: %s\n',outputFname); ...@@ -369,7 +396,8 @@ fprintf('Saving rectification file in: %s\n',outputFname);
save(outputFname,'imgFname','firstImgFname','lastImgFname',... save(outputFname,'imgFname','firstImgFname','lastImgFname',...
'LON','LAT',... 'LON','LAT',...
'LON0','LAT0',... 'LON0','LAT0',...
'lon_gcp','lat_gcp',... 'lon_gcp0','lat_gcp0',...
'lon_gcp','lat_gcp','h_gcp',...
'i_gcp','j_gcp',... 'i_gcp','j_gcp',...
'hfov','lambda','phi','H','theta',... 'hfov','lambda','phi','H','theta',...
'errGeoFit','errPolyFit',... 'errGeoFit','errPolyFit',...
......
...@@ -18,6 +18,12 @@ function [h_figure,h_pcolor,h_datetext] = g_viz_field(imgFname,rectFile,varargin ...@@ -18,6 +18,12 @@ function [h_figure,h_pcolor,h_datetext] = g_viz_field(imgFname,rectFile,varargin
% corresponding objects in the figure, for use in g_viz_anim % corresponding objects in the figure, for use in g_viz_anim
% %
% Set some plotting parameters
ms = 10; % Marker Size
fs = 10; % Font size
lw = 2; % Line width
% Option
show_time = 0; show_time = 0;
show_land = 0; show_land = 0;
land_color = [241 235 144]/255; land_color = [241 235 144]/255;
...@@ -34,30 +40,21 @@ if length(varargin) > 1 ...@@ -34,30 +40,21 @@ if length(varargin) > 1
end end
end end
% Load the georectification file
% Set some plotting parameters
ms = 10; % Marker Size
fs = 10; % Font size
lw = 2; % Line width
load(rectFile); load(rectFile);
%p = size(LON,3); % Determine the region to plot, delimited by the GCP and the camera
% position. Add a factor (fac) all around
lon_min = min(lon_gcp);
lon_max = max(lon_gcp);
lat_min = min(lat_gcp);
lat_max = max(lat_gcp);
lon_min = min(lon_min,LON0);
lon_max = max(lon_max,LON0);
lat_min = min(lat_min,LAT0);
lat_max = max(lat_max,LAT0);
fac = 0.1; fac = 0.1;
lon_min= lon_min - fac*abs(lon_max-lon_min); lon_min = min([lon_gcp,LON0]);
lon_max= lon_max + fac*abs(lon_max-lon_min); lon_max = max([lon_gcp,LON0]);
lat_min= lat_min - fac*abs(lat_max-lat_min); lat_min = min([lat_gcp,LAT0]);
lat_max= lat_max + fac*abs(lat_max-lat_min); lat_max = max([lat_gcp,LAT0]);
lon_min = lon_min - fac*abs(lon_max - lon_min);
lon_max = lon_max + fac*abs(lon_max - lon_min);
lat_min = lat_min - fac*abs(lat_max - lat_min);
lat_max = lat_max + fac*abs(lat_max - lat_min);
rgb0 = double(imread(imgFname))/255; rgb0 = double(imread(imgFname))/255;
...@@ -70,19 +67,19 @@ end ...@@ -70,19 +67,19 @@ end
clear rgb0; clear rgb0;
int = int'; int = int';
int = int - nanmean(nanmean(int)); % Normalize the image if wanted
%int = int - nanmean(nanmean(int));
figure('Renderer', 'painters', 'Position', [100 100 900 700]) %figure('Renderer', 'painters', 'Position', [100 100 900 700])
figure
m_proj('mercator','longitudes',[lon_min lon_max],'latitudes',[lat_min lat_max]); m_proj('mercator','longitudes',[lon_min lon_max],'latitudes',[lat_min lat_max]);
hold on; hold on;
[X,Y] = m_ll2xy(LON,LAT);
cmap = contrast(int,256); cmap = contrast(int,256);
colormap(cmap); colormap(cmap);
h = pcolor(X,Y,int); h = m_pcolor(LON,LAT,int);
shading('interp'); shading('interp');
% Uncomment one of these lines if you want the coastline to be plotted. % Uncomment one of these lines if you want the coastline to be plotted.
if show_land if show_land
if strcmpi(show_land,'fjord') if strcmpi(show_land,'fjord')
...@@ -109,7 +106,14 @@ m_plot(LON0,LAT0,'kx','markersize',ms,'linewidth',lw); % Camera location ...@@ -109,7 +106,14 @@ m_plot(LON0,LAT0,'kx','markersize',ms,'linewidth',lw); % Camera location
%% Plot GCPs and ICPs. %% Plot GCPs and ICPs.
for n=1:length(i_gcp) for n=1:length(i_gcp)
% Plot the original GCPs that may have elevations
m_plot(lon_gcp0(n),lat_gcp0(n),'ko','markersize',ms,'linewidth',lw);
% Plot the projected GCPs that are actually used for the minimization
m_plot(lon_gcp(n),lat_gcp(n),'bo','markersize',ms,'linewidth',lw); m_plot(lon_gcp(n),lat_gcp(n),'bo','markersize',ms,'linewidth',lw);
% Plot the Image Control Points once georectified
m_plot(LON(i_gcp(n),j_gcp(n)),LAT(i_gcp(n),j_gcp(n)),'rx','MarkerSize',ms,'linewidth',lw); m_plot(LON(i_gcp(n),j_gcp(n)),LAT(i_gcp(n),j_gcp(n)),'rx','MarkerSize',ms,'linewidth',lw);
end end
...@@ -118,7 +122,7 @@ end ...@@ -118,7 +122,7 @@ end
clear LON LAT X Y clear LON LAT X Y
m_grid_old('box','fancy','fontsize',fs); m_grid('box','fancy','fontsize',fs);
%m_gshhs('f','patch', 'gray'); %m_gshhs('f','patch', 'gray');
%print('g_rect_drone_1', '-dpng') %print('g_rect_drone_1', '-dpng')
if nargout > 1 if nargout > 1
......
function g_viz_lab(imgFname,outputFname); function g_viz_lab(imgFname,rectFile);
% Set some plotting parameters % Set some plotting parameters
ms = 10; % Marker Size ms = 10; % Marker Size
fs = 10; % Font size fs = 10; % Font size
lw = 2; % Line width lw = 2; % Line width
load(outputFname); load(rectFile);