% make atlas plots

atlas = load_atlas2d(fbase);

if atlas.Adj(1,2) == 1
    j = find(atlas.Adj);
    % this fixes old style adj mats
    lut = [4 3 1 1];
    atlas.Adj(j) = lut(atlas.Adj(j));
    atlas.Prob = atlas.Adj;
end

atlas1 = map_projection(atlas,1)
%atlas2 = map_projection(atlas,0)
[atlas2,err] = optimize_global_map(atlas);

if ~exist('save_figures'), save_figures = 0; end

save_figures = 0;
%basename = 'c:/mike/sydney_tree_data/scanmatch_';
%basename = 'c:/mike/mine_run3_';
%basename = 'c:/mike/laser_and_sonar_mit_';
%basename = 'c:/mike/ten_loops_scanmatch_';
%basename = 'c:/mike/hurdles_aug_8_fast_';
%basename = 'c:/users/mike/my_papers/Thesis/tex/figures/laserlines/tenloops_';
basename = 'c:/users/mike/my_papers/Thesis/tex/figures/laserlines/killian_uncorrected_';
%basename = 'c:/users/mike/documents/Thesis/tex/figures/scanmatch/killian_';
%basename = 'c:/users/mike/documents/Thesis/tex/figures/scanmatch/victoria_park_';
%basename = 'c:/users/mike/documents/Thesis/tex/figures/scanmatch/tenloops_';
%basename = 'c:/users/mike/documents/Thesis/tex/figures/scanmatch/mines_';
%basename = 'c:/users/mike/documents/Thesis/tex/figures/sonar/killian_';

if exist('angle')~=1, angle = 0.0; end
%angle = -0.3630;

fig = 0;

fig = fig+1; figure(fig)
draw_atlas2d(atlas1,0,1,-angle)%, axis([-10 10 -20 20])
title('dijkstra projected map')
if save_figures, print(gcf,'-depsc2',[basename 'dijkstra_map.eps']); end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'dijkstra_map.jpg']); end

fig = fig+1; figure(fig)
draw_atlas2d(atlas2,0,1,-angle)%, axis([-10 10 -20 20])
title('optimized global map')
if save_figures, print(gcf,'-depsc2',[basename 'optimized_map.eps']); end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'optimized_map.jpg']); end

fig = fig+1; figure(fig)
plot_adjacency(atlas.Adj), axis square
title('Atlas adjacency matrix')
if save_figures, print(gcf,'-depsc2',[basename 'adjacency_matrix.eps']) ; end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'adjacency_matrix.jpg']) ; end

fig = fig+1; figure(fig)
get_map_times(atlas);
if save_figures, print(gcf,'-depsc2',[basename 'map_times.eps']) ; end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'map_times.jpg']) ; end

if isfield(atlas.res(1),'laserlines_res')
if all(size(atlas.res(2).laserlines_res))
    lres = cat(2,atlas.res.laserlines_res)';
    mean(lres(:,3:5))
    std(lres(:,3:5))
    
    g = find(lres(:,3));
    laser_mean_sqerr_rho = mean(lres(g,3).^2).^.5
    laser_mean_sqerr_phi = mean(lres(g,4).^2).^.5 *180/pi
    
    fig = fig+1; figure(fig)
    g = find(lres(:,3));
    hist(lres(g,3)*100,-20:.5:20)
    xlabel cm
    if save_figures, print(gcf,'-depsc2',[basename 'res_rho.eps']) ; end
    if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_rho.jpg']) ; end
    
    fig = fig+1; figure(fig)
    g = find(lres(:,4));
    hist(lres(g,4)*180/pi,-10:.5:10)
    xlabel deg
    if save_figures, print(gcf,'-depsc2',[basename 'res_phi.eps']) ; end
    if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_phi.jpg']) ; end
end
if all(size(atlas.res(2).sonarpoint_res))
    spres = cat(2,atlas.res.sonarpoint_res)';
    g = find(spres(:,3));
    sonar_mean_sqerr_point = mean(spres(g,3).^2).^.5

    slres = cat(2,atlas.res.sonarline_res)';
    g = find(slres(:,3));
    sonar_mean_sqerr_point = mean(slres(g,3).^2).^.5

    fig = fig+1; figure(fig)
    g = find(spres(:,3));
    hist(spres(g,3)*100,-10:.2:10)
    xlabel(' sonar point res (cm)')
    if save_figures, print(gcf,'-depsc2',[basename 'res_sp.eps']) ; end
    if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_sp.jpg']) ; end

    fig = fig+1; figure(fig)
    g = find(slres(:,3));
    hist(slres(g,3)*100,-10:.2:10)
    xlabel(' sonar line res (cm)')
    if save_figures, print(gcf,'-depsc2',[basename 'res_sl.eps']) ; end
    if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_sl.jpg']) ; end
end

track = cat(2,atlas.map.track)';
b = find(abs(diff(track(:,1)))>1.0);
track(b,:) = nan;
track(:,1) = track(:,1)-track(1,1);

fig = fig+1; figure(fig)
plot(track(:,1)/60,100*track(:,14),'b');
%hold on; plot(track(:,1),track(:,15)*[3 -3]*100,'r'); hold off
xlabel('time (min)'); ylabel('odo bias (%)')
if save_figures, print(gcf,'-depsc2',[basename 'odo_bias.eps']) ; end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'odo_bias.jpg']) ; end

else
%scanmatch residuals    
    smres = cat(2,atlas.res.scanmatch_res)';
    g = find(smres(:,3));
    scanmatch_mean_sqerr = mean(smres(g,3:5).^2).^.5*diag([100 100 180/pi])
    
    fig = fig+1; figure(fig)
    subplot(4,3,[1 4 7])
    hist(smres(g,3)*100,-10:.1:10),set(gca,'xlim',[-10 10])
    xlabel('x res (cm)');
    subplot(4,3,[2 5 8])
    hist(smres(g,4)*100,-10:.1:10),set(gca,'xlim',[-10 10])
    xlabel('y res (cm)');
    subplot(4,3,[3 6 9])
    hist(smres(g,5)*180/pi,-3:.01:3),set(gca,'xlim',[-3 3])
    xlabel('\theta res (degrees)');
    pp = get(gcf,'position'); pp(3:4) = [600 200]; set(gcf,'position',pp)
    set(gcf,'paperposition',[1 1 8 8/3])
    subplot(111)
    if save_figures, print(gcf,'-depsc2',[basename 'res_sm.eps']) ; end
    if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_sm.jpg']) ; end
    
end

fig = fig+1; figure(fig)
ind = find(atlas.Adj > 1);
subplot(4,3,[1 4 7])
hist(err(1,ind),20)
xlabel('x res (meters)');
subplot(4,3,[2 5 8])
hist(err(2,ind),20)
xlabel('y res (meters)');
subplot(4,3,[3 6 9])
hist(err(3,ind)*180/pi,20)
xlabel('\theta res (degrees)');
pp = get(gcf,'position'); pp(3:4) = [600 200]; set(gcf,'position',pp)
set(gcf,'paperposition',[1 1 6 2])
subplot(111)
if save_figures, print(gcf,'-depsc2',[basename 'res_optim.eps']) ; end
if save_figures, print(gcf,'-djpeg75','-r100',[basename 'res_optim.jpg']) ; end

