ollo5 = imread('ollo5.jpg');
ollo5_gray = rgb2gray(ollo5);
figure, imshow(ollo5_gray);

ollo6 = imread('ollo6.jpg');
ollo6_gray = rgb2gray(ollo6);
figure, imshow(ollo6_gray);


ollo5_canny = edge(ollo5_gray, 'canny', 0.1);
ollo6_canny = edge(ollo6_gray, 'canny', 0.1);

[ollo5_points_x, ollo5_points_y, v] = find(ollo5_canny>0);
[ollo6_points_x, ollo6_points_y, v] = find(ollo6_canny>0);

ollo5_points = [ollo5_points_x, ollo5_points_y];
ollo6_points = [ollo6_points_x, ollo6_points_y];

[R,T] = icp(ollo5_points', ollo6_points', 200, 50);

t_ollo6_points = zeros(size(ollo6_points));

npoints = length(ollo6_points);
for j=1:npoints;
    t_ollo6_points(j,:) = R*ollo6_points(j,:)'+T; 
end;

t_ollo6_points_i = int32(t_ollo6_points);

reg_ollo5 = ollo5;
reg_ollo5(:,:,1) = ollo5_gray(:,:);
reg_ollo5(:,:,2) = ollo6_canny(:,:) * 255;
reg_ollo5(:,:,3) = 0;

for i = 1:npoints;
    if ( t_ollo6_points_i(i,1)> 0 &&  t_ollo6_points_i(i,2)> 0 );
        reg_ollo5(t_ollo6_points_i(i,1), t_ollo6_points_i(i,2), 3) = 255;
    end
end;

imshow(reg_ollo5);




