% Requires: rtcat

close all;clc;clear all

%% First pose.
params.position = [-0.3;0.1;deg2rad(10)];
%% Step.
truth_cov = diag([0.35^2 0.35^2 deg2rad(10.5)^2]);
params.truth = sample_normal([0;0;0], truth_cov, 1);
%% Covariance of the first guess.
params.guess_cov = diag([0.35^2 0.35^2 deg2rad(7.5)^2]);
params.fov = 1.4*pi;
params.nrays = 180;
params.sigma = 0.01;
params.epsilon_xy=0.00001;
params.epsilon_theta=deg2rad(0.0001);

%% Second pose.
params.position2 = rtcat(params.position, params.truth);

L = 5;
seglist = { 0.4*L*[-1.2 1; -1 -1]', L*[1.5 1; 1 -1]', ...
        0.3*L*[1 -1.3; -1 -1]', L*[1.4 1; -1 1]'}

params.laser_ref  = ...
    ray_tracing_polygon(seglist, params.position, 180, params.fov, inf);
params.laser_sens = ...
    ray_tracing_polygon(seglist, params.position2, params.nrays, params.fov, inf);

params.laser_ref.estiamte=[0;0;0];
params.laser_sens.estimate=[0;0;0];

params.interactive = false;

%% Plot stuff


figure('position',[1000 50 900 900])
axis([-10 10 -10 10]); axis square; grid on 
set(gca, 'LooseInset', get(gca,'TightInset'))

% plotLaserData(params.laser_ref,params)

% params.color = 'b.';

% plotLaserData(params.laser_sens,params)

params.interactive=1

ld.color='r.';
ld_plot(params.laser_ref,ld)

ld.color='b.';
ld_plot(params.laser_sens,ld)

% res = icp2(params);	
res = gpm(params);	
% res.rototranslated=1;

% figure('position',[1000 50 900 900])
% % axis([-10 10 -10 10]); axis square; grid on 
% set(gca, 'LooseInset', get(gca,'TightInset'))

% ld.color='r.';
% ld_plot(res.laser_ref,ld)

res.laser_sens.estimate=res.X;

ld.color='r.';
ld_plot(res.laser_sens,ld)
% figure('position',[1000 50 900 900])
% axis([-8 8 -8 8]); axis square; grid on 
% set(gca, 'LooseInset', get(gca,'TightInset'))

% res.color='.r';
% plotLaserData(res.laser_ref, res)

% res.color='.b';
% plotLaserData(res.laser_sens, res)

% plotLaserData(res.laser_ref, res)
% % 
% params.color = 'b.';
% 
% plotLaserData(params.laser_sens, params)
