Squashed commit of the sb-vbs branch.

Includes the SD-VBS benchmarks modified to:
- Use libextra to loop as realtime jobs
- Preallocate memory before starting their main computation
- Accept input via stdin instead of via argc

Does not include the SD-VBS matlab code.

Fixes libextra execution in LITMUS^RT.

74 files changed, 8209 insertions, 0 deletions

diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m
new file mode 100755
index 0000000..a82f583
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m
@@ -0,0 +1,391 @@
+function [fc_2,Rc_2,Tc_2,H_2,distance,V_vert,V_hori,x_all_c,V_hori_pix,V_vert_pix,V_diag1_pix,V_diag2_pix]=Distor2Calib(k_dist,grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,f_ini,N_iter,two_focal);
+
+% Computes the calibration parameters knowing the
+% distortion factor k_dist
+
+% grid_pts_centered are the grid point coordinates after substraction of
+% the optical center.
+
+% can give an optional guess for the focal length f_ini (can set to [])
+% can provide the number of iterations for the Iterative Vanishing Point Algorithm
+
+% if the focal length is known perfectly, then, there is no need to iterate,
+% and therefore, one can fix: N_iter = 0;
+
+% California Institute of Technology
+% (c) Jean-Yves Bouguet - October 7th, 1997
+
+
+
+%keyboard;
+
+if exist('two_focal'),
+   if isempty(two_focal),
+      two_focal=0;
+   end;
+else
+   two_focal = 0;
+end;
+
+
+if exist('N_iter'),
+   if ~isempty(N_iter),
+      disp('Use number of iterations provided');
+   else
+      N_iter = 10;
+   end;
+else
+   N_iter = 10;
+end;
+
+if exist('f_ini'),
+   if ~isempty(f_ini),
+      disp('Use focal provided');
+      if length(f_ini)<2, f_ini=[f_ini;f_ini]; end;
+      fc_2 = f_ini;
+      x_all_c = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)];
+      x_all_c = comp_distortion(x_all_c,k_dist); % we can this time!!!
+   else
+     fc_2 = [1;1];
+     x_all_c = grid_pts_centered;
+   end;
+else
+   fc_2 = [1;1];
+   x_all_c = grid_pts_centered;
+end;
+
+
+dX = W/n_sq_x;
+dY = L/n_sq_y;
+
+
+N_x = n_sq_x+1;
+N_y = n_sq_y+1;
+
+
+x_grid = zeros(N_x,N_y);
+y_grid = zeros(N_x,N_y);
+
+
+
+
+
+%%% Computation of the four vanishing points in pixels
+
+
+   x_grid(:) = grid_pts_centered(1,:);
+   y_grid(:) = grid_pts_centered(2,:);
+         
+   for k=1:n_sq_x+1,
+      [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]);
+      vert(:,k) = U(:,3);
+   end;
+   
+   for k=1:n_sq_y+1,
+      [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]);
+      hori(:,k) = U(:,3);
+   end;
+   
+   % 2 principle Vanishing points:
+   [U,S,V] = svd(vert);
+   V_vert = U(:,3);
+   [U,S,V] = svd(hori);
+   V_hori = U(:,3);
+   
+   
+
+   % Square warping:
+   
+   
+   vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert;
+   vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert;
+   
+   hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori;
+   hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori;
+   
+   
+   x1 = cross(hori_first,vert_first);
+   x2 = cross(hori_first,vert_last);
+   x3 = cross(hori_last,vert_last);
+   x4 = cross(hori_last,vert_first);
+   
+   x1 = x1/x1(3);
+   x2 = x2/x2(3);
+   x3 = x3/x3(3);
+   x4 = x4/x4(3);
+   
+   
+   
+   [square] = Rectangle2Square([x1 x2 x3 x4],W,L);
+
+   y1 = square(:,1);
+   y2 = square(:,2);
+   y3 = square(:,3);
+   y4 = square(:,4);
+
+   H2 = cross(V_vert,V_hori);
+   
+   V_diag1 = cross(cross(y1,y3),H2);
+   V_diag2 = cross(cross(y2,y4),H2);
+
+   V_diag1 = V_diag1 / norm(V_diag1);
+   V_diag2 = V_diag2 / norm(V_diag2);
+
+   V_hori_pix = V_hori;
+   V_vert_pix = V_vert;
+   V_diag1_pix = V_diag1;
+   V_diag2_pix = V_diag2;
+
+
+% end of computation of the vanishing points in pixels.
+
+
+
+
+
+
+
+
+if two_focal, % only if we attempt to estimate two focals...
+   % Use diagonal lines also to add two extra vanishing points (?)
+   N_min = min(N_x,N_y);
+   
+   if N_min < 2,
+      use_diag = 0;
+      two_focal = 0;
+      disp('Cannot estimate two focals (no existing diagonals)');   
+   else
+      use_diag = 1;
+      Delta_N = abs(N_x-N_y);
+      N_extra = round((N_min - Delta_N - 1)/2);
+      diag_list = -N_extra:Delta_N+N_extra;
+      N_diag = length(diag_list);
+      diag_1 = zeros(3,N_diag);
+      diag_2 = zeros(3,N_diag);
+   end;
+else   
+   % Give up the use of the diagonals (so far)
+   % it seems that the error is increased
+   use_diag = 0;
+end;
+
+
+
+% The vertical lines: vert, Horizontal lines: hori
+vert = zeros(3,n_sq_x+1);
+hori = zeros(3,n_sq_y+1);
+ 
+for counter_k = 1:N_iter,       % the Iterative Vanishing Points Algorithm to
+                                % estimate the focal length accurately
+   
+   x_grid(:) = x_all_c(1,:);
+   y_grid(:) = x_all_c(2,:);
+         
+   for k=1:n_sq_x+1,
+      [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]);
+      vert(:,k) = U(:,3);
+   end;
+   
+   for k=1:n_sq_y+1,
+      [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]);
+      hori(:,k) = U(:,3);
+   end;
+   
+   % 2 principle Vanishing points:
+   [U,S,V] = svd(vert);
+   V_vert = U(:,3);
+   [U,S,V] = svd(hori);
+   V_hori = U(:,3);
+   
+   
+
+   % Square warping:
+   
+   
+   vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert;
+   vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert;
+   
+   hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori;
+   hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori;
+   
+   
+   x1 = cross(hori_first,vert_first);
+   x2 = cross(hori_first,vert_last);
+   x3 = cross(hori_last,vert_last);
+   x4 = cross(hori_last,vert_first);
+   
+   x1 = x1/x1(3);
+   x2 = x2/x2(3);
+   x3 = x3/x3(3);
+   x4 = x4/x4(3);
+   
+   
+   
+   [square] = Rectangle2Square([x1 x2 x3 x4],W,L);
+
+   y1 = square(:,1);
+   y2 = square(:,2);
+   y3 = square(:,3);
+   y4 = square(:,4);
+
+   H2 = cross(V_vert,V_hori);
+   
+   V_diag1 = cross(cross(y1,y3),H2);
+   V_diag2 = cross(cross(y2,y4),H2);
+
+   V_diag1 = V_diag1 / norm(V_diag1);
+   V_diag2 = V_diag2 / norm(V_diag2);
+
+   
+   
+   
+   % Estimation of the focal length, and normalization:
+   
+   % Compute the ellipsis of (1/f^2) positions:
+   % a * (1/fx)^2 + b * (1/fx)^2 = -c
+   
+   
+   a1 = V_hori(1);
+   b1 = V_hori(2);
+   c1 = V_hori(3);
+   
+   a2 = V_vert(1);
+   b2 = V_vert(2);
+   c2 = V_vert(3);
+   
+   a3 = V_diag1(1);
+   b3 = V_diag1(2);
+   c3 = V_diag1(3);
+   
+   a4 = V_diag2(1);
+   b4 = V_diag2(2);
+   c4 = V_diag2(3);
+   
+   
+   if two_focal,
+      
+      
+      A = [a1*a2 b1*b2;a3*a4 b3*b4];
+      b = -[c1*c2;c3*c4];
+      
+      f = sqrt(abs(1./(inv(A)*b)));
+
+   else
+      
+      f = sqrt(abs(-(c1*c2*(a1*a2 + b1*b2) + c3*c4*(a3*a4 + b3*b4))/(c1^2*c2^2 + c3^2*c4^2)));
+      
+      f = [f;f];
+      
+   end;
+   
+
+   
+   % REMARK:
+   % if both a and b are small, the calibration is impossible.
+   % if one of them is small, only the other focal length is observable
+   % if none is small, both focals are observable
+   
+   
+   fc_2 = fc_2 .* f;
+   
+      
+   % DEBUG PART: fix focal to 500...
+   %fc_2= [500;500]; disp('Line 293 to be earased in Distor2Calib.m');
+   
+   
+   % end of focal compensation
+   
+   % normalize by the current focal:
+   
+   x_all = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)];
+   
+   % Compensate by the distortion factor:
+   
+   x_all_c = comp_distortion(x_all,k_dist);
+   
+end;
+   
+% At that point, we hope that the distortion is gone...
+
+x_grid(:) = x_all_c(1,:);
+y_grid(:) = x_all_c(2,:);
+
+for k=1:n_sq_x+1,
+   [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]);
+   vert(:,k) = U(:,3);
+end;
+
+for k=1:n_sq_y+1,
+   [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]);
+   hori(:,k) = U(:,3);
+end;
+
+% Vanishing points:
+[U,S,V] = svd(vert);
+V_vert = U(:,3);
+[U,S,V] = svd(hori);
+V_hori = U(:,3);
+
+% Horizon:
+
+H_2 = cross(V_vert,V_hori);
+   
+%   H_2 = cross(V_vert,V_hori);
+
+% pick a plane in front of the camera (positive depth)
+if H_2(3) < 0, H_2 = -H_2; end;
+
+
+% Rotation matrix:
+
+if V_hori(1) < 0, V_hori = -V_hori; end;
+
+V_hori = V_hori/norm(V_hori);
+H_2 = H_2/norm(H_2);
+
+V_hori = V_hori - dot(V_hori,H_2)*H_2;
+
+Rc_2 = [V_hori cross(H_2,V_hori) H_2];
+
+Rc_2 = Rc_2 / det(Rc_2);
+
+%omc_2 = rodrigues(Rc_2);
+
+%Rc_2 = rodrigues(omc_2);
+
+% Find the distance of the plane for translation vector:
+
+xc_2 = [x_all_c;ones(1,Np)];
+
+Zc_2 = 1./sum(xc_2 .* (Rc_2(:,3)*ones(1,Np)));
+
+Xo_2 = [sum(xc_2 .* (Rc_2(:,1)*ones(1,Np))).*Zc_2 ; sum(xc_2 .* (Rc_2(:,2)*ones(1,Np))).*Zc_2];
+
+XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np);
+
+distance_x = norm(Xgrid_2(1,:))/norm(XXo_2(1,:));
+distance_y = norm(Xgrid_2(2,:))/norm(XXo_2(2,:));
+
+
+distance = sum(sum(XXo_2(1:2,:).*Xgrid_2(1:2,:)))/sum(sum(XXo_2(1:2,:).^2));
+
+alpha = abs(distance_x - distance_y)/distance;
+
+if (alpha>0.1)&~two_focal,
+   disp('Should use two focals in x and y...');
+end;
+
+% Deduce the translation vector:
+
+Tc_2 = distance * H_2;
+
+
+
+
+
+return;
+
+   V_hori_pix/V_hori_pix(3)
+   V_vert_pix/V_vert_pix(3)
+   V_diag1_pix/V_diag1_pix(3)
+   V_diag2_pix/V_diag2_pix(3)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m
new file mode 100755
index 0000000..a6bbbe5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m
@@ -0,0 +1,19 @@
+function [square] = Rectangle2Square(rectangle,L,W);
+
+% Generate the square from a rectangle of known segment lengths
+% from pt1 to pt2 : L
+% from pt2 to pt3 : W
+
+[u_hori,u_vert] = UnWarpPlane(rectangle);
+
+coeff_x = sqrt(W/L);
+coeff_y = 1/coeff_x;
+
+x_coord = [ 0 coeff_x  coeff_x 0];
+y_coord = [ 0 0 coeff_y coeff_y];
+
+
+square = rectangle(:,1) * ones(1,4) + u_hori*x_coord + u_vert*y_coord;
+square = square ./ (ones(3,1)*square(3,:));
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m
new file mode 100755
index 0000000..8addf52
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m
@@ -0,0 +1,54 @@
+function  [u_hori,u_vert] = UnWarpPlane(x1,x2,x3,x4);
+
+% Recovers the two 3D directions of the rectangular patch x1x2x3x4
+% x1 is the origin point, ie any point of planar coordinate (x,y) on the
+% rectangular patch will be projected on the image plane at:
+% x1 + x * u_hori + y * u_vert
+%
+% Note: u_hori and u_vert are also the two vanishing points.
+
+
+if nargin < 4,
+   
+   x4 = x1(:,4);
+   x3 = x1(:,3);
+   x2 = x1(:,2);
+   x1 = x1(:,1);
+   
+end;
+
+
+% Image Projection:
+L1 = cross(x1,x2);
+L2 = cross(x4,x3);
+L3 = cross(x2,x3);
+L4 = cross(x1,x4);
+
+% Vanishing point:
+V1 = cross(L1,L2);
+V2 = cross(L3,L4);
+
+% Horizon line:
+H = cross(V1,V2);
+
+if H(3) < 0, H  = -H; end;
+
+
+H = H / norm(H);
+
+
+X1 = x1 / dot(H,x1);
+X2 = x2 / dot(H,x2);
+X3 = x3 / dot(H,x3);
+X4 = x4 / dot(H,x4);
+
+scale = X1(3);
+
+X1 = X1/scale;
+X2 = X2/scale;
+X3 = X3/scale;
+X4 = X4/scale;
+
+
+u_hori = X2 - X1;
+u_vert = X4 - X1;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m
new file mode 100755
index 0000000..a8a32c0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m
@@ -0,0 +1,98 @@
+
+if ~exist('n_ima'),
+   fprintf(1,'No data to process.\n');
+   return;
+end;
+
+
+check_active_images;
+
+
+fprintf(1,'\nThis function is useful to select a subset of images to calibrate\n');
+
+   fprintf(1,'\nThere are currently %d active images selected for calibration (out of %d):\n',length(ind_active),n_ima);
+   
+   if ~isempty(ind_active),
+   
+        for ii = 1:length(ind_active)-2,
+        
+         fprintf(1,'%d, ',ind_active(ii));
+         
+      end;
+      
+      fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end));
+
+   end;
+   
+   fprintf(1,'\n');
+   
+   
+fprintf(1,'\nDo you want to suppress or add images from that list?\n');
+
+choice = 2;
+
+while (choice~=0)&(choice~=1),
+   choice = input('For suppressing images enter 0, for adding images enter 1 ([]=no change): ');
+   if isempty(choice),
+      fprintf(1,'No change applied to the list of active images.\n');
+      return;
+   end;
+   if (choice~=0)&(choice~=1),
+      disp('Bad entry. Try again.');
+   end;
+end;
+
+
+if choice,
+   
+        ima_numbers = input('Number(s) of image(s) to add ([] = all images) = ');
+
+if isempty(ima_numbers),
+           fprintf(1,'All %d images are now active\n',n_ima);
+        ima_proc = 1:n_ima;
+        else
+        ima_proc = ima_numbers;
+        end;
+   
+else
+   
+   
+        ima_numbers = input('Number(s) of image(s) to suppress ([] = no image) = ');
+
+        if isempty(ima_numbers),
+      fprintf(1,'No image has been suppressed. No modication of the list of active images.\n',n_ima);
+        ima_proc = [];
+        else
+        ima_proc = ima_numbers;
+        end;
+   
+end;
+
+if ~isempty(ima_proc),
+   
+   active_images(ima_proc) = choice * ones(1,length(ima_proc));
+   
+end;
+
+
+   check_active_images;
+   
+
+   fprintf(1,'\nThere is now a total of %d active images for calibration:\n',length(ind_active));
+   
+   if ~isempty(ind_active),
+   
+        for ii = 1:length(ind_active)-2,
+        
+         fprintf(1,'%d, ',ind_active(ii));
+         
+      end;
+      
+      fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end));
+
+   end;
+   
+   fprintf(1,'\n\nYou may now run ''Calibration'' to recalibrate based on this new set of images.\n');
+   
+   
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m
new file mode 100755
index 0000000..7a9cf0f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m
@@ -0,0 +1,141 @@
+% Color code for each image:
+
+if ~exist('n_ima')|~exist('fc'),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+check_active_images;
+
+if ~exist(['ex_' num2str(ind_active(1)) ]),
+   fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n');
+   return;
+end;
+
+
+%if ~exist('no_grid'),
+   no_grid = 0;
+%end;
+
+colors = 'brgkcm';
+
+   
+figure(5);
+   
+for kk = 1:n_ima,
+   if exist(['y_' num2str(kk)]),
+   if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']),
+      
+      if ~no_grid,
+         eval(['XX_kk = X_' num2str(kk) ';']);
+         N_kk = size(XX_kk,2);
+         
+         if ~exist(['n_sq_x_' num2str(kk)]),
+            no_grid = 1;
+         end;
+         
+         if ~no_grid,
+            eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
+            eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
+            if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))),
+               no_grid = 1;
+            end;
+         end;
+      end;
+      
+      eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']);
+      
+      hold on;
+   end;
+   end;
+end;
+
+hold off;
+axis('equal');
+if 1, %~no_grid,
+   title('Reprojection error (in pixel) - To exit: right button');
+else
+   title('Reprojection error (in pixel)');   
+end;
+xlabel('x');
+ylabel('y');
+
+set(5,'Name','error','NumberTitle','off');
+
+
+
+err_std = std(ex')';
+
+fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f] (all active images)\n\n',err_std); 
+
+
+b = 1;
+
+while b==1,
+   
+[xp,yp,b] = ginput3(1);
+
+if b==1,
+ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2;
+
+[mind,indmin] = min(ddd);
+
+
+done = 0;
+kk_ima = 1;
+while (~done)&(kk_ima<=n_ima),
+   %fprintf(1,'%d...',kk_ima);
+   eval(['ex_kk = ex_' num2str(kk_ima) ';']);
+   sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin)));
+   if isempty(sol_kk),
+        kk_ima = kk_ima + 1;
+   else
+      done = 1;
+   end;
+end;
+
+if ~no_grid,
+
+eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']);
+eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']);
+
+Nx = n_sq_x+1;
+Ny = n_sq_y+1;
+
+y1 = floor((sol_kk-1)./Nx);
+x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx);
+
+y1 = (n_sq_y+1) - y1;
+x1 = x1 + 1;
+
+fprintf(1,'\nSelected image: %d\nSelected point: (col,row)=(%d,%d)\nNcol=%d, Nrow=%d\n',[kk_ima x1 y1 Nx Ny]);
+fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]);
+
+else
+   
+   eval(['x_kk = x_' num2str(kk_ima) ';']);
+   
+   xpt = x_kk(:,sol_kk);
+   
+fprintf(1,'\nSelected image: %d\nImage coordinates (in pixel): (%3.2f,%3.2f)\n',[kk_ima xpt']);
+fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]);
+
+
+end;
+
+
+if exist(['wintx_' num2str(kk_ima)]),
+
+   eval(['wintx = wintx_' num2str(kk_ima) ';']);
+   eval(['winty = winty_' num2str(kk_ima) ';']);
+   
+   fprintf(1,'Window size: wintx = %d, winty = %d\n',[wintx winty]);
+end;
+
+
+end;
+
+end;
+
+disp('done');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m
new file mode 100755
index 0000000..f5c5b48
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m
@@ -0,0 +1,137 @@
+function [xd,dxddk] = apply_distortion(x,k)
+
+
+[m,n] = size(x);
+
+% Add distortion:
+
+r2 = x(1,:).^2 + x(2,:).^2;
+
+r4 = r2.^2;
+
+% Radial distortion:
+
+cdist = 1 + k(1) * r2 + k(2) * r4;
+
+if nargout > 1,
+        dcdistdk = [ r2' r4' zeros(n,2)];
+end;
+
+
+xd1 = x .* (ones(2,1)*cdist);
+
+coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
+
+if nargout > 1,
+        dxd1dk = zeros(2*n,4);
+        dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
+        dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;
+end;
+
+
+% tangential distortion:
+
+a1 = 2.*x(1,:).*x(2,:);
+a2 = r2 + 2*x(1,:).^2;
+a3 = r2 + 2*x(2,:).^2;
+
+delta_x = [k(3)*a1 + k(4)*a2 ;
+   k(3) * a3 + k(4)*a1];
+
+aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
+bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
+cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);
+
+if nargout > 1,
+        ddelta_xdk = zeros(2*n,4);
+        ddelta_xdk(1:2:end,3) = a1';
+        ddelta_xdk(1:2:end,4) = a2';
+        ddelta_xdk(2:2:end,3) = a3';
+        ddelta_xdk(2:2:end,4) = a1';
+end;
+
+xd = xd1 + delta_x;
+
+if nargout > 1,
+        dxddk = dxd1dk + ddelta_xdk ;
+end;
+
+
+return;
+
+% Test of the Jacobians:
+
+n = 10;
+
+X = 10*randn(3,n);
+om = randn(3,1);
+T = [10*randn(2,1);40];
+f = 1000*rand(2,1);
+c = 1000*randn(2,1);
+k = 0.5*randn(4,1);
+
+
+[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k);
+
+
+% Test on om: NOT OK
+
+dom = 0.000000001 * norm(om)*randn(3,1);
+om2 = om + dom;
+
+[x2] = project_points(X,om2,T,f,c,k);
+
+x_pred = x + reshape(dxdom * dom,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on T: OK!!
+
+dT = 0.0001 * norm(T)*randn(3,1);
+T2 = T + dT;
+
+[x2] = project_points(X,om,T2,f,c,k);
+
+x_pred = x + reshape(dxdT * dT,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+
+% Test on f: OK!!
+
+df = 0.001 * norm(f)*randn(2,1);
+f2 = f + df;
+
+[x2] = project_points(X,om,T,f2,c,k);
+
+x_pred = x + reshape(dxdf * df,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on c: OK!!
+
+dc = 0.01 * norm(c)*randn(2,1);
+c2 = c + dc;
+
+[x2] = project_points(X,om,T,f,c2,k);
+
+x_pred = x + reshape(dxdc * dc,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
+
+% Test on k: OK!!
+
+dk = 0.001 * norm(4)*randn(4,1);
+k2 = k + dk;
+
+[x2] = project_points(X,om,T,f,c,k2);
+
+x_pred = x + reshape(dxdk * dk,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m
new file mode 100755
index 0000000..d591d03
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m
@@ -0,0 +1,117 @@
+fig_number = 1;
+
+n_row = 4;
+n_col = 4;
+
+string_list = cell(n_row,n_col);
+callback_list = cell(n_row,n_col);
+
+x_size = 85;
+y_size = 14;
+gap_x = 0;
+font_name = 'clean';
+font_size = 8;
+
+title_figure = 'Camera Calibration Toolbox';
+
+string_list{1,1} = 'Image names';
+string_list{1,2} = 'Read images';
+string_list{1,3} = 'Extract grid corners';
+string_list{1,4} = 'Calibration';
+string_list{2,1} = 'Show Extrinsic';
+string_list{2,2} = 'Reproject on images';
+string_list{2,3} = 'Analyse error';
+string_list{2,4} = 'Recomp. corners';
+string_list{3,1} = 'Add/Suppress images';
+string_list{3,2} = 'Save';
+string_list{3,3} = 'Load';
+string_list{3,4} = 'Exit';
+
+string_list{4,1} = 'Comp. Extrinsic';
+string_list{4,2} = 'Undistort image';
+string_list{4,3} = 'Export calib data';
+
+
+callback_list{1,1} = 'data_calib;';
+callback_list{1,2} = 'ima_read_calib;';
+callback_list{1,3} = 'click_calib;';
+callback_list{1,4} = 'go_calib_optim;';
+callback_list{2,1} = 'ext_calib;';
+callback_list{2,2} = 'reproject_calib;';
+callback_list{2,3} = 'analyse_error;';
+callback_list{2,4} = 'recomp_corner_calib;';
+callback_list{3,1} = 'add_suppress;';
+callback_list{3,2} = 'saving_calib;';
+callback_list{3,3} = 'loading_calib;';
+callback_list{3,4} = ['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');'];
+
+callback_list{4,1} = 'extrinsic_computation;';
+callback_list{4,2} = 'undistort_image;';
+callback_list{4,3} = 'export_calib_data;';
+
+
+%------- BEGIN PROECTED REGION -----------%
+%------- DO NOT EDIT IN THIS REGION -----------%
+
+figure(fig_number); clf;
+pos = get(fig_number,'Position');
+
+fig_size_x = x_size*n_col+(n_col+1)*gap_x;
+fig_size_y = y_size*n_row+(n_row+1)*gap_x;
+
+set(fig_number,'Units','points', ...
+        'BackingStore','off', ...
+        'Color',[0.8 0.8 0.8], ...
+        'MenuBar','none', ...
+        'Resize','off', ...
+        'Name',title_figure, ...
+'Position',[pos(1) pos(2) fig_size_x fig_size_y], ...
+'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']);
+
+h_mat = zeros(n_row,n_col);
+
+posx = zeros(n_row,n_col);
+posy = zeros(n_row,n_col);
+
+for i=n_row:-1:1,
+   for j = n_col:-1:1,
+      posx(i,j) = gap_x+(j-1)*(x_size+gap_x);
+      posy(i,j) = fig_size_y - i*(gap_x+y_size);
+   end;
+end;
+
+for i=n_row:-1:1,
+   for j = n_col:-1:1,
+      if ~isempty(string_list{i,j}) & ~isempty(callback_list{i,j}),
+         h_mat(i,j) = uicontrol('Parent',fig_number, ...
+            'Units','points', ...
+            'Callback',callback_list{i,j}, ...
+            'ListboxTop',0, ...
+            'Position',[posx(i,j)  posy(i,j)  x_size   y_size], ...
+                                'String',string_list{i,j}, ...
+                                'fontsize',font_size,...
+                                'fontname',font_name,...
+            'Tag','Pushbutton1');
+      end;
+        end;
+end;
+
+%------ END PROTECTED REGION ----------------%
+
+if 0,
+%-- VERSION:
+
+uicontrol('Parent',fig_number, ...
+   'Units','points', ...
+   'ListboxTop',0, ...
+   'Position',[(fig_size_x-x_size/2)-2  -5  x_size/2   y_size], ...
+   'String','ver. 1.0', ...
+   'fontsize',8,...
+   'BackgroundColor',[0.8 0.8 0.8], ...
+   'fontname','clean',...
+   'HorizontalAlignment','right', ...
+   'Style','text');
+end;
+
+
+%clear callback_list string_list fig_number fig_size_x fig_size_y i j n_col n_row pos string_list title_figure x_size y_size font_name font_size gap_x h_mat posx posy
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m
new file mode 100755
index 0000000..fc365a5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m
@@ -0,0 +1,19 @@
+
+if ~exist('active_images'),
+        active_images = ones(1,n_ima);
+end;
+n_act = length(active_images);
+if n_act < n_ima,
+   active_images = [active_images ones(1,n_ima-n_act)];
+else
+   if n_act > n_ima,
+      active_images = active_images(1:n_ima);
+   end;
+end;
+
+ind_active = find(active_images);
+
+if prod(active_images == 0),
+   disp('Error: There is no active image');
+   break
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m
new file mode 100755
index 0000000..c4b13fd
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m
@@ -0,0 +1,48 @@
+%%% Replay the set of solution vectors:
+
+
+if ~exist('param_list'),
+   if ~exist('solution');
+      fprintf(1,'Error: Need to calibrate first\n');
+      return;
+   else
+      param_list = solution;
+   end;
+end;
+
+N_iter = size(param_list,2);
+
+if N_iter == 1, 
+   fprintf(1,'Warning: There is a unique state in the list of parameters.\n');
+end;
+
+
+
+%M = moviein(N_iter);
+
+for nn = 1:N_iter,
+   
+   solution = param_list(:,nn);
+   
+   extract_parameters;
+   comp_error_calib;
+   
+   ext_calib;
+   
+   drawnow;
+   
+%   Mnn = getframe(gcf);
+   
+%   M(:,nn) = Mnn;
+   
+end;
+
+%fig = gcf;
+
+
+%figure(fig+1);
+%close;
+%figure(fig+1);
+
+%clf;
+%movie(M,20);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m
new file mode 100755
index 0000000..dc23149
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m
@@ -0,0 +1,97 @@
+% This small script looks in the direcory and checks if the images are there.
+%
+% This works only on Matlab 5.x (otherwise, the dir commands works differently)
+
+% (c) Jean-Yves Bouguet - Dec. 27th, 1999
+
+l = dir([calib_name '*']);
+
+Nl = size(l,1);
+Nima_valid = 0;
+ind_valid = [];
+loc_extension = [];
+length_name = size(calib_name,2);
+
+if Nl > 0,
+   
+   for pp = 1:Nl,
+      filenamepp =  l(pp).name;
+      iii = findstr(filenamepp,calib_name);
+      
+      loc_ext = findstr(filenamepp,format_image);
+      string_num = filenamepp(length_name+1:loc_ext - 2);
+      
+      if isempty(str2num(string_num)),
+         iii = [];
+      end;
+      
+      
+      if ~isempty(iii),
+                        if (iii(1) ~= 1),
+                         iii = [];
+                        end;
+      end;
+      
+      
+      
+      if ~isempty(iii) & ~isempty(loc_ext),
+         
+                        Nima_valid = Nima_valid + 1;
+                        ind_valid = [ind_valid pp];
+                        loc_extension = [loc_extension loc_ext(1)];
+         
+      end;
+      
+   end;
+   
+   if (Nima_valid==0),
+      
+      fprintf(1,'No image found. File format may be wrong.\n');
+      
+   else
+      
+      % Get all the string numbers:
+      
+      string_length = zeros(1,Nima_valid);
+      indices =  zeros(1,Nima_valid);
+      
+      
+      for ppp = 1:Nima_valid,
+         
+         name = l(ind_valid(ppp)).name;
+         string_num = name(length_name+1:loc_extension(ppp) - 2);
+         string_length(ppp) = size(string_num,2);
+         indices(ppp) = str2num(string_num);
+         
+      end;
+      
+      % Number of images...
+      first_num = min(indices);
+      n_ima = max(indices) - first_num + 1;
+      
+      if min(string_length) == max(string_length),
+         
+         N_slots = min(string_length);
+         type_numbering = 1;
+         
+      else
+        
+         N_slots = 1;
+         type_numbering = 0;
+         
+      end;
+      
+      image_numbers = first_num:n_ima-1+first_num;
+      
+      %%% By default, all the images are active for calibration:
+      
+      active_images = ones(1,n_ima);
+   
+   end;
+   
+else
+   
+   fprintf(1,'No image found. Basename may be wrong.\n');
+  
+end;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m
new file mode 100755
index 0000000..fa7df87
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m
@@ -0,0 +1,37 @@
+check_active_images;
+
+for kk =  ind_active,
+   
+   if ~exist(['x_' num2str(kk)]),
+      
+      fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk);
+      
+      active_images(kk) = 0;
+      
+      eval(['dX_' num2str(kk) ' = NaN;']);
+      eval(['dY_' num2str(kk) ' = NaN;']);  
+      
+      eval(['wintx_' num2str(kk) ' = NaN;']);
+      eval(['winty_' num2str(kk) ' = NaN;']);
+
+      eval(['x_' num2str(kk) ' = NaN*ones(2,1);']);
+      eval(['X_' num2str(kk) ' = NaN*ones(3,1);']);
+      
+      eval(['n_sq_x_' num2str(kk) ' = NaN;']);
+      eval(['n_sq_y_' num2str(kk) ' = NaN;']);
+   
+   else
+      
+      eval(['xkk = x_' num2str(kk) ';']);
+      
+      if isnan(xkk(1)),
+         
+         fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk);
+
+         active_images(kk) = 0;
+         
+      end;
+      
+   end;
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m
new file mode 100755
index 0000000..1eccbd3
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m
@@ -0,0 +1,4 @@
+for kk = 1:n_ima,
+   eval(['clear wintx_' num2str(kk)]);
+   eval(['clear winty_' num2str(kk)]);
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m
new file mode 100755
index 0000000..a04be67
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m
@@ -0,0 +1,10 @@
+% Function that clears all the wintx_i and winty_i
+% In normal operation of the toolbox, this function should not be
+% useful.
+% only in cases where you want to re-extract corners using the Extract grid corners another time... not common. You might as well use the Recomp. corners.
+
+for kk = 1:n_ima,
+   
+   eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]);
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m
new file mode 100755
index 0000000..1a6d2d7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m
@@ -0,0 +1,193 @@
+%if exist('images_read');
+%   active_images = active_images & images_read;
+%end;
+
+var2fix = 'dX_default';
+
+fixvariable;
+
+var2fix = 'dY_default';
+
+fixvariable;
+
+var2fix = 'map';
+
+fixvariable;
+
+
+if ~exist('n_ima'),
+   data_calib;
+end;
+
+check_active_images;
+
+if ~exist(['I_' num2str(ind_active(1))]),
+   ima_read_calib;
+   if isempty(ind_read),
+      disp('Cannot extract corners without images');
+      return;
+   end;
+end;
+
+
+%wintx = 10; % neigborhood of integration for
+%winty = 10; % the corner finder
+
+fprintf(1,'\nExtraction of the grid corners on the images\n');
+
+
+if ~exist('map'), map = gray(256); end;
+
+
+disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!')
+
+if ~exist('dX_default');
+
+% Default size of the pattern squares;
+
+% Setup of JY (old at Caltech)
+%dX_default = 21.9250/11;
+%dY_default = 18.1250/9;
+
+% Setup of JY (new at Intel)
+%dX_default = 1.9750;
+%dY_default = 1.9865;
+
+
+% Setup of Luis and Enrico
+%dX_default = 67.7/16;
+%dY_default = 50.65/12;
+
+
+% Setup of German
+%dX_default = 10.16;
+%dY_default = 10.16;
+
+% Setup of JY (new at Intel)
+%dX_default = 1.9750*2.54;
+%dY_default = 1.9865*2.54;
+
+% Setup of JY - 3D calibration rig at Intel (new at Intel)
+%dX_default = 3;
+%dY_default = 3;
+
+% Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang
+dX_default = 30;
+dY_default = 30;
+
+end;
+
+
+if ~exist('dont_ask'),
+   dont_ask = 0;
+end;
+
+
+if ~dont_ask,
+   ima_numbers = input('Number(s) of image(s) to process ([] = all images) = ');
+else
+   ima_numbers = [];
+end;
+
+if isempty(ima_numbers),
+   ima_proc = 1:n_ima;
+else
+   ima_proc = ima_numbers;
+end;
+
+
+% Useful option to add images:
+kk_first = ima_proc(1); %input('Start image number ([]=1=first): ');
+
+%if isempty(kk_first), kk_first = 1; end;
+
+
+if exist(['wintx_' num2str(kk_first)]),
+   
+   eval(['wintxkk = wintx_' num2str(kk_first) ';']);
+   
+   if isempty(wintxkk) | isnan(wintxkk),
+      
+        disp('Window size for corner finder (wintx and winty):');
+        wintx = input('wintx ([] = 5) = ');
+        if isempty(wintx), wintx = 5; end;
+        wintx = round(wintx);
+        winty = input('winty ([] = 5) = ');
+        if isempty(winty), winty = 5; end;
+        winty = round(winty);
+   
+        fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+     
+     end;
+     
+else
+     
+        disp('Window size for corner finder (wintx and winty):');
+        wintx = input('wintx ([] = 5) = ');
+        if isempty(wintx), wintx = 5; end;
+        wintx = round(wintx);
+        winty = input('winty ([] = 5) = ');
+        if isempty(winty), winty = 5; end;
+        winty = round(winty);
+   
+        fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+     
+end;
+     
+
+for kk = ima_proc,
+   if exist(['I_' num2str(kk)]),
+      click_ima_calib;
+      active_images(kk) = 1;
+   else
+        eval(['dX_' num2str(kk) ' = NaN;']);
+        eval(['dY_' num2str(kk) ' = NaN;']);  
+   
+        eval(['wintx_' num2str(kk) ' = NaN;']);
+        eval(['winty_' num2str(kk) ' = NaN;']);
+
+        eval(['x_' num2str(kk) ' = NaN*ones(2,1);']);
+        eval(['X_' num2str(kk) ' = NaN*ones(3,1);']);
+   
+        eval(['n_sq_x_' num2str(kk) ' = NaN;']);
+        eval(['n_sq_y_' num2str(kk) ' = NaN;']);
+   end;
+end;
+
+
+check_active_images;
+
+
+% Fix potential non-existing variables:
+
+for kk = 1:n_ima,
+   if ~exist(['x_' num2str(kk)]),
+        eval(['dX_' num2str(kk) ' = NaN;']);
+        eval(['dY_' num2str(kk) ' = NaN;']);  
+   
+        eval(['wintx_' num2str(kk) ' = NaN;']);
+        eval(['winty_' num2str(kk) ' = NaN;']);
+
+        eval(['x_' num2str(kk) ' = NaN*ones(2,1);']);
+        eval(['X_' num2str(kk) ' = NaN*ones(3,1);']);
+   
+        eval(['n_sq_x_' num2str(kk) ' = NaN;']);
+        eval(['n_sq_y_' num2str(kk) ' = NaN;']);
+   end;
+end;
+
+
+string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY';
+
+for kk = 1:n_ima,
+   string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)];
+end;
+
+eval(string_save);
+
+disp('done');
+
+return;
+
+go_calib_optim;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m
new file mode 100755
index 0000000..f0fd4ca
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m
@@ -0,0 +1,230 @@
+        % Cleaned-up version of init_calib.m
+   
+   fprintf(1,'\nProcessing image %d...\n',kk);
+   
+        eval(['I = I_' num2str(kk) ';']);
+   
+   if exist(['wintx_' num2str(kk)]),
+      
+      eval(['wintxkk = wintx_' num2str(kk) ';']);
+   
+      if ~isempty(wintxkk) & ~isnan(wintxkk),
+
+         eval(['wintx = wintx_' num2str(kk) ';']);
+         eval(['winty = winty_' num2str(kk) ';']);
+      
+      end;
+   end;
+        
+   
+   fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d\n',wintx,winty,2*wintx+1,2*winty+1);
+
+   
+        figure(2);
+        image(I);
+   colormap(map);
+   
+   title(['Click on the four extreme corners of the rectangular pattern... Image ' num2str(kk)]);
+   
+   disp('Click on the four extreme corners of the rectangular complete pattern...');
+   
+   [x,y] = ginput3(4);
+   
+   [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners
+   
+   x = Xc(1,:)';
+   y = Xc(2,:)';
+   
+   [y,indy] = sort(y);
+   x = x(indy);
+   
+   if (x(2) > x(1)),
+      x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2);
+   else
+      x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1);
+   end;
+   if (x(3) > x(4)),
+      x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4);
+   else
+      x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3);
+   end;
+   
+   x = [x1;x2;x3;x4];
+   y = [y1;y2;y3;y4];
+   
+   
+   figure(2); hold on;
+   plot([x;x(1)],[y;y(1)],'g-');
+   plot(x,y,'og');
+   hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X');
+   set(hx,'color','g','Fontsize',14);
+   hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y');
+   set(hy,'color','g','Fontsize',14);
+   hold off;
+   
+   
+   % Try to automatically count the number of squares in the grid
+   
+   n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx);
+   n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx);
+   n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx);
+   n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx);
+   
+  
+   
+   % If could not count the number of squares, enter manually
+   
+   if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2),
+      
+
+         disp('Could not count the number of squares in the grid. Enter manually.');
+         n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6
+         if isempty(n_sq_x), n_sq_x = 10; end;
+         n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6
+         if isempty(n_sq_y), n_sq_y = 10; end; 
+   
+   else
+               
+      n_sq_x = n_sq_x1;
+      n_sq_y = n_sq_y1;
+      
+   end;
+   
+   
+   % Enter the size of each square
+   
+   dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']);
+        dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']);
+        if isempty(dX), dX = dX_default; else dX_default = dX; end;
+        if isempty(dY), dY = dY_default; else dY_default = dY; end;
+   
+   % Compute the inside points through computation of the planar homography (collineation)
+   
+        a00 = [x(1);y(1);1];
+        a10 = [x(2);y(2);1];
+        a11 = [x(3);y(3);1];
+        a01 = [x(4);y(4);1];
+
+
+        % Compute the planar collineation: (return the normalization matrix as well)
+   
+   [Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]);
+
+
+        % Build the grid using the planar collineation:
+
+        x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x;
+   y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y;
+   pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]';
+   
+   XX = Homo*pts;
+        XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:));
+
+   
+   % Complete size of the rectangle
+   
+   W = n_sq_x*dX;
+   L = n_sq_y*dY;
+   
+   
+   
+   
+   %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   figure(2);
+   hold on;
+   plot(XX(1,:),XX(2,:),'r+');
+   title('The red crosses should be close to the image corners');
+   hold off;
+   
+   disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,');
+   disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)');
+   quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) ');
+  
+   quest_distort = ~isempty(quest_distort);
+   
+   if quest_distort,
+      % Estimation of focal length:
+      c_g = [size(I,2);size(I,1)]/2 + .5;
+                f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1);
+      f_g = mean(f_g);
+      script_fit_distortion;
+   end;
+   %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   
+   
+   
+   
+   
+   Np = (n_sq_x+1)*(n_sq_y+1);
+
+   disp('Corner extraction...');
+   
+   grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points!
+   
+
+   
+   %save all_corners x y grid_pts
+   
+   grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C)
+   
+   
+   
+   ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners
+   ind_orig = (n_sq_x+1)*n_sq_y + 1;
+   xorig = grid_pts(1,ind_orig);
+   yorig = grid_pts(2,ind_orig);
+   dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]');
+   dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]');
+   
+   
+   x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)];
+   y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)];
+
+   
+   figure(3);
+   image(I); colormap(map); hold on;
+   plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+');
+   plot(x_box_kk+1,y_box_kk+1,'-b');
+   plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo');
+   plot(xorig+1,yorig+1,'*m');
+   h = text(xorig-15,yorig-15,'O');
+   set(h,'Color','m','FontSize',14);
+   h2 = text(dxpos(1)-10,dxpos(2)-10,'dX');
+   set(h2,'Color','g','FontSize',14);
+   h3 = text(dypos(1)-25,dypos(2)-3,'dY');
+   set(h3,'Color','g','FontSize',14);
+   xlabel('Xc (in camera frame)');
+   ylabel('Yc (in camera frame)');
+   title('Extracted corners');
+   zoom on;
+   drawnow;
+   hold off;
+   
+   
+   Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)';
+   Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)';
+   Zi = zeros(1,Np);
+   
+   Xgrid = [Xi;Yi;Zi];
+   
+   
+        % All the point coordinates (on the image, and in 3D) - for global optimization:
+
+   x = grid_pts;
+   X = Xgrid;
+
+
+        % Saves all the data into variables:
+
+   eval(['dX_' num2str(kk) ' = dX;']);
+   eval(['dY_' num2str(kk) ' = dY;']);  
+   
+   eval(['wintx_' num2str(kk) ' = wintx;']);
+   eval(['winty_' num2str(kk) ' = winty;']);
+
+   eval(['x_' num2str(kk) ' = x;']);
+   eval(['X_' num2str(kk) ' = X;']);
+   
+   eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']);
+   eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']);
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m
new file mode 100755
index 0000000..7718268
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m
@@ -0,0 +1,482 @@
+        % Cleaned-up version of init_calib.m
+
+        eval(['I = I_' num2str(kk) ';']);
+        
+        figure(2);
+        image(I);
+   colormap(map);
+   
+   
+   
+   
+   
+   %%%%%%%%%%%%%%%%%%%%%%%%% LEFT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+   
+  
+   
+   title(['Click on the four extreme corners of the left rectangular pattern... Image ' num2str(kk)]);
+   
+   disp('Click on the four extreme corners of the left rectangular pattern...');
+   
+   [x,y] = ginput3(4);
+   
+   [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners
+   
+   x = Xc(1,:)';
+   y = Xc(2,:)';
+   
+   [y,indy] = sort(y);
+   x = x(indy);
+   
+   if (x(2) > x(1)),
+      x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2);
+   else
+      x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1);
+   end;
+   if (x(3) > x(4)),
+      x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4);
+   else
+      x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3);
+   end;
+   
+   x = [x1;x2;x3;x4];
+   y = [y1;y2;y3;y4];
+   
+   
+   figure(2); hold on;
+   plot([x;x(1)],[y;y(1)],'g-');
+   plot(x,y,'og');
+   hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X');
+   set(hx,'color','g','Fontsize',14);
+   hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y');
+   set(hy,'color','g','Fontsize',14);
+   hold off;
+   
+   drawnow;
+   
+   
+   % Try to automatically count the number of squares in the grid
+   
+   n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx);
+   n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx);
+   n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx);
+   n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx);
+   
+  
+   
+   % If could not count the number of squares, enter manually
+   
+   if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2),
+      
+
+         disp('Could not count the number of squares in the grid. Enter manually.');
+         n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6
+         if isempty(n_sq_x), n_sq_x = 10; end;
+         n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6
+         if isempty(n_sq_y), n_sq_y = 10; end; 
+   
+   else
+               
+      n_sq_x = n_sq_x1;
+      n_sq_y = n_sq_y1;
+      
+   end;
+   
+   
+   if 1,
+        % Enter the size of each square
+   
+        dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']);
+                dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']);
+                if isempty(dX), dX = dX_default; else dX_default = dX; end;
+                if isempty(dY), dY = dY_default; else dY_default = dY; end;
+      
+   else
+      
+      dX = 3;
+      dY = 3;
+      
+   end;
+   
+   
+   % Compute the inside points through computation of the planar homography (collineation)
+   
+        a00 = [x(1);y(1);1];
+        a10 = [x(2);y(2);1];
+        a11 = [x(3);y(3);1];
+        a01 = [x(4);y(4);1];
+
+
+        % Compute the planart collineation: (return the normalization matrice as well)
+
+        [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01);
+
+
+        % Build the grid using the planar collineation:
+
+        x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x;
+   y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y;
+   pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]';
+   
+   XX = Homo*pts;
+        XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:));
+
+   
+   % Complete size of the rectangle
+   
+   W = n_sq_x*dX;
+   L = n_sq_y*dY;
+   
+   
+   
+   if 1,
+   %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   figure(2);
+   hold on;
+   plot(XX(1,:),XX(2,:),'r+');
+   title('The red crosses should be close to the image corners');
+   hold off;
+   
+   disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,');
+   disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)');
+   quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) ');
+  
+   quest_distort = ~isempty(quest_distort);
+   
+   if quest_distort,
+      % Estimation of focal length:
+      c_g = [size(I,2);size(I,1)]/2 + .5;
+                f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1);
+      f_g = mean(f_g);
+      script_fit_distortion;
+   end;
+   %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   end;   
+   
+   
+   Np = (n_sq_x+1)*(n_sq_y+1);
+
+   disp('Corner extraction...');
+   
+   grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points!
+   
+   %save all_corners x y grid_pts
+   
+   grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C)
+   
+   
+   % Global Homography from plane to pixel coordinates:
+   
+   H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1];
+   % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line)
+   % If it is not done, then this matrix should not appear (in C)
+   H_total = H_total / H_total(3,3);   
+   
+   
+   ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners
+   ind_orig = (n_sq_x+1)*n_sq_y + 1;
+   xorig = grid_pts(1,ind_orig);
+   yorig = grid_pts(2,ind_orig);
+   dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]');
+   dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]');
+   
+   
+   x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)];
+   y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)];
+
+   
+   figure(3);
+   image(I); colormap(map); hold on;
+   plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+');
+   plot(x_box_kk+1,y_box_kk+1,'-b');
+   plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo');
+   plot(xorig+1,yorig+1,'*m');
+   h = text(xorig-15,yorig-15,'O');
+   set(h,'Color','m','FontSize',14);
+   h2 = text(dxpos(1)-10,dxpos(2)-10,'dX');
+   set(h2,'Color','g','FontSize',14);
+   h3 = text(dypos(1)-25,dypos(2)-3,'dY');
+   set(h3,'Color','g','FontSize',14);
+   xlabel('Xc (in camera frame)');
+   ylabel('Yc (in camera frame)');
+   title('Extracted corners');
+   zoom on;
+   drawnow;
+   hold off;
+   
+   
+   Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)';
+   Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)';
+   Zi = zeros(1,Np);
+   
+   Xgrid = [Xi;Yi;Zi];
+   
+   
+   % All the point coordinates (on the image, and in 3D) - for global optimization:
+
+   x = grid_pts;
+   X = Xgrid;
+   
+   
+   % The left pannel info:
+   
+   xl = x;
+   Xl = X;
+   nl_sq_x = n_sq_x;
+   nl_sq_y = n_sq_y;
+   Hl = H_total;
+   
+   
+   
+   
+   
+   
+   %%%%%%%%%%%%%%%%%%%%%%%%% RIGHT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+   
+  
+   x1 = a10(1)/a10(3);
+   x4 = a11(1)/a11(3);
+   
+   y1 = a10(2)/a10(3);
+   y4 = a11(2)/a11(3);
+   
+
+  figure(2);
+  hold on;
+  plot([x1 x4],[y1 y4],'c-');
+  plot([x1 x4],[y1 y4],'co');
+  hold off;
+
+   title(['Click on the two remaining extreme corners of the right rectangular pattern... Image ' num2str(kk)]);
+   
+   disp('Click on the two remaining extreme corners of the right rectangular pattern...');
+   
+   [x,y] = ginput3(2);
+   
+   [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners
+   
+   x = Xc(1,:)';
+   y = Xc(2,:)';
+   
+   [y,indy] = sort(y);
+   x = x(indy);
+   
+   x2 = x(2);
+   x3 = x(1);
+   
+   y2 = y(2);
+   y3 = y(1);
+   
+   
+   x = [x1;x2;x3;x4];
+   y = [y1;y2;y3;y4];
+   
+   figure(2); hold on;
+   plot([x;x(1)],[y;y(1)],'c-');
+   plot(x,y,'oc');
+   hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X');
+   set(hx,'color','c','Fontsize',14);
+   hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y');
+   set(hy,'color','c','Fontsize',14);
+   hold off;
+   drawnow;
+   
+   
+   % Try to automatically count the number of squares in the grid
+   
+   n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx);
+   n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx);
+   n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx);
+   n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx);
+   
+  
+   
+   % If could not count the number of squares, enter manually
+   
+   if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2),
+      
+
+         disp('Could not count the number of squares in the grid. Enter manually.');
+         n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6
+         if isempty(n_sq_x), n_sq_x = 10; end;
+         n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6
+         if isempty(n_sq_y), n_sq_y = 10; end; 
+   
+   else
+               
+      n_sq_x = n_sq_x1;
+      n_sq_y = n_sq_y1;
+      
+   end;
+   
+   
+   if 1,
+        % Enter the size of each square
+   
+        dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']);
+                dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']);
+                if isempty(dX), dX = dX_default; else dX_default = dX; end;
+                if isempty(dY), dY = dY_default; else dY_default = dY; end;
+      
+   else
+      
+      dX = 3;
+      dY = 3;
+      
+   end;
+   
+   
+   % Compute the inside points through computation of the planar homography (collineation)
+   
+        a00 = [x(1);y(1);1];
+        a10 = [x(2);y(2);1];
+        a11 = [x(3);y(3);1];
+        a01 = [x(4);y(4);1];
+
+
+        % Compute the planart collineation: (return the normalization matrice as well)
+
+        [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01);
+
+
+        % Build the grid using the planar collineation:
+
+        x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x;
+   y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y;
+   pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]';
+   
+   XX = Homo*pts;
+        XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:));
+
+   
+   % Complete size of the rectangle
+   
+   W = n_sq_x*dX;
+   L = n_sq_y*dY;
+   
+   
+   
+   if 1,
+   %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   figure(2);
+   hold on;
+   plot(XX(1,:),XX(2,:),'r+');
+   title('The red crosses should be close to the image corners');
+   hold off;
+   
+   disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,');
+   disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)');
+   quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) ');
+  
+   quest_distort = ~isempty(quest_distort);
+   
+   if quest_distort,
+      % Estimation of focal length:
+      c_g = [size(I,2);size(I,1)]/2 + .5;
+                f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1);
+      f_g = mean(f_g);
+      script_fit_distortion;
+   end;
+   %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   end;   
+   
+   
+   Np = (n_sq_x+1)*(n_sq_y+1);
+
+   disp('Corner extraction...');
+   
+   grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points!
+   
+   %save all_corners x y grid_pts
+   
+   grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C)
+   
+   
+   % Global Homography from plane to pixel coordinates:
+   
+   H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1];
+   % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line)
+   % If it is not done, then this matrix should not appear (in C)
+   H_total = H_total / H_total(3,3);   
+   
+   
+   ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners
+   ind_orig = (n_sq_x+1)*n_sq_y + 1;
+   xorig = grid_pts(1,ind_orig);
+   yorig = grid_pts(2,ind_orig);
+   dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]');
+   dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]');
+   
+   
+   x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)];
+   y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)];
+
+   
+   figure(3);
+   hold on;
+   plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+');
+   plot(x_box_kk+1,y_box_kk+1,'-b');
+   plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo');
+   plot(xorig+1,yorig+1,'*m');
+   h = text(xorig-15,yorig-15,'O');
+   set(h,'Color','m','FontSize',14);
+   h2 = text(dxpos(1)-10,dxpos(2)-10,'dX');
+   set(h2,'Color','g','FontSize',14);
+   h3 = text(dypos(1)-25,dypos(2)-3,'dY');
+   set(h3,'Color','g','FontSize',14);
+   xlabel('Xc (in camera frame)');
+   ylabel('Yc (in camera frame)');
+   title('Extracted corners');
+   zoom on;
+   drawnow;
+   hold off;
+   
+   
+   Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)';
+   Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)';
+   Zi = zeros(1,Np);
+   
+   Xgrid = [Xi;Yi;Zi];
+   
+   
+   % All the point coordinates (on the image, and in 3D) - for global optimization:
+
+   x = grid_pts;
+   X = Xgrid;
+   
+   
+   % The right pannel info:
+   
+   xr = x;
+   Xr = X;
+   nr_sq_x = n_sq_x;
+   nr_sq_y = n_sq_y;
+   Hr = H_total;
+
+
+
+%%%%%%%% REGROUP THE LEFT AND RIHT PATTERNS %%%%%%%%%%%%%
+
+
+Xr2 = [0 0 1;0 1 0;-1 0 0]*Xr + [dX*nl_sq_x;0;0]*ones(1,length(Xr));
+
+
+x = [xl xr];
+
+X = [Xl Xr2];
+
+
+   
+   eval(['x_' num2str(kk) ' = x;']);
+   eval(['X_' num2str(kk) ' = X;']);
+   
+   eval(['nl_sq_x_' num2str(kk) ' = nl_sq_x;']);
+   eval(['nl_sq_y_' num2str(kk) ' = nl_sq_y;']);
+   
+   eval(['nr_sq_x_' num2str(kk) ' = nr_sq_x;']);
+   eval(['nr_sq_y_' num2str(kk) ' = nr_sq_y;']);
+   
+   % Save the global planar homography:
+   
+   eval(['Hl_' num2str(kk) ' = Hl;']);
+   eval(['Hr_' num2str(kk) ' = Hr;']);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m
new file mode 100755
index 0000000..a0f03de
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m
@@ -0,0 +1,38 @@
+function [x_comp]  = comp_distortion(x_dist,k2);
+
+%       [x_comp] = comp_distortion(x_dist,k2);
+%       
+%       compensates the radial distortion of the camera
+%       on the image plane.
+%       
+%       x_dist : the image points got without considering the
+%                radial distortion.
+%       x : The image plane points after correction for the distortion
+%       
+%       x and x_dist are 2xN arrays
+%
+%       NOTE : This compensation has to be done after the substraction
+%              of the center of projection, and division by the focal
+%              length.
+%       
+%       (do it up to a second order approximation)
+
+[two,N] = size(x_dist);
+
+if (two ~= 2 ), 
+ error('ERROR : The dimension of the points should be 2xN');
+end;
+
+if length(k2) > 2,
+   [x_comp]  = comp_distortion_oulu(x_dist,k2);
+else
+   
+radius_2= x_dist(1,:).^2 + x_dist(2,:).^2;
+radial_distortion = 1 + ones(2,1)*(k2 * radius_2);
+radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:);
+radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp);
+x_comp = x_dist ./ radial_distortion;
+
+end;
+
+%% Function completely checked : It works fine !!!
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m
new file mode 100755
index 0000000..532ee9a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m
@@ -0,0 +1,71 @@
+function [x_comp]  = comp_distortion(x_dist,k2);
+
+%       [x_comp] = comp_distortion(x_dist,k2);
+%       
+%       compensates the radial distortion of the camera
+%       on the image plane.
+%       
+%       x_dist : the image points got without considering the
+%                radial distortion.
+%       k2: Radial distortion factor
+%
+%       x : The image plane points after correction for the distortion
+%       
+%       x and x_dist are 2xN arrays
+%
+%       NOTE : This compensation has to be done after the substraction
+%              of the center of projection, and division by the focal
+%              length.
+%       
+%  Solve for cubic roots using method from Numerical Recipes in C 2nd Ed.
+%  pages 184-185.
+
+
+% California Institute of Technology
+% (c) Jean-Yves Bouguet - April 27th, 1998
+
+% fully checked! JYB, april 27th, 1998 - 2am
+
+if k2 ~= 0,
+
+[two,N] = size(x_dist);
+
+if (two ~= 2 ), 
+ error('ERROR : The dimension of the points should be 2xN');
+end;
+
+
+ph = atan2(x_dist(2,:),x_dist(1,:));
+
+Q = -1/(3*k2);
+R = -x_dist(1,:)./(2*k2*cos(ph));
+
+R2 = R.^2;
+Q3 = Q^3;
+
+
+if k2 < 0,
+   
+   % this works in all practical situations (it starts failing for very large
+   % values of k2)
+      
+   th = acos(R./sqrt(Q3));
+   r = -2*sqrt(Q)*cos((th-2*pi)/3);
+
+else
+   
+   % note: this always works, even for ridiculous values of k2
+   
+   A = (sqrt(R2-Q3)-R).^(1/3); 
+   B = Q*(1./A);
+   r = (A+B);
+  
+end;
+
+x_comp = [r.*cos(ph); r.*sin(ph)];
+
+else
+   
+   x_comp = x_dist;
+
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m
new file mode 100755
index 0000000..67d02d5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m
@@ -0,0 +1,47 @@
+function [x] = comp_distortion_oulu(xd,k);
+
+%comp_distortion_oulu.m
+%
+%[x] = comp_distortion_oulu(xd,k)
+%
+%Compensates for radial and tangential distortion. Model From Oulu university.
+%For more informatino about the distortion model, check the forward projection mapping function:
+%project_points.m
+%
+%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix)
+%       k: Distortion coefficients (radial and tangential) (4x1 vector)
+%
+%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix)
+%
+%Method: Iterative method for compensation.
+%
+%NOTE: This compensation has to be done after the subtraction
+%      of the principal point, and division by the focal length.
+
+
+if length(k) < 4,
+   
+   [x] = comp_distortion(xd,k);
+   
+else
+   
+   
+   k1 = k(1);
+   k2 = k(2);
+   p1 = k(3);
+   p2 = k(4);
+   
+   x = xd;                              % initial guess
+   
+   for kk=1:5;
+      
+      r_2 = sum(x.^2);
+      k_radial =  1 + k1 * r_2 + k2 * r_2.^2;
+      delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2) ;
+            p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];
+      x = (xd - delta_x)./(ones(2,1)*k_radial);
+      
+   end;
+   
+end;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m
new file mode 100755
index 0000000..c7bf662
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m
@@ -0,0 +1,46 @@
+%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%
+
+check_active_images;
+
+% Reproject the patterns on the images, and compute the pixel errors:
+
+ex = []; % Global error vector
+x = []; % Detected corners on the image plane
+y = []; % Reprojected points
+
+if ~exist('alpha_c'),
+   alpha_c = 0;
+end;
+
+for kk = 1:n_ima,
+   
+   eval(['omckk = omc_' num2str(kk) ';']);
+   eval(['Tckk = Tc_' num2str(kk) ';']);   
+   
+   if active_images(kk) & (~isnan(omckk(1,1))),
+      
+      %Rkk = rodrigues(omckk);
+      
+      eval(['y_' num2str(kk) '  = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']);
+      
+      eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' -y_' num2str(kk) ';']);
+      
+      eval(['x_kk = x_' num2str(kk) ';']);
+      
+      eval(['ex = [ex ex_' num2str(kk) '];']);
+      eval(['x = [x x_' num2str(kk) '];']);
+      eval(['y = [y y_' num2str(kk) '];']);
+      
+   else
+      
+      % eval(['y_' num2str(kk) '  = NaN*ones(2,1);']);
+
+   
+      % If inactivated image, the error does not make sense:
+      eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']);
+      
+   end;
+   
+end;
+
+err_std = std(ex')';
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m
new file mode 100755
index 0000000..809c309
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m
@@ -0,0 +1,66 @@
+function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01);
+
+% new formalism using homographies
+
+a00 = a00 / a00(3);
+a10 = a10 / a10(3);
+a11 = a11 / a11(3);
+a01 = a01 / a01(3);
+
+
+% Prenormalization of point coordinates (very important):
+% (Affine normalization)
+
+ax = [a00(1);a10(1);a11(1);a01(1)];
+ay = [a00(2);a10(2);a11(2);a01(2)];
+
+mxx = mean(ax);
+myy = mean(ay);
+ax = ax - mxx;
+ay = ay - myy;
+
+scxx = mean(abs(ax));
+scyy = mean(abs(ay));
+
+
+Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
+inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
+
+
+a00n = Hnorm*a00;
+a10n = Hnorm*a10;
+a11n = Hnorm*a11;
+a01n = Hnorm*a01;
+
+
+% Computation of the vanishing points:
+
+V1n = cross(cross(a00n,a10n),cross(a01n,a11n));
+V2n = cross(cross(a00n,a01n),cross(a10n,a11n));
+
+V1 = inv_Hnorm*V1n;
+V2 = inv_Hnorm*V2n;
+
+
+% Normalizaion of the vanishing points:
+
+V1n = V1n/norm(V1n);
+V2n = V2n/norm(V2n);
+
+
+% Closed-form solution of the coefficients:
+
+alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2));
+
+alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2));
+
+
+% Remaining Homography
+
+Hrem = [alpha_x*V1n  alpha_y*V2n a00n];
+
+
+% Final homography:
+
+H = inv_Hnorm*Hrem;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m
new file mode 100755
index 0000000..5217351
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m
@@ -0,0 +1,123 @@
+function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c)
+%
+%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
+%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
+%Works with planar and non-planar structures.
+%
+%INPUT: x_kk: Feature locations on the images
+%       X_kk: Corresponding grid coordinates
+%       fc: Camera focal length
+%       cc: Principal point coordinates
+%       kc: Distortion coefficients
+%       alpha_c: Skew coefficient
+%
+%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
+%        Tckk: 3D translation vector attached to the grid positions in space
+%        Rckk: 3D rotation matrices corresponding to the omc vectors
+%        H: Homography between points on the grid and points on the image plane (in pixel)
+%           This makes sense only if the planar that is used in planar.
+%        x: Reprojections of the points on the image plane
+%        ex: Reprojection error: ex = x_kk - x;
+%
+%Method: Computes the normalized point coordinates, then computes the 3D pose
+%
+%Important functions called within that program:
+%
+%normalize: Computes the normalize image point coordinates.
+%
+%pose3D: Computes the 3D pose of the structure given the normalized image projection.
+%
+%project_points.m: Computes the 2D image projections of a set of 3D points
+
+
+
+if nargin < 8,
+   thresh_cond = inf;
+end;
+
+
+if nargin < 7,
+   MaxIter = 20;
+end;
+
+
+if nargin < 6,
+   alpha_c = 0;
+        if nargin < 5,
+        kc = zeros(4,1);
+        if nargin < 4,
+        cc = zeros(2,1);
+        if nargin < 3,
+                fc = ones(2,1);
+                if nargin < 2,
+                error('Need 2D projections and 3D points (in compute_extrinsic.m)');
+                return;
+                end;
+        end;
+        end;
+        end;
+end;
+
+% Initialization:
+
+[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);
+
+% Refinement:
+
+[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);
+
+
+% computation of the homography (not useful in the end)
+
+H = [Rckk(:,1:2) Tckk];
+
+% Computes the reprojection error in pixels:
+
+x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
+
+ex = x_kk - x;
+
+
+% Converts the homography in pixel units:
+
+KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];
+
+H = KK*H;
+
+
+
+
+return;
+
+
+% Test of compte extrinsic:
+
+Np = 4;
+sx = 10;
+sy = 10;
+sz = 5;
+
+om = randn(3,1);
+T = [0;0;100];
+
+noise = 2/1000;
+
+XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
+xx = project_points(XX,om,T);
+
+xxn = xx + noise * randn(2,Np);
+
+[omckk,Tckk] = compute_extrinsic(xxn,XX);
+
+[om omckk om-omckk]
+[T Tckk T-Tckk]
+
+figure(3);
+plot(xx(1,:),xx(2,:),'r+');
+hold on;
+plot(xxn(1,:),xxn(2,:),'g+');
+hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m
new file mode 100755
index 0000000..2e6d821
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m
@@ -0,0 +1,151 @@
+function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c)
+%
+%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
+%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
+%Works with planar and non-planar structures.
+%
+%INPUT: x_kk: Feature locations on the images
+%       X_kk: Corresponding grid coordinates
+%       fc: Camera focal length
+%       cc: Principal point coordinates
+%       kc: Distortion coefficients
+%       alpha_c: Skew coefficient
+%
+%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
+%        Tckk: 3D translation vector attached to the grid positions in space
+%        Rckk: 3D rotation matrices corresponding to the omc vectors
+%
+%Method: Computes the normalized point coordinates, then computes the 3D pose
+%
+%Important functions called within that program:
+%
+%normalize: Computes the normalize image point coordinates.
+%
+%pose3D: Computes the 3D pose of the structure given the normalized image projection.
+%
+%project_points.m: Computes the 2D image projections of a set of 3D points
+
+
+
+if nargin < 6,
+   alpha_c = 0;
+        if nargin < 5,
+        kc = zeros(4,1);
+        if nargin < 4,
+        cc = zeros(2,1);
+        if nargin < 3,
+                fc = ones(2,1);
+                if nargin < 2,
+                error('Need 2D projections and 3D points (in compute_extrinsic.m)');
+                return;
+                end;
+        end;
+        end;
+        end;
+end;
+
+
+% Compute the normalized coordinates:
+
+xn = normalize(x_kk,fc,cc,kc,alpha_c);
+
+
+
+Np = size(xn,2);
+
+%% Check for planarity of the structure:
+
+X_mean = mean(X_kk')';
+
+Y = X_kk - (X_mean*ones(1,Np));
+
+YY = Y*Y';
+
+[U,S,V] = svd(YY);
+
+r = S(3,3)/S(2,2);
+
+if (r < 1e-3)|(Np < 6), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity
+   
+   %fprintf(1,'Planar structure detected: r=%f\n',r);
+
+   % Transform the plane to bring it in the Z=0 plane:
+   
+   R_transform = V';
+   
+   if det(R_transform) < 0, R_transform = -R_transform; end;
+   
+        T_transform = -(R_transform)*X_mean;
+
+        X_new = R_transform*X_kk + T_transform*ones(1,Np);
+   
+   
+   % Compute the planar homography:
+   
+   H = compute_homography (xn,X_new(1:2,:));
+   
+   % De-embed the motion parameters from the homography:
+   
+   sc = mean([norm(H(:,1));norm(H(:,2))]);
+   
+   H = H/sc;
+   
+   omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
+   Rckk = rodrigues(omckk);
+   Tckk = H(:,3);
+   
+   %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform
+   
+   Tckk = Tckk + Rckk* T_transform;
+   Rckk = Rckk * R_transform;
+
+   omckk = rodrigues(Rckk);
+   Rckk = rodrigues(omckk);
+   
+   
+else
+   
+   %fprintf(1,'Non planar structure detected: r=%f\n',r);
+
+   % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!):
+   % The DLT method is applied here!!
+   
+   J = zeros(2*Np,12);
+        
+        xX = (ones(3,1)*xn(1,:)).*X_kk;
+        yX = (ones(3,1)*xn(2,:)).*X_kk;
+        
+        J(1:2:end,[1 4 7]) = -X_kk';
+        J(2:2:end,[2 5 8]) = X_kk';
+        J(1:2:end,[3 6 9]) = xX';
+        J(2:2:end,[3 6 9]) = -yX';
+        J(1:2:end,12) = xn(1,:)';
+        J(2:2:end,12) = -xn(2,:)';
+        J(1:2:end,10) = -ones(Np,1);
+        J(2:2:end,11) = ones(Np,1);
+        
+        JJ = J'*J;
+        [U,S,V] = svd(JJ);
+   
+   RR = reshape(V(1:9,12),3,3);
+   
+   if det(RR) < 0,
+      V(:,12) = -V(:,12);
+      RR = -RR;
+   end;
+   
+   [Ur,Sr,Vr] = svd(RR);
+   
+   Rckk = Ur*Vr';
+   
+   sc = norm(V(1:9,12)) / norm(Rckk(:));
+   Tckk = V(10:12,12)/sc;
+   
+        omckk = rodrigues(Rckk);
+   Rckk = rodrigues(omckk);
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m
new file mode 100755
index 0000000..a4d066c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m
@@ -0,0 +1,113 @@
+function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk] = compute_extrinsic_refine(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter)
+%
+%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
+%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
+%Works with planar and non-planar structures.
+%
+%INPUT: x_kk: Feature locations on the images
+%       X_kk: Corresponding grid coordinates
+%       fc: Camera focal length
+%       cc: Principal point coordinates
+%       kc: Distortion coefficients
+%       alpha_c: Skew coefficient
+%       MaxIter: Maximum number of iterations
+%
+%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
+%        Tckk: 3D translation vector attached to the grid positions in space
+%        Rckk: 3D rotation matrices corresponding to the omc vectors
+
+%
+%Method: Computes the normalized point coordinates, then computes the 3D pose
+%
+%Important functions called within that program:
+%
+%normalize: Computes the normalize image point coordinates.
+%
+%pose3D: Computes the 3D pose of the structure given the normalized image projection.
+%
+%project_points.m: Computes the 2D image projections of a set of 3D points
+
+
+if nargin < 10,
+   thresh_cond = inf;
+end;
+
+
+if nargin < 9,
+   MaxIter = 20;
+end;
+
+if nargin < 8,
+   alpha_c = 0;
+        if nargin < 7,
+        kc = zeros(4,1);
+        if nargin < 6,
+        cc = zeros(2,1);
+        if nargin < 5,
+                fc = ones(2,1);
+                if nargin < 4,
+                error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)');
+                return;
+                end;
+        end;
+        end;
+        end;
+end;
+
+
+% Initialization:
+
+omckk = omc_init;
+Tckk = Tc_init;
+
+
+% Final optimization (minimize the reprojection error in pixel):
+% through Gradient Descent:
+
+param = [omckk;Tckk];
+
+change = 1;
+
+iter = 0;
+
+%keyboard;
+
+%fprintf(1,'Gradient descent iterations: ');
+
+while (change > 1e-10)&(iter < MaxIter),
+   
+   %fprintf(1,'%d...',iter+1);
+
+   [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
+      
+   ex = x_kk - x;
+   
+   %keyboard;
+   
+   JJ = [dxdom dxdT];
+   
+   if cond(JJ) > thresh_cond,
+      change = 0;
+   else
+      
+        JJ2 = JJ'*JJ;
+   
+                param_innov = inv(JJ2)*(JJ')*ex(:);
+                param_up = param + param_innov;
+                change = norm(param_innov)/norm(param_up);
+                param = param_up;
+                iter = iter + 1;
+   
+        omckk = param(1:3);
+        Tckk = param(4:6);
+   end;
+   
+end;
+
+%fprintf(1,'\n');
+
+Rckk = rodrigues(omckk);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m
new file mode 100755
index 0000000..fcc9003
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m
@@ -0,0 +1,163 @@
+function [H,Hnorm,inv_Hnorm] = compute_homography (m,M);
+
+%compute_homography
+%
+%[H,Hnorm,inv_Hnorm] = compute_homography (m,M)
+%
+%Computes the planar homography between the point coordinates on the plane (M) and the image
+%point coordinates (m).
+%
+%INPUT: m: homogeneous coordinates in the image plane (3xN matrix)
+%       M: homogeneous coordinates in the plane in 3D (3xN matrix)
+%
+%OUTPUT: H: Homography matrix (3x3 homogeneous matrix)
+%        Hnorm: Normlization matrix used on the points before homography computation
+%               (useful for numerical stability is points in pixel coordinates)
+%        inv_Hnorm: The inverse of Hnorm
+%
+%Definition: m ~ H*M where "~" means equal up to a non zero scalar factor.
+%
+%Method: First computes an initial guess for the homography through quasi-linear method.
+%        Then, if the total number of points is larger than 4, optimize the solution by minimizing
+%        the reprojection error (in the least squares sense).
+%
+%
+%Important functions called within that program:
+%
+%comp_distortion_oulu: Undistorts pixel coordinates.
+%
+%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
+%
+%project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian
+%                  matrix (derivative with respect to the intrinsic and extrinsic parameters).
+%                  This function is called within the minimization loop.
+
+
+
+
+Np = size(m,2);
+
+if size(m,1)<3,
+   m = [m;ones(1,Np)];
+end;
+
+if size(M,1)<3,
+   M = [M;ones(1,Np)];
+end;
+
+
+m = m ./ (ones(3,1)*m(3,:));
+M = M ./ (ones(3,1)*M(3,:));
+
+% Prenormalization of point coordinates (very important):
+% (Affine normalization)
+
+ax = m(1,:);
+ay = m(2,:);
+
+mxx = mean(ax);
+myy = mean(ay);
+ax = ax - mxx;
+ay = ay - myy;
+
+scxx = mean(abs(ax));
+scyy = mean(abs(ay));
+
+
+Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
+inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
+
+mn = Hnorm*m;
+
+% Compute the homography between m and mn:
+
+% Build the matrix:
+
+L = zeros(2*Np,9);
+
+L(1:2:2*Np,1:3) = M';
+L(2:2:2*Np,4:6) = M';
+L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
+L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';
+
+if Np > 4,
+        L = L'*L;
+end;
+
+[U,S,V] = svd(L);
+
+hh = V(:,9);
+hh = hh/hh(9);
+
+Hrem = reshape(hh,3,3)';
+%Hrem = Hrem / Hrem(3,3);
+
+% Final homography:
+
+H = inv_Hnorm*Hrem;
+
+
+%%% Homography refinement if there are more than 4 points:
+
+if Np > 4,
+   
+   % Final refinement:
+   
+   hhv = reshape(H',9,1);
+   hhv = hhv(1:8);
+   
+   for iter=1:10,
+   
+                mrep = H * M;
+
+                J = zeros(2*Np,8);
+
+                MMM = (M ./ (ones(3,1)*mrep(3,:)));
+
+                J(1:2:2*Np,1:3) = -MMM';
+                J(2:2:2*Np,4:6) = -MMM';
+                
+                mrep = mrep ./ (ones(3,1)*mrep(3,:));
+
+                m_err = m(1:2,:) - mrep(1:2,:);
+                m_err = m_err(:);
+
+                MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
+                MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;
+
+                J(1:2:2*Np,7:8) = MMM2(1:2,:)';
+                J(2:2:2*Np,7:8) = MMM3(1:2,:)';
+
+                MMM = (M ./ (ones(3,1)*mrep(3,:)))';
+
+                hh_innov  = inv(J'*J)*J'*m_err;
+
+                hhv_up = hhv - hh_innov;
+
+                H_up = reshape([hhv_up;1],3,3)';
+
+                %norm(m_err)
+                %norm(hh_innov)
+
+                hhv = hhv_up;
+      H = H_up;
+      
+   end;
+   
+end;
+
+
+
+
+
+return;
+
+%test of Jacobian
+
+mrep = H*M;
+mrep = mrep ./ (ones(3,1)*mrep(3,:));
+
+m_err = mrep(1:2,:) - m(1:2,:);
+figure(8);
+plot(m_err(1,:),m_err(2,:),'r+');
+std(m_err')
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m
new file mode 100755
index 0000000..9bfa51f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m
@@ -0,0 +1,215 @@
+function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2);
+
+%[xc] = cornerfinder(xt,I);
+%
+%Finds the sub-pixel corners on the image I with initial guess xt
+%xt and xc are 2xN matrices. The first component is the x coordinate
+%(horizontal) and the second component is the y coordinate (vertical)
+% 
+%Based on Harris corner finder method
+%
+%Finds corners to a precision below .1 pixel!
+%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
+%Sept 1998 - UPDATED to handle diverged points: we keep the original points
+%good is a binary vector indicating wether a feature point has been properly
+%found.
+%
+%Add a zero zone of size wx2,wy2
+%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
+%resolution and larger number of iterations.
+
+
+% California Institute of Technology
+% (c) Jean-Yves Bouguet -- Oct. 14th, 1997
+
+
+
+line_feat = 1; % set to 1 to allow for extraction of line features.
+
+xt = xt';
+xt = fliplr(xt);
+
+
+if nargin < 4,
+   winty = 5;
+   if nargin < 3,
+      wintx = 5;
+   end;
+end;
+
+
+if nargin < 6,
+   wx2 = -1;
+   wy2 = -1;
+end;
+
+
+%mask = ones(2*wintx+1,2*winty+1);
+mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);
+
+
+if (wx2>0) & (wy2>0),
+   if ((wintx - wx2)>=2)&((winty - wy2)>=2),
+      mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
+   end;
+end;
+
+offx = [-wintx:wintx]'*ones(1,2*winty+1);
+offy = ones(2*wintx+1,1)*[-winty:winty];
+
+resolution = 0.005;
+
+MaxIter = 10;
+
+[nx,ny] = size(I);
+N = size(xt,1);
+
+xc = xt; % first guess... they don't move !!!
+
+type = zeros(1,N);
+
+
+for i=1:N,
+   
+   v_extra = resolution + 1;            % just larger than resolution
+   
+   compt = 0;                           % no iteration yet
+   
+   while (norm(v_extra) > resolution) & (compt<MaxIter),
+      
+      cIx = xc(i,1);                    %
+      cIy = xc(i,2);                    % Coords. of the point
+      crIx = round(cIx);                % on the initial image
+      crIy = round(cIy);                %      
+      itIx = cIx - crIx;                % Coefficients
+      itIy = cIy - crIy;                % to compute
+      if itIx > 0,                      % the sub pixel
+         vIx = [itIx 1-itIx 0]';        % accuracy.
+      else
+         vIx = [0 1+itIx -itIx]';
+      end;
+      if itIy > 0,
+         vIy = [itIy 1-itIy 0];
+      else
+         vIy = [0 1+itIy -itIy];
+      end;
+
+      
+      % What if the sub image is not in?
+      
+      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
+      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
+      else
+         xmin = crIx-wintx-2; xmax = crIx+wintx+2;
+      end;
+
+      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
+      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
+      else
+         ymin = crIy-winty-2; ymax = crIy+winty+2;
+      end;
+      
+      
+      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
+      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
+      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
+      [gy,gx] = gradient(SI);           % The gradient image
+      gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
+      gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients
+      
+      px = cIx + offx;
+      py = cIy + offy;
+      
+      gxx = gx .* gx .* mask;
+      gyy = gy .* gy .* mask;
+      gxy = gx .* gy .* mask;
+   
+      
+      bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];
+      
+      a = sum(sum(gxx));
+      b = sum(sum(gxy));
+      c = sum(sum(gyy));
+      
+      dt = a*c - b^2;
+      
+      xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
+      
+      
+      %keyboard;
+      
+      if line_feat,
+      
+         G = [a b;b c];
+         [U,S,V]  = svd(G);
+         
+         %keyboard;
+         
+         % If non-invertible, then project the point onto the edge orthogonal:
+         
+         if (S(1,1)/S(2,2) > 50),
+            % projection operation:
+            xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)';
+            type(i) = 1;
+         end;
+      
+      end;
+      
+      
+      %keyboard;
+      
+%      G = [a b;b c];
+%      [U,S,V]  = svd(G);
+
+
+%      if S(1,1)/S(2,2) > 150,
+%        bb2 = U'*bb;
+%        xc2 = (V*[bb2(1)/S(1,1) ;0])';
+%      else
+%        xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
+%      end;
+      
+      
+      %if (abs(a)> 50*abs(c)),
+%        xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)];
+%      elseif (abs(c)> 50*abs(a))
+%        xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt];
+%      else
+%        xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
+%      end;
+      
+      %keyboard;
+      
+      v_extra = xc(i,:) - xc2;
+      
+      xc(i,:) = xc2;
+      
+%      keyboard;
+
+      compt = compt + 1;
+      
+   end
+end;
+
+
+% check for points that diverge:
+
+delta_x = xc(:,1) - xt(:,1);
+delta_y = xc(:,2) - xt(:,2);
+
+%keyboard;
+
+
+bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
+good = ~bad;
+in_bad = find(bad);
+
+% For the diverged points, keep the original guesses:
+
+xc(in_bad,:) = xt(in_bad,:);
+
+xc = fliplr(xc);
+xc = xc';
+
+bad = bad';
+good = good';
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m
new file mode 100755
index 0000000..0e226c0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m
@@ -0,0 +1,74 @@
+function ns = count_squares(I,x1,y1,x2,y2,win);
+
+%keyboard;
+
+[ny,nx] = size(I);
+
+lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1];
+
+lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda;
+
+l1 = lambda + [0;0;win];
+l2 = lambda - [0;0;win];
+
+
+dx = x2-x1;
+dy = y2 - y1;
+
+
+if abs(dx) > abs(dy),
+   
+   if x2 > x1,
+      xs = x1:x2;
+   else
+      xs = x1:-1:x2;
+   end;
+   
+   ys = -(lambda(3) + lambda(1)*xs)/lambda(2);
+   
+else
+   
+   if y2 > y1,
+      ys = y1:y2;
+   else
+      ys = y1:-1:y2;
+   end;
+   xs = -(lambda(3) + lambda(2)*ys)/lambda(1);
+   
+end;
+
+
+   
+        Np = length(xs);
+
+        xs_mat = ones(2*win + 1,1)*xs;
+        ys_mat = ones(2*win + 1,1)*ys;
+
+        win_mat = (-win:win)'*ones(1,Np);
+
+
+        xs_mat2 = round(xs_mat - win_mat * lambda(1));
+        ys_mat2 = round(ys_mat - win_mat * lambda(2));
+
+        ind_mat = (xs_mat2 - 1) * ny + ys_mat2;
+
+        ima_patch = zeros(2*win + 1,Np);
+
+        ima_patch(:) = I(ind_mat(:));
+
+        %ima2 = ima_patch(:,win+1:end-win);
+
+        filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)];
+
+        out_f = sum(filtk.*ima_patch);
+
+        out_f_f = conv2(out_f,[1/4 1/2 1/4],'same');
+
+        out_f_f = out_f_f(win+1:end-win);
+
+        ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1;
+
+   
+
+
+return;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m
new file mode 100755
index 0000000..422769b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m
@@ -0,0 +1,92 @@
+%%% This script alets the user enter the name of the images (base name, numbering scheme,...
+
+      
+% Checks that there are some images in the directory:
+
+l_ras = dir('*ras');
+s_ras = size(l_ras,1);
+l_bmp = dir('*bmp');
+s_bmp = size(l_bmp,1);
+l_tif = dir('*tif');
+s_tif = size(l_tif,1);
+l_pgm = dir('*pgm');
+s_pgm = size(l_pgm,1);
+l_jpg = dir('*jpg');
+s_jpg = size(l_jpg,1);
+
+s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg;
+
+if s_tot < 1,
+   fprintf(1,'No image in this directory in either ras, bmp, tif, pgm or jpg format. Change directory and try again.\n');
+   break;
+end;
+
+
+% IF yes, display the directory content:
+
+dir;
+
+Nima_valid = 0;
+
+while (Nima_valid==0),
+
+   fprintf(1,'\n');
+   calib_name = input('Basename camera calibration images (without number nor suffix): ','s');
+   
+   format_image = '0';
+   
+        while format_image == '0',
+   
+        format_image =  input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s');
+                
+                if isempty(format_image),
+                format_image = 'ras';
+                end;
+      
+      if lower(format_image(1)) == 'm',
+         format_image = 'ppm';
+      else
+         if lower(format_image(1)) == 'b',
+            format_image = 'bmp';
+         else
+            if lower(format_image(1)) == 't',
+               format_image = 'tif';
+            else
+               if lower(format_image(1)) == 'p',
+                  format_image = 'pgm';
+               else
+                  if lower(format_image(1)) == 'j',
+                     format_image = 'jpg';
+                  else
+                     if lower(format_image(1)) == 'r',
+                        format_image = 'ras';
+                     else  
+                        disp('Invalid image format');
+                        format_image = '0'; % Ask for format once again
+                     end;
+                  end;
+               end;
+            end;
+         end;
+      end;
+   end;
+
+      
+   check_directory;
+   
+end;
+
+
+
+%string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num';
+
+%eval(string_save);
+
+
+
+if (Nima_valid~=0),
+   % Reading images:
+   
+   ima_read_calib; % may be launched from the toolbox itself
+end;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m
new file mode 100755
index 0000000..85feac5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m
@@ -0,0 +1,182 @@
+%%% ERROR_ANALYSIS
+%%% This simulation helps coputing the acturacies of calibration
+%%% Run it after the main calibration
+
+
+
+N_runs = 200;
+
+%N_ima_active = 4;
+
+saving = 1;
+
+if 1, %~exist('fc_list'), % initialization
+   
+   % Initialization:
+   
+   load Calib_Results;
+   check_active_images;
+   
+        fc_list = [];
+        cc_list = [];
+        kc_list = [];
+   active_images_list = [];
+   
+   
+        for kk=1:n_ima,
+        
+        eval(['omc_list_' num2str(kk) ' = [];']);
+        eval(['Tc_list_' num2str(kk) ' = [];']);
+      
+   end;
+        
+        %sx = median(abs(ex(1,:)))*1.4836;
+        %sy = median(abs(ex(2,:)))*1.4836;
+        
+        sx = std(ex(1,:));
+        sy = std(ex(2,:));
+   
+        % Saving the feature locations:
+
+        for kk = 1:n_ima,
+        
+        eval(['x_save_' num2str(kk) ' = x_' num2str(kk) ';']);
+        eval(['y_save_' num2str(kk) ' = y_' num2str(kk) ';']);
+
+        end;
+   
+   active_images_save = active_images;
+   ind_active_save = ind_active;
+   
+   fc_save = fc;
+   cc_save = cc;
+   kc_save = kc;
+   KK_save = KK;
+   
+
+end;
+
+
+
+
+%%% The main loop:
+
+
+for ntrial = 1:N_runs,
+   
+   fprintf(1,'\nRun number: %d\n',ntrial);
+   fprintf(1,  '----------\n');
+   
+   for kk = 1:n_ima,
+      
+      eval(['y_kk = y_save_' num2str(kk) ';'])
+      
+      if active_images(kk) & ~isnan(y_kk(1,1)),
+         
+         Nkk = size(y_kk,2);
+         
+         x_kk_new = y_kk + [sx * randn(1,Nkk);sy*randn(1,Nkk)];
+         
+         eval(['x_' num2str(kk) ' = x_kk_new;']);
+         
+      end;
+      
+   end;
+   
+   N_active = length(ind_active_save);
+   junk = randn(1,N_active);
+   [junk,junk2] = sort(junk);
+   
+   active_images = zeros(1,n_ima);
+   active_images(ind_active_save(junk2(1:N_ima_active))) = ones(1,N_ima_active);
+   
+   fc = fc_save;
+   cc = cc_save;
+   kc = kc_save;
+   KK = KK_save;
+   
+   go_calib_optim;
+   
+   fc_list = [fc_list fc];
+   cc_list = [cc_list cc];
+   kc_list = [kc_list kc];
+   active_images_list = [active_images_list active_images'];
+   
+   for kk=1:n_ima,
+   
+        eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']);
+        eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']);
+   
+        end;
+
+end;
+
+
+
+
+if 0,
+
+% Restoring the feature locations:
+
+for kk = 1:n_ima,
+   
+   eval(['x_' num2str(kk) ' = x_save_' num2str(kk) ';']);
+   
+end;
+
+fprintf(1,'\nFinal run (with the real data)\n');
+fprintf(1,  '------------------------------\n');
+
+active_images = active_images_save;
+ind_active = ind_active_save;
+
+go_calib_optim;
+   
+fc_list = [fc_list fc];
+cc_list = [cc_list cc];
+kc_list = [kc_list kc];
+active_images_list = [active_images_list active_images'];
+
+for kk=1:n_ima,
+   
+   eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']);
+   eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']);
+   
+end;
+
+end;
+
+
+
+
+
+if saving,
+   
+disp(['Save Calibration accuracy results under Calib_Accuracies_' num2str(N_ima_active) '.mat']);
+
+string_save = ['save Calib_Accuracies_' num2str(N_ima_active) ' active_images n_ima N_ima_active N_runs active_images_list fc cc kc fc_list cc_list kc_list'];
+
+for kk = 1:n_ima,
+   string_save = [string_save ' Tc_list_' num2str(kk) ' omc_list_' num2str(kk)  ' Tc_' num2str(kk) ' omc_' num2str(kk) ];
+end;
+
+eval(string_save);
+
+end;
+
+
+return;
+
+std(fc_list')
+
+std(cc_list')
+
+std(kc_list')
+
+for kk = 1:n_ima,
+   
+   eval(['std(Tc_list_'  num2str(kk) ''')'])
+   
+end;
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m
new file mode 100755
index 0000000..39506a8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m
@@ -0,0 +1,99 @@
+%% Export calibration data (corners + 3D coordinates) to
+%% text files (in Willson-Heikkila's format or Zhang's format)
+
+%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion
+%% of adding thsi export function to the toolbox.
+
+
+if ~exist('n_ima'),
+   fprintf(1,'ERROR: No calibration data to export\n');
+   
+else
+
+        check_active_images;
+
+        check_extracted_images;
+
+        check_active_images;
+   
+   fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n');
+   
+   qformat = -1;
+   
+   while (qformat ~=0)&(qformat ~=1),
+      
+      fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n')
+      qformat = input('Format of export (enter 0 or 1): ');
+      
+      if isempty(qformat)
+         qformat = -1;
+      end;
+      
+      if (qformat ~=0)&(qformat ~=1),
+         
+         fprintf(1,'Invalid entry. Try again.\n')
+         
+      end;
+      
+   end;
+   
+   if qformat == 0,
+      
+                fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n');
+                outputfile = input('File basename: ','s');
+        
+                for kk = ind_active,
+        
+                eval(['X_kk = X_' num2str(kk) ';']);
+        eval(['x_kk = x_' num2str(kk) ';']);
+         
+         Xx = [X_kk ; x_kk]';
+         
+                        file_name = [outputfile num2str(kk)];
+        
+                        disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']);
+         
+         eval(['save ' file_name ' Xx -ASCII']);
+      
+        end;
+      
+   else
+      
+      fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n');
+      modelfile = input('File basename for the 3D world coordinates: ','s');
+      datafile = input('File basename for the 2D image coordinates: ','s');
+      
+      for kk = ind_active,
+         
+                eval(['X_kk = X_' num2str(kk) ';']);
+         eval(['x_kk = x_' num2str(kk) ';']);
+         
+         if ~exist(['n_sq_x_' num2str(kk)]),
+            n_sq_x = 1;
+            n_sq_y = size(X_kk,2);
+         else
+            eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
+                eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
+         end;
+         
+              X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)';
+              Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)';
+         XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1));
+          
+         x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)';
+              y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)';
+         xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1));
+         
+         disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']);
+
+         eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']);
+         eval(['save ' datafile num2str(kk) '.txt xy -ASCII']);
+               
+        end;
+
+      
+end;
+
+fprintf(1,'done\n');
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m
new file mode 100755
index 0000000..04d6319
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m
@@ -0,0 +1,152 @@
+
+%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%%
+
+if ~exist('n_ima')|~exist('fc'),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+check_active_images;
+
+if ~exist(['omc_' num2str(ind_active(1))]),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+%if ~exist('no_grid'),
+   no_grid = 0;
+%end;
+
+if ~exist(['n_sq_x_' num2str(ind_active(1))]),
+   no_grid = 1;
+end;
+
+
+if 0,
+
+err_std = std(ex');
+
+fprintf(1,'\n\nCalibration results without principal point estimation:\n\n');
+fprintf(1,'Focal Length:     fc = [ %3.5f   %3.5f]\n',fc);
+fprintf(1,'Principal point:  cc = [ %3.5f   %3.5f]\n',cc);
+fprintf(1,'Distortion:       kc = [ %3.5f   %3.5f   %3.5f   %3.5f]\n',kc);   
+fprintf(1,'Pixel error:      err = [ %3.5f   %3.5f]\n\n',err_std); 
+
+end;
+
+
+% Color code for each image:
+
+colors = 'brgkcm';
+
+
+%%% Show the extrinsic parameters
+
+if ~exist('dX'),
+   eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']);
+   dY = dX;
+end;
+
+IP = 5*dX*([0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1] - [cc;0]*ones(1,5)) ./ ([fc;1]*ones(1,5));
+BASE = 5*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]);
+IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15);
+
+figure(4);
+[a,b] = view;
+
+figure(4);
+plot3(BASE(1,:),BASE(3,:),-BASE(2,:),'b-','linewidth',2');
+hold on;
+plot3(IP(1,:),IP(3,:),-IP(2,:),'r-','linewidth',2);
+text(6*dX,0,0,'X_c');
+text(-dX,5*dX,0,'Z_c');
+text(0,0,-6*dX,'Y_c');
+text(-dX,-dX,dX,'O_c');
+
+
+for kk = 1:n_ima,
+   
+   if active_images(kk);
+      
+      if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]),
+         
+         eval(['XX_kk = X_' num2str(kk) ';']);
+
+         if ~isnan(XX_kk(1,1))
+            
+            eval(['omc_kk = omc_' num2str(kk) ';']);
+            eval(['Tc_kk = Tc_' num2str(kk) ';']);
+            N_kk = size(XX_kk,2);
+            
+            if ~exist(['n_sq_x_' num2str(kk)]),
+               no_grid = 1;
+            else
+               eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
+               if isnan(n_sq_x(1)),
+                  no_grid = 1;
+               end;  
+            end;
+               
+            
+            if ~no_grid,
+               eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
+               eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
+               if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))),
+                  no_grid = 1;
+               end;
+            end;
+            
+            if ~isnan(omc_kk(1,1)),
+               
+               R_kk = rodrigues(omc_kk);
+               
+               YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk));
+               
+               uu = [-dX;-dY;0]/2;
+               uu = R_kk * uu + Tc_kk; 
+               
+               if ~no_grid,
+                  YYx = zeros(n_sq_x+1,n_sq_y+1);
+                  YYy = zeros(n_sq_x+1,n_sq_y+1);
+                  YYz = zeros(n_sq_x+1,n_sq_y+1);
+                  
+                  YYx(:) = YY_kk(1,:);
+                  YYy(:) = YY_kk(2,:);
+                  YYz(:) = YY_kk(3,:);
+                  
+                  %keyboard;
+                  
+                  figure(4);
+                  hhh= mesh(YYx,YYz,-YYy);
+                  set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none');
+                  %plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['o' colors(rem(kk-1,6)+1)]);
+                  text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1));
+               else
+                  
+                  figure(4);
+                  plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]);
+                  text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1));
+                  
+               end;
+            
+            end;
+         
+         end;
+         
+      end;
+      
+   end;
+   
+end;
+
+figure(4);rotate3d on;
+axis('equal');
+title('Extrinsic parameters');
+%view(60,30);
+view(a,b);
+hold off;
+
+set(4,'Name','3D','NumberTitle','off');
+
+%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m
new file mode 100755
index 0000000..0cf9abe
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m
@@ -0,0 +1,234 @@
+function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY);
+
+map = gray(256);
+
+minI = min(I(:));
+maxI = max(I(:));
+
+Id = 255*(I - minI)/(maxI - minI);
+
+        figure(2);
+        image(Id);
+   colormap(map);
+   
+   
+   if nargin < 2,
+      
+                disp('Window size for corner finder (wintx and winty):');
+                wintx = input('wintx ([] = 5) = ');
+                if isempty(wintx), wintx = 5; end;
+                wintx = round(wintx);
+                winty = input('winty ([] = 5) = ');
+                if isempty(winty), winty = 5; end;
+                winty = round(winty);
+
+                fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+      
+   end;
+   
+
+   
+   title('Click on the four extreme corners of the rectangular pattern...');
+   
+   disp('Click on the four extreme corners of the rectangular complete pattern...');
+   
+   [x,y] = ginput3(4);
+   
+   [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners
+   
+   x = Xc(1,:)';
+   y = Xc(2,:)';
+   
+   [y,indy] = sort(y);
+   x = x(indy);
+   
+   if (x(2) > x(1)),
+      x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2);
+   else
+      x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1);
+   end;
+   if (x(3) > x(4)),
+      x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4);
+   else
+      x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3);
+   end;
+   
+   x = [x1;x2;x3;x4];
+   y = [y1;y2;y3;y4];
+   
+   
+   figure(2); hold on;
+   plot([x;x(1)],[y;y(1)],'g-');
+   plot(x,y,'og');
+   hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X');
+   set(hx,'color','g','Fontsize',14);
+   hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y');
+   set(hy,'color','g','Fontsize',14);
+   hold off;
+   
+   
+   % Try to automatically count the number of squares in the grid
+   
+   n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx);
+   n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx);
+   n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx);
+   n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx);
+   
+  
+   
+   % If could not count the number of squares, enter manually
+   
+   if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2),
+      
+
+         disp('Could not count the number of squares in the grid. Enter manually.');
+         n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6
+         if isempty(n_sq_x), n_sq_x = 10; end;
+         n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6
+         if isempty(n_sq_y), n_sq_y = 10; end; 
+   
+   else
+               
+      n_sq_x = n_sq_x1;
+      n_sq_y = n_sq_y1;
+      
+   end;
+   
+   if ~exist('dX')|~exist('dY'),
+   
+        % Enter the size of each square
+   
+        dX = input(['Size dX of each square along the X direction ([]=30mm) = ']);
+                dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']);
+                if isempty(dX), dX = 30; end;
+                if isempty(dY), dY = 30; end;
+      
+   end;
+   
+   
+   % Compute the inside points through computation of the planar homography (collineation)
+   
+        a00 = [x(1);y(1);1];
+        a10 = [x(2);y(2);1];
+        a11 = [x(3);y(3);1];
+        a01 = [x(4);y(4);1];
+
+
+        % Compute the planart collineation: (return the normalization matrice as well)
+   
+   [Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]);
+   
+   
+        % Build the grid using the planar collineation:
+
+        x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x;
+   y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y;
+   pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]';
+   
+   XX = Homo*pts;
+        XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:));
+
+   
+   % Complete size of the rectangle
+   
+   W = n_sq_x*dX;
+   L = n_sq_y*dY;
+   
+   
+   
+   if nargin < 6,
+   
+   %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   figure(2);
+   hold on;
+   plot(XX(1,:),XX(2,:),'r+');
+   title('The red crosses should be close to the image corners');
+   hold off;
+   
+   disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,');
+   disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)');
+   quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) ');
+  
+   quest_distort = ~isempty(quest_distort);
+   
+   if quest_distort,
+      % Estimation of focal length:
+      c_g = [size(I,2);size(I,1)]/2 + .5;
+                f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1);
+      f_g = mean(f_g);
+      script_fit_distortion;
+   end;
+   %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%%
+   
+        else
+        
+        xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(1)],kc);
+         
+                xu = xy_corners_undist(1,:)';
+                yu = xy_corners_undist(2,:)';
+         
+                [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid
+       
+      r2 = sum(XXu.^2);       
+                XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu;
+                XX(1,:) = fc(1)*XX(1,:)+cc(1);
+                XX(2,:) = fc(2)*XX(2,:)+cc(2);
+       
+   end;
+    
+   
+   Np = (n_sq_x+1)*(n_sq_y+1);
+
+   disp('Corner extraction...');
+   
+   grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points!
+   
+   grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C)
+   
+   ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners
+   ind_orig = (n_sq_x+1)*n_sq_y + 1;
+   xorig = grid_pts(1,ind_orig);
+   yorig = grid_pts(2,ind_orig);
+   dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]');
+   dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]');
+   
+   
+   ind_x = (n_sq_x+1)*(n_sq_y + 1);
+        ind_y = 1;
+
+   x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)];
+   y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)];
+
+   
+   figure(3);
+   image(Id); colormap(map); hold on;
+   plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+');
+   plot(x_box_kk+1,y_box_kk+1,'-b');
+   plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo');
+   plot(xorig+1,yorig+1,'*m');
+   h = text(xorig-15,yorig-15,'O');
+   set(h,'Color','m','FontSize',14);
+   h2 = text(dxpos(1)-10,dxpos(2)-10,'dX');
+   set(h2,'Color','g','FontSize',14);
+   h3 = text(dypos(1)-25,dypos(2)-3,'dY');
+   set(h3,'Color','g','FontSize',14);
+   xlabel('Xc (in camera frame)');
+   ylabel('Yc (in camera frame)');
+   title('Extracted corners');
+   zoom on;
+   drawnow;
+   hold off;
+   
+   
+   Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)';
+   Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)';
+   Zi = zeros(1,Np);
+   
+   Xgrid = [Xi;Yi;Zi];
+   
+   
+        % All the point coordinates (on the image, and in 3D) - for global optimization:
+
+   x = grid_pts;
+   X = Xgrid;
+   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m
new file mode 100755
index 0000000..035b97d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m
@@ -0,0 +1,46 @@
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+check_active_images;
+
+fc = solution(1:2);%***
+cc = solution(3:4);%***
+alpha_c = solution(5);%***
+kc = solution(6:9);%***
+
+
+% Calibration matrix:
+        
+KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1];
+inv_KK = inv(KK);       
+
+% Extract the extrinsic paramters, and recomputer the collineations
+
+for kk = 1:n_ima,
+   
+   if active_images(kk),   
+      
+      omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***   
+      Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
+      
+        Rckk = rodrigues(omckk);
+   
+        Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];
+   
+        Hkk = Hkk / Hkk(3,3);
+      
+   else
+      
+      omckk = NaN*ones(3,1);   
+      Tckk = NaN*ones(3,1);
+      Rckk = NaN*ones(3,3);
+      Hkk = NaN*ones(3,3);
+      
+   end;
+   
+   eval(['omc_' num2str(kk) ' = omckk;']);
+   eval(['Rc_' num2str(kk) ' = Rckk;']);
+   eval(['Tc_' num2str(kk) ' = Tckk;']);
+        eval(['H_' num2str(kk) '= Hkk;']);
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m
new file mode 100755
index 0000000..841c6ab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m
@@ -0,0 +1,36 @@
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+
+fc = solution(1:2);
+kc = solution(3:6);
+cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3);
+
+% Calibration matrix:
+        
+KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1];
+inv_KK = inv(KK);       
+
+% Extract the extrinsic paramters, and recomputer the collineations
+
+for kk = 1:n_ima,
+   
+   omckk = solution(4+6*(kk-1) + 3:6*kk + 3);
+   
+   Tckk = solution(6*kk+1 + 3:6*kk+3 + 3);
+   
+   Rckk = rodrigues(omckk);
+   
+   Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];
+   
+   Hlkk = Hlkk / Hlkk(3,3);
+   
+   eval(['omc_' num2str(kk) ' = omckk;']);
+   eval(['Rc_' num2str(kk) ' = Rckk;']);
+        eval(['Tc_' num2str(kk) ' = Tckk;']);
+   
+   eval(['Hl_' num2str(kk) '=Hlkk;']);
+   
+end;
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m
new file mode 100755
index 0000000..fbba78e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m
@@ -0,0 +1,185 @@
+%%% INPUT THE IMAGE FILE NAME:
+
+if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'),
+   fprintf(1,'No intrinsic camera parameters available.\n');
+   return;
+end;
+
+dir;
+
+fprintf(1,'\n');
+disp('Computation of the extrinsic parameters from an image of a pattern');
+disp('The intrinsic camera parameters are assumed to be known (previously computed)');
+
+fprintf(1,'\n');
+image_name = input('Image name (full name without extension): ','s');
+
+format_image2 = '0';
+
+while format_image2 == '0',
+   
+   format_image2 =  input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s');
+        
+        if isempty(format_image2),
+        format_image2 = 'ras';
+        end;
+   
+   if lower(format_image2(1)) == 'm',
+      format_image2 = 'ppm';
+   else
+      if lower(format_image2(1)) == 'b',
+         format_image2 = 'bmp';
+      else
+         if lower(format_image2(1)) == 't',
+            format_image2 = 'tif';
+         else
+            if lower(format_image2(1)) == 'p',
+               format_image2 = 'pgm';
+            else
+               if lower(format_image2(1)) == 'j',
+                  format_image2 = 'jpg';
+               else
+                  if lower(format_image2(1)) == 'r',
+                     format_image2 = 'ras';
+                  else  
+                     disp('Invalid image format');
+                     format_image2 = '0'; % Ask for format once again
+                  end;
+               end;
+            end;
+         end;
+      end;
+   end;
+end;
+
+ima_name = [image_name '.' format_image2];
+
+
+%%% READ IN IMAGE:
+
+if format_image2(1) == 'p',
+   if format_image2(2) == 'p',
+      I = double(loadppm(ima_name));
+   else
+      I = double(loadpgm(ima_name));
+   end;
+else
+   if format_image2(1) == 'r',
+      I = readras(ima_name);
+   else
+      I = double(imread(ima_name));
+   end;
+end;
+
+if size(I,3)>1,
+   I = I(:,:,2);
+end;
+
+
+%%% EXTRACT GRID CORNERS:
+
+fprintf(1,'\nExtraction of the grid corners on the image\n');
+
+disp('Window size for corner finder (wintx and winty):');
+wintx = input('wintx ([] = 5) = ');
+if isempty(wintx), wintx = 5; end;
+wintx = round(wintx);
+winty = input('winty ([] = 5) = ');
+if isempty(winty), winty = 5; end;
+winty = round(winty);
+
+fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+
+[x_ext,X_ext,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc);
+
+
+
+%%% Computation of the Extrinsic Parameters attached to the grid:
+
+[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c);
+
+
+%%% Reproject the points on the image:
+
+[x_reproj] = project_points2(X_ext,omc_ext,Tc_ext,fc,cc,kc,alpha_c);
+
+err_reproj = x_ext - x_reproj;
+
+err_std2 = std(err_reproj')';
+
+
+Basis = [X_ext(:,[ind_orig ind_x ind_orig ind_y ind_orig ])];
+
+VX = Basis(:,2) - Basis(:,1);
+VY = Basis(:,4) - Basis(:,1);
+
+nX = norm(VX);
+nY = norm(VY);
+
+VZ = min(nX,nY) * cross(VX/nX,VY/nY);
+
+Basis = [Basis VZ];
+
+[x_basis] = project_points2(Basis,omc_ext,Tc_ext,fc,cc,kc,alpha_c);
+
+dxpos = (x_basis(:,2) + x_basis(:,1))/2;
+dypos = (x_basis(:,4) + x_basis(:,3))/2;
+dzpos = (x_basis(:,6) + x_basis(:,5))/2;
+
+
+
+figure(2);
+image(I);
+colormap(gray(256));
+hold on;
+plot(x_ext(1,:)+1,x_ext(2,:)+1,'r+');
+plot(x_reproj(1,:)+1,x_reproj(2,:)+1,'yo');
+h = text(x_ext(1,ind_orig)-25,x_ext(2,ind_orig)-25,'O');
+set(h,'Color','g','FontSize',14);
+h2 = text(dxpos(1)+1,dxpos(2)-30,'X');
+set(h2,'Color','g','FontSize',14);
+h3 = text(dypos(1)-30,dypos(2)+1,'Y');
+set(h3,'Color','g','FontSize',14);
+h4 = text(dzpos(1)-10,dzpos(2)-20,'Z');
+set(h4,'Color','g','FontSize',14);
+plot(x_basis(1,:)+1,x_basis(2,:)+1,'g-','linewidth',2);
+title('Image points (+) and reprojected grid points (o)');
+hold off;
+
+
+fprintf(1,'\n\nExtrinsic parameters:\n\n');
+fprintf(1,'Translation vector: Tc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',Tc_ext);
+fprintf(1,'Rotation vector:   omc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',omc_ext);
+fprintf(1,'Rotation matrix:    Rc_ext = [ %3.6f \t %3.6f \t %3.6f\n',Rc_ext(1,:)');
+fprintf(1,'                               %3.6f \t %3.6f \t %3.6f\n',Rc_ext(2,:)');
+fprintf(1,'                               %3.6f \t %3.6f \t %3.6f ]\n',Rc_ext(3,:)');
+fprintf(1,'Pixel error:           err = [ %3.5f \t %3.5f ]\n\n',err_std2); 
+
+
+
+
+
+return;
+
+
+% Stores the results:
+
+kk = 1;
+
+% Stores location of grid wrt camera:
+
+eval(['omc_' num2str(kk) ' = omc_ext;']);
+eval(['Tc_' num2str(kk) ' = Tc_ext;']);
+
+% Stores the projected points:
+      
+eval(['y_' num2str(kk) ' = x_reproj;']);
+eval(['X_' num2str(kk) ' = X_ext;']);
+eval(['x_' num2str(kk) ' = x_ext;']);      
+      
+            
+% Organize the points in a grid:
+      
+eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']);
+eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']);
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m
new file mode 100755
index 0000000..b5808f3
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m
@@ -0,0 +1,19 @@
+% Code that clears all empty or NaN variables
+
+varlist = whos;
+
+if ~isempty(varlist),
+   
+   Nvar = size(varlist,1);
+   
+   for c_var = 1:Nvar,
+      
+      var2fix = varlist(c_var).name;
+      
+      fixvariable;
+      
+   end;
+
+end;
+
+clear varlist var2fix Nvar c_var
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m
new file mode 100755
index 0000000..2213431
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m
@@ -0,0 +1,18 @@
+% Code that clears an empty variable, or a NaN vsriable.
+% Does not clear structures, or cells.
+
+if exist('var2fix'),
+   if   eval(['exist(''' var2fix ''') == 1']),
+      if eval(['isempty(' var2fix ')']),
+         eval(['clear ' var2fix ]);
+      else
+         if eval(['~isstruct(' var2fix ')']),
+            if eval(['~iscell(' var2fix ')']),
+               if eval(['isnan(' var2fix '(1))']),
+                  eval(['clear ' var2fix ]);
+               end; 
+            end;
+         end;
+      end;
+   end;
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m
new file mode 100755
index 0000000..56fe496
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m
@@ -0,0 +1,216 @@
+function [out1,out2,out3] = ginput2(arg1)
+%GINPUT Graphical input from mouse.
+%   [X,Y] = GINPUT(N) gets N points from the current axes and returns 
+%   the X- and Y-coordinates in length N vectors X and Y.  The cursor
+%   can be positioned using a mouse (or by using the Arrow Keys on some 
+%   systems).  Data points are entered by pressing a mouse button
+%   or any key on the keyboard except carriage return, which terminates
+%   the input before N points are entered.
+%
+%   [X,Y] = GINPUT gathers an unlimited number of points until the
+%   return key is pressed.
+% 
+%   [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that 
+%   contains a vector of integers specifying which mouse button was
+%   used (1,2,3 from left) or ASCII numbers if a key on the keyboard
+%   was used.
+
+%   Copyright (c) 1984-96 by The MathWorks, Inc.
+%   $Revision: 5.18 $  $Date: 1996/11/10 17:48:08 $
+
+% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines
+% More visible for images
+
+P = NaN*ones(16,16);
+P(1:15,1:15) = 2*ones(15,15);
+P(2:14,2:14) = ones(13,13);
+P(3:13,3:13) = NaN*ones(11,11);
+P(6:10,6:10) = 2*ones(5,5);
+P(7:9,7:9) = 1*ones(3,3);
+
+out1 = []; out2 = []; out3 = []; y = [];
+c = computer;
+if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA')
+    tp = get(0,'TerminalProtocol');
+else
+    tp = 'micro';
+end
+
+if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'),
+    if nargout == 1,
+        if nargin == 1,
+            eval('out1 = trmginput(arg1);');
+        else
+            eval('out1 = trmginput;');
+        end
+    elseif nargout == 2 | nargout == 0,
+        if nargin == 1,
+            eval('[out1,out2] = trmginput(arg1);');
+        else
+            eval('[out1,out2] = trmginput;');
+        end
+        if  nargout == 0
+            out1 = [ out1 out2 ];
+        end
+    elseif nargout == 3,
+        if nargin == 1,
+            eval('[out1,out2,out3] = trmginput(arg1);');
+        else
+            eval('[out1,out2,out3] = trmginput;');
+        end
+    end
+else
+
+    fig = gcf;
+    figure(gcf);
+
+    if nargin == 0
+        how_many = -1;
+        b = [];
+    else
+        how_many = arg1;
+        b = [];
+        if  isstr(how_many) ...
+            | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ...
+            | ~(fix(how_many) == how_many) ...
+            | how_many < 0
+            error('Requires a positive integer.')
+        end
+        if how_many == 0
+            ptr_fig = 0;
+            while(ptr_fig ~= fig)
+                ptr_fig = get(0,'PointerWindow');
+            end
+            scrn_pt = get(0,'PointerLocation');
+            loc = get(fig,'Position');
+            pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)];
+            out1 = pt(1); y = pt(2);
+        elseif how_many < 0
+            error('Argument must be a positive integer.')
+        end
+    end
+
+pointer = get(gcf,'pointer');
+
+set(gcf,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]);
+%set(gcf,'pointer','crosshair');
+    fig_units = get(fig,'units');
+    char = 0;
+
+    while how_many ~= 0
+        % Use no-side effect WAITFORBUTTONPRESS
+        waserr = 0;
+        eval('keydown = wfbp;', 'waserr = 1;');
+        if(waserr == 1)
+      if(ishandle(fig))
+        set(fig,'pointer',pointer,'units',fig_units);
+        error('Interrupted');
+      else
+        error('Interrupted by figure deletion');
+      end
+        end
+      
+        ptr_fig = get(0,'CurrentFigure');
+        if(ptr_fig == fig)
+            if keydown
+                char = get(fig, 'CurrentCharacter');
+                button = abs(get(fig, 'CurrentCharacter'));
+                scrn_pt = get(0, 'PointerLocation');
+                set(fig,'units','pixels')
+                loc = get(fig, 'Position');
+                pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)];
+                set(fig,'CurrentPoint',pt);
+            else
+                button = get(fig, 'SelectionType');
+                if strcmp(button,'open')
+                    button = b(length(b));
+                elseif strcmp(button,'normal')
+                    button = 1;
+                elseif strcmp(button,'extend')
+                    button = 2;
+                elseif strcmp(button,'alt')
+                    button = 3;
+                else
+                    error('Invalid mouse selection.')
+                end
+            end
+            pt = get(gca, 'CurrentPoint');
+
+            how_many = how_many - 1;
+
+            if(char == 13) % & how_many ~= 0)
+                % if the return key was pressed, char will == 13,
+                % and that's our signal to break out of here whether
+                % or not we have collected all the requested data
+                % points.  
+                % If this was an early breakout, don't include
+                % the <Return> key info in the return arrays.
+                % We will no longer count it if it's the last input.
+                break;
+            end
+
+            out1 = [out1;pt(1,1)];
+            y = [y;pt(1,2)];
+            b = [b;button];
+        end
+    end
+
+    set(fig,'pointer',pointer,'units',fig_units);
+
+    if nargout > 1
+        out2 = y;
+        if nargout > 2
+            out3 = b;
+        end
+    else
+        out1 = [out1 y];
+    end
+
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function key = wfbp
+%WFBP   Replacement for WAITFORBUTTONPRESS that has no side effects.
+
+% Remove figure button functions
+fprops = {'windowbuttonupfcn','buttondownfcn', ...
+          'windowbuttondownfcn','windowbuttonmotionfcn'};
+fig = gcf;
+fvals = get(fig,fprops);
+set(fig,fprops,{'','','',''})
+
+% Remove all other buttondown functions
+ax = findobj(fig,'type','axes');
+if isempty(ax)
+    ch = {};
+else
+    ch = get(ax,{'Children'});
+end
+for i=1:length(ch),
+    ch{i} = ch{i}(:)';
+end
+h = [ax(:)',ch{:}];
+vals = get(h,{'buttondownfcn'});
+mt = repmat({''},size(vals));
+set(h,{'buttondownfcn'},mt);
+
+% Now wait for that buttonpress, and check for error conditions
+waserr = 0;
+eval(['if nargout==0,', ...
+      '   waitforbuttonpress,', ...
+      'else,', ...
+      '   keydown = waitforbuttonpress;',...
+      'end' ], 'waserr = 1;');
+
+% Put everything back
+if(ishandle(fig))
+  set(fig,fprops,fvals)
+  set(h,{'buttondownfcn'},vals)
+end
+
+if(waserr == 1)
+  error('Interrupted');
+end
+
+if nargout>0, key = keydown; end
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m
new file mode 100755
index 0000000..ad19f64
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m
@@ -0,0 +1,139 @@
+%go_calib_optim
+%
+%Main calibration function. Computes the intrinsic andextrinsic parameters.
+%Runs as a script.
+%
+%INPUT: x_1,x_2,x_3,...: Feature locations on the images
+%       X_1,X_2,X_3,...: Corresponding grid coordinates
+%
+%OUTPUT: fc: Camera focal length
+%        cc: Principal point coordinates
+%        kc: Distortion coefficients
+%        KK: The camera matrix (containing fc and cc)
+%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
+%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
+%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
+%
+%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
+%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
+%
+%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
+%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
+%
+%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
+%      corresponding entry in the active_images vector to zero.
+%
+%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
+%that is so far implemented to work only with 2D rigs.
+%In the future, a more general function will be there.
+%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length
+
+
+if ~exist('n_ima'),
+   data_calib; % Load the images
+   click_calib; % Extract the corners
+end;
+
+
+check_active_images;
+
+check_extracted_images;
+
+check_active_images;
+ 
+
+desactivated_images = [];
+
+
+if ~exist('center_optim'),
+        center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
+                     %%% Required when using one image, and VERY RECOMMENDED WHEN USING LESS THAN 4 images
+end;                  
+                  
+% Check 3D-ness of the calibration rig:
+rig3D = 0;
+for kk = ind_active,
+   eval(['X_kk = X_' num2str(kk) ';']);
+   if is3D(X_kk),
+      rig3D = 1;
+   end;
+end;
+
+
+if center_optim & (length(ind_active) < 2) & ~rig3D,
+   fprintf(1,'\nPrincipal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n');
+   center_optim = 0; %%% when using a single image, please, no principal point estimation!!!
+   est_alpha = 0;
+end;
+
+if ~exist('dont_ask'),
+   dont_ask = 0;
+end;
+
+if center_optim & (length(ind_active) < 5),
+   fprintf(1,'\nThe principal point estimation may be unreliable (using less than 5 images for calibration).\n');
+   if ~dont_ask,
+      quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) ');
+      center_optim = isempty(quest);
+   end;
+end;
+
+if center_optim,
+   fprintf(1,'\nINFO: To reject the principal point from the optimization, set center_optim = 0 in go_calib_optim.m\n');
+end;
+
+if ~exist('est_alpha'),
+   est_alpha = 0; % by default, do not estimate skew
+end;
+
+if ~center_optim & (est_alpha),
+   fprintf(1,'WARNING: Since there is no principal point estimation, no skew estimation (est_alpha = 0)\n');
+   est_alpha = 0;
+else
+        if ~est_alpha,
+        fprintf(1,'WARNING: Skew not optimized. Check variable est_alpha.\n');
+        alpha_c = 0;
+        else
+        fprintf(1,'WARNING: Skew is optimized. To disable skew estimation, set est_alpha=0.\n');
+        end;   
+end;
+
+
+if ~exist('est_dist');
+   est_dist = [1;1;1;1];
+end;
+if ~prod(est_dist),
+   fprintf(1,'\nWARNING: Distortion not fully estimated. Check variable est_dist.\n');
+end;
+
+
+
+
+%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation)
+go_calib_optim_iter;
+
+
+
+if ~isempty(desactivated_images),
+   
+   param_list_save = param_list;
+   
+   fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n');
+   active_images(desactivated_images) = ones(1,length(desactivated_images));
+   desactivated_images = [];
+   
+   go_calib_optim_iter;
+   
+   if ~isempty(desactivated_images),
+      fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] );
+   end;
+   
+   param_list = [param_list_save(:,1:end-1) param_list];
+   
+end;
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+%graphout_calib;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m
new file mode 100755
index 0000000..e3d22f6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m
@@ -0,0 +1,394 @@
+%go_calib_optim_iter
+%
+%Main calibration function. Computes the intrinsic andextrinsic parameters.
+%Runs as a script.
+%
+%INPUT: x_1,x_2,x_3,...: Feature locations on the images
+%       X_1,X_2,X_3,...: Corresponding grid coordinates
+%
+%OUTPUT: fc: Camera focal length
+%        cc: Principal point coordinates
+%        kc: Distortion coefficients
+%        KK: The camera matrix (containing fc and cc)
+%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
+%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
+%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
+%
+%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
+%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
+%
+%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
+%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
+%
+%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
+%      corresponding entry in the active_images vector to zero.
+%
+%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
+%that is so far implemented to work only with 2D rigs.
+%In the future, a more general function will be there.
+%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length
+
+
+if ~exist('center_optim'),
+        center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
+end;
+
+if ~exist('est_dist'),
+   est_dist = [1;1;1;1];
+end;
+
+if ~exist('est_alpha'),
+   est_alpha = 0; % by default, do not estimate skew
+end;
+
+
+% Little fix in case of stupid values in the binary variables:
+center_optim = ~~center_optim;
+est_alpha = ~~est_alpha;
+est_dist = ~~est_dist;
+
+
+if ~exist('nx')&~exist('ny'),
+   fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n');
+   nx = 640;
+   ny = 480;
+end;
+
+
+check_active_images;
+
+
+quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)
+
+
+if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image
+   fprintf(1,'Principal point not optimized (center_optim=0). It is kept at the center of the image.\n');
+   cc = [(nx-1)/2;(ny-1)/2];
+end;
+
+
+if ~prod(est_dist),
+   fprintf(1,'\nDistortion not fully estimated. Check variable est_dist.\n');
+end;
+
+if ~est_alpha,
+   fprintf(1,'Skew not optimized (est_alpha=0).\n');
+   alpha_c = 0;
+end;
+
+
+% Check 3D-ness of the calibration rig:
+rig3D = 0;
+for kk = ind_active,
+   eval(['X_kk = X_' num2str(kk) ';']);
+   if is3D(X_kk),
+      rig3D = 1;
+   end;
+end;
+
+% If the rig is 3D, then no choice: the only valid initialization is manual!
+if rig3D,
+   quick_init = 1;
+end;
+
+
+
+alpha_smooth = 1; % set alpha_smooth = 1; for steepest gradient descent
+
+
+% Conditioning threshold for view rejection
+thresh_cond = 1e6;
+
+
+
+%% Initialization of the intrinsic parameters (if necessary)
+
+if ~exist('cc'),
+   fprintf(1,'Initialization of the principal point at the center of the image.\n');
+   cc = [(nx-1)/2;(ny-1)/2];
+   alpha_smooth = 0.4; % slow convergence
+end;
+
+
+if ~exist('kc'),
+   fprintf(1,'Initialization of the image distortion to zero.\n');
+   kc = zeros(4,1);
+   alpha_smooth = 0.4; % slow convergence
+end;
+
+if ~exist('alpha_c'),
+   fprintf(1,'Initialization of the image skew to zero.\n');
+   alpha_c = 0;
+   alpha_smooth = 0.4; % slow convergence
+end;
+
+if ~exist('fc')& quick_init,
+   FOV_angle = 35; % Initial camera field of view in degrees
+   fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']);
+   fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1);
+   alpha_smooth = 0.4; % slow 
+end;
+
+
+if ~exist('fc'),
+   % Initialization of the intrinsic parameters:
+   fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n')
+   init_intrinsic_param; % The right way to go (if quick_init is not active)!
+   alpha_smooth = 0.4; % slow convergence
+end;
+
+
+if ~prod(est_dist),
+   % If no distortion estimated, set to zero the variables that are not estimated
+   kc = kc .* est_dist;
+end;
+
+
+
+
+
+%% Initialization of the extrinsic parameters for global minimization:
+
+
+init_param = [fc;cc;alpha_c;kc;zeros(6,1)]; 
+
+
+
+for kk = 1:n_ima,
+
+        if exist(['x_' num2str(kk)]),
+   
+        eval(['x_kk = x_' num2str(kk) ';']);
+      eval(['X_kk = X_' num2str(kk) ';']);
+      
+        if (isnan(x_kk(1,1))),
+        if active_images(kk),
+                fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
+                fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
+        end;
+        active_images(kk) = 0;
+      end;
+      if active_images(kk),
+         [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);
+         [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond);
+         if cond(JJ_kk)> thresh_cond,
+            active_images(kk) = 0;
+            omckk = NaN*ones(3,1);
+            Tckk = NaN*ones(3,1);
+            fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk)
+            desactivated_images = [desactivated_images kk];
+         end;
+         if isnan(omckk(1,1)),
+            %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk])
+            active_images(kk) = 0;
+         end;
+      else
+         omckk = NaN*ones(3,1);
+         Tckk = NaN*ones(3,1);
+      end;
+   
+   else
+      
+      omckk = NaN*ones(3,1);
+      Tckk = NaN*ones(3,1);
+      
+      if active_images(kk),
+         fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
+         fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
+      end;
+      
+      active_images(kk) = 0;
+      
+   end;
+   
+   eval(['omc_' num2str(kk) ' = omckk;']);
+   eval(['Tc_' num2str(kk) ' = Tckk;']);
+   
+   init_param = [init_param; omckk ; Tckk];
+   
+end;
+
+
+check_active_images;
+
+
+
+%-------------------- Main Optimization:
+
+fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));
+
+
+param = init_param;
+change = 1;
+
+iter = 0;
+
+fprintf(1,'Gradient descent iterations: ');
+
+param_list = param;
+
+MaxIter = 30;
+
+
+while (change > 1e-6)&(iter < MaxIter),
+   
+   fprintf(1,'%d...',iter+1);
+   
+   
+   %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active
+   %% images) through a one step steepest gradient descent.
+   
+        JJ = [];
+        ex = [];
+           
+   f = param(1:2);
+   c = param(3:4);
+   alpha = param(5);
+   k = param(6:9);
+
+   
+   for kk = 1:n_ima,
+      
+      if active_images(kk),
+         
+                %omckk = param(4+6*(kk-1) + 3:6*kk + 3); 
+        
+         %Tckk = param(6*kk+1 + 3:6*kk+3 + 3);  
+         
+         omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 
+        
+         Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 
+         
+         if isnan(omckk(1)),
+            fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);
+            return;
+         end;
+         
+                eval(['X_kk = X_' num2str(kk) ';']);
+                eval(['x_kk = x_' num2str(kk) ';']);
+        
+                Np = size(X_kk,2);
+        
+                JJkk = zeros(2*Np,n_ima * 6 + 15);
+        
+                [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha);
+      
+                exkk = x_kk - x;
+   
+                ex = [ex;exkk(:)];
+         
+         JJkk(:,1:2) = dxdf;
+                JJkk(:,3:4) = dxdc;
+         JJkk(:,5) = dxdalpha;
+         JJkk(:,6:9) = dxdk;
+                JJkk(:,15+6*(kk-1) + 1:15+6*(kk-1) + 3) = dxdom;
+                JJkk(:,15+6*(kk-1) + 4:15+6*(kk-1) + 6) = dxdT;
+
+         
+         
+         JJ = [JJ;JJkk];
+         
+         
+         % Check if this view is ill-conditioned:
+         JJ_kk = [dxdom dxdT];
+         if cond(JJ_kk)> thresh_cond,
+            active_images(kk) = 0;
+            fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk)
+            desactivated_images = [desactivated_images kk];
+            param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); 
+         end;
+            
+         
+      end;
+        
+   end;
+   
+   
+   % List of active images (necessary if changed):
+   check_active_images;
+   
+   
+   % The following vector helps to select the variables to update (for only active images):
+   
+   ind_Jac = find([ones(2,1);center_optim*ones(2,1);est_alpha;est_dist;zeros(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)])';
+
+   
+   JJ = JJ(:,ind_Jac);
+   
+   JJ2 = JJ'*JJ;
+   
+   
+   % Smoothing coefficient:
+   
+   alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!
+   
+   
+   param_innov = alpha_smooth2*inv(JJ2)*(JJ')*ex;
+   param_up = param(ind_Jac) + param_innov;
+   param(ind_Jac) = param_up;
+   
+   
+   % New intrinsic parameters:
+   
+   fc_current = param(1:2);
+   cc_current = param(3:4);
+   alpha_current = param(5);
+   kc_current = param(6:9);
+
+   
+   % Change on the intrinsic parameters:
+   change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]);
+   
+   
+   %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!!
+   %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)
+   %% The complete gradient descent method is useful to precisely update the intrinsic parameters.
+      
+   MaxIter2 = 20;
+   
+   
+   for kk = 1:n_ima,
+      if active_images(kk),
+         omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
+         Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
+        eval(['X_kk = X_' num2str(kk) ';']);
+         eval(['x_kk = x_' num2str(kk) ';']);
+         [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);
+         [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond);
+         if cond(JJ_kk)> thresh_cond,
+            active_images(kk) = 0;
+            fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk)
+            desactivated_images = [desactivated_images kk];
+            omckk = NaN*ones(3,1);
+            Tckk = NaN*ones(3,1);
+         end;
+         param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk;
+         param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk;
+      end;
+   end;
+   
+   param_list = [param_list param];
+   
+   iter = iter + 1;
+   
+end;
+
+fprintf(1,'\n');
+
+
+solution = param;
+
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters;
+
+comp_error_calib;
+
+fprintf(1,'\n\nCalibration results after optimization:\n\n');
+fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ]\n',fc);
+fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ]\n',cc);
+fprintf(1,'Skew:             alpha_c = [ %3.5f ]   => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi);
+fprintf(1,'Distortion:            kc = [ %3.5f   %3.5f   %3.5f   %3.5f ]\n',kc);   
+fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f ]\n\n',err_std); 
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m
new file mode 100755
index 0000000..dbbc4e0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m
@@ -0,0 +1,158 @@
+
+if ~exist('calib_name')|~exist('format_image'),
+   data_calib;
+   return;
+end;
+
+check_directory;
+
+if ~exist('n_ima'),
+   data_calib;
+   return;
+end;
+
+check_active_images;
+
+
+images_read = active_images;
+
+
+if exist('image_numbers'),
+   first_num = image_numbers(1);
+end;
+
+
+% Just to fix a minor bug:
+if ~exist('first_num'),
+   first_num = image_numbers(1);
+end;
+
+
+image_numbers = first_num:n_ima-1+first_num;
+
+no_image_file = 0;
+
+i = 1;
+
+while (i <= n_ima), % & (~no_image_file),
+   
+   if active_images(i),
+   
+        %fprintf(1,'Loading image %d...\n',i);
+   
+        if ~type_numbering,   
+        number_ext =  num2str(image_numbers(i));
+        else
+        number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i));
+        end;
+        
+      ima_name = [calib_name  number_ext '.' format_image];
+      
+      if i == ind_active(1),
+         fprintf(1,'Loading image ');
+      end;
+      
+      if exist(ima_name),
+         
+         fprintf(1,'%d...',i);
+         
+         if format_image(1) == 'p',
+            if format_image(2) == 'p',
+               Ii = double(loadppm(ima_name));
+            else
+               Ii = double(loadpgm(ima_name));
+            end;
+         else
+            if format_image(1) == 'r',
+               Ii = readras(ima_name);
+            else
+               Ii = double(imread(ima_name));
+            end;
+         end;
+
+        
+                if size(Ii,3)>1,
+                Ii = Ii(:,:,2);
+                end;
+   
+                eval(['I_' num2str(i) ' = Ii;']);
+         
+      else
+         
+         fprintf(1,'%d...no image...',i);
+         
+         images_read(i) = 0;
+         
+         %no_image_file = 1;
+         
+      end;
+      
+   end;
+   
+   i = i+1;   
+   
+end;
+
+
+ind_read = find(images_read);
+
+
+
+
+if isempty(ind_read),
+   
+   fprintf(1,'\nWARNING! No image were read\n');
+   
+   no_image_file = 1;
+   
+   
+else
+   
+
+   %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n');
+
+   
+   if no_image_file,
+      
+        %fprintf(1,'WARNING! Some images were not read properly\n');
+     
+   end;
+     
+   
+   fprintf(1,'\n');
+
+   if size(I_1,1)~=480,
+        small_calib_image = 1;
+        else
+        small_calib_image = 0;
+        end;
+   
+   [Hcal,Wcal] = size(I_1);     % size of the calibration image
+   
+   [ny,nx] = size(I_1);
+   
+   clickname = [];
+   
+   map = gray(256);
+   
+        %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image';
+
+        %eval(string_save);
+
+        disp('done');
+        %click_calib;
+
+end;
+
+if ~exist('map'), map = gray(256); end;
+
+active_images = images_read;
+
+% Show all the calibration images:
+
+
+if ~isempty(ind_read),
+  
+   mosaic;
+
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m
new file mode 100755
index 0000000..94a5240
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m
@@ -0,0 +1,158 @@
+%init_intrinsic_param
+%
+%Initialization of the intrinsic parameters.
+%Runs as a script.
+%
+%INPUT: x_1,x_2,x_3,...: Feature locations on the images
+%       X_1,X_2,X_3,...: Corresponding grid coordinates
+%
+%OUTPUT: fc: Camera focal length
+%        cc: Principal point coordinates
+%             kc: Distortion coefficients
+%        alpha_c: skew coefficient
+%        KK: The camera matrix (containing fc, cc and alpha_c)
+%
+%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes
+%        the focal length fc from orthogonal vanishing points constraint.
+%        The principal point cc is assumed at the center of the image.
+%        Assumes no image distortion (kc = [0;0;0;0])
+%
+%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
+%      corresponding entry in the active_images vector to zero.
+%
+%
+%Important function called within that program:
+%
+%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
+%
+%
+%VERY IMPORTANT: This function works only with 2D rigs.
+%In the future, a more general function will be there (working with 3D rigs as well).
+
+
+
+check_active_images;
+
+if ~exist(['x_' num2str(ind_active(1)) ]),
+   click_calib;
+end;
+
+
+fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active));
+
+
+% Initialize the homographies:
+
+for kk = 1:n_ima,
+   eval(['x_kk = x_' num2str(kk) ';']);
+   eval(['X_kk = X_' num2str(kk) ';']);
+   if (isnan(x_kk(1,1))),
+      if active_images(kk),
+         fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
+         fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
+      end;
+      active_images(kk) = 0;
+   end;
+   if active_images(kk),
+      eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']);
+   else
+      eval(['H_' num2str(kk) ' = NaN*ones(3,3);']);
+   end;
+end;
+
+check_active_images;
+
+% initial guess for principal point and distortion:
+
+if ~exist('nx'), [ny,nx] = size(I); end;
+
+c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image
+k_init = [0;0;0;0]; % initialize to zero (no distortion)
+
+
+
+% Compute explicitely the focal length using all the (mutually orthogonal) vanishing points
+% note: The vanihing points are hidden in the planar collineations H_kk
+
+A = [];
+b = [];
+
+% matrix that subtract the principal point:
+Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1];
+
+for kk=1:n_ima,
+   
+   if active_images(kk),
+      
+        eval(['Hkk = H_' num2str(kk) ';']);
+   
+        Hkk = Sub_cc * Hkk;   
+   
+        % Extract vanishing points (direct and diagonals):
+   
+        V_hori_pix = Hkk(:,1);
+        V_vert_pix = Hkk(:,2);
+        V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2;
+        V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2;
+   
+        V_hori_pix = V_hori_pix/norm(V_hori_pix);
+        V_vert_pix = V_vert_pix/norm(V_vert_pix);
+        V_diag1_pix = V_diag1_pix/norm(V_diag1_pix);
+        V_diag2_pix = V_diag2_pix/norm(V_diag2_pix);
+      
+      a1 = V_hori_pix(1);
+      b1 = V_hori_pix(2);
+      c1 = V_hori_pix(3);
+      
+        a2 = V_vert_pix(1);
+        b2 = V_vert_pix(2);
+        c2 = V_vert_pix(3);
+        
+        a3 = V_diag1_pix(1);
+        b3 = V_diag1_pix(2);
+        c3 = V_diag1_pix(3);
+        
+        a4 = V_diag2_pix(1);
+        b4 = V_diag2_pix(2);
+        c4 = V_diag2_pix(3);
+        
+           A_kk = [a1*a2  b1*b2;
+                    a3*a4  b3*b4];
+   
+        b_kk = -[c1*c2;c3*c4];
+        
+                
+        A = [A;A_kk];
+        b = [b;b_kk];
+      
+   end;
+   
+end;
+
+
+% use all the vanishing points to estimate focal length:
+
+f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess
+
+alpha_init = 0;
+
+%f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used
+
+
+% Global calibration matrix (initial guess):
+        
+KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1];
+inv_KK = inv(KK);
+
+
+cc = c_init;
+fc = f_init;
+kc = k_init;
+alpha_c = alpha_init;
+
+
+fprintf(1,'\n\nCalibration parameters after initialization:\n\n');
+fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ]\n',fc);
+fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ]\n',cc);
+fprintf(1,'Skew:             alpha_c = [ %3.5f ]   => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi);
+fprintf(1,'Distortion:            kc = [ %3.5f   %3.5f   %3.5f   %3.5f ]\n',kc);   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m
new file mode 100755
index 0000000..ab00b3d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m
@@ -0,0 +1,19 @@
+function test = is3D(X),
+
+
+Np = size(X,2);
+
+%% Check for planarity of the structure:
+
+X_mean = mean(X')';
+
+Y = X - (X_mean*ones(1,Np));
+
+YY = Y*Y';
+
+[U,S,V] = svd(YY);
+
+r = S(3,3)/S(2,2);
+
+test = (r > 1e-3);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m
new file mode 100755
index 0000000..a0f50d2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m
@@ -0,0 +1,10 @@
+if ~exist('Calib_Results.mat'),
+   fprintf(1,'\nCalibration file Calib_Results.mat not found!\n');
+   return;
+end;
+
+fprintf(1,'\nLoading calibration results from Calib_Results.mat\n');
+
+load Calib_Results
+
+fprintf(1,'done\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m
new file mode 100755
index 0000000..91b6f89
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m
@@ -0,0 +1,52 @@
+%LOADINR        Load an INRIMAGE format file
+%
+%       LOADINR(filename, im)
+%
+%       Load an INRIA image format file and return it as a matrix
+%
+% SEE ALSO:     saveinr
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+
+% Peter Corke 1996
+
+function im = loadinr(fname, im)
+
+        fid = fopen(fname, 'r');
+
+        s = fgets(fid);
+        if strcmp(s(1:12), '#INRIMAGE-4#') == 0,
+                error('not INRIMAGE format');
+        end
+
+        % not very complete, only looks for the X/YDIM keys
+        while 1,
+                s = fgets(fid);
+                n = length(s) - 1;
+                if s(1) == '#',
+                        break
+                end
+                if strcmp(s(1:5), 'XDIM='),
+                        cols = str2num(s(6:n));
+                end
+                if strcmp(s(1:5), 'YDIM='),
+                        rows = str2num(s(6:n));
+                end
+                if strcmp(s(1:4), 'CPU='),
+                        if strcmp(s(5:n), 'sun') == 0,
+                                error('not sun data ordering');
+                        end
+                end
+                
+        end
+        disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)])
+
+        % now the binary data
+        fseek(fid, 256, 'bof');
+        [im count] = fread(fid, [cols rows], 'float32');
+        im = im';
+        if count ~= (rows*cols),
+                error('file too short');
+        end
+        fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m
new file mode 100755
index 0000000..dfa8b61
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m
@@ -0,0 +1,89 @@
+%LOADPGM        Load a PGM image
+%
+%       I = loadpgm(filename)
+%
+%       Returns a matrix containing the image loaded from the PGM format
+%       file filename.  Handles ASCII (P2) and binary (P5) PGM file formats.
+%
+%       If the filename has no extension, and open fails, a '.pgm' will
+%       be appended.
+%
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+
+% Peter Corke 1994
+
+function I = loadpgm(file)
+        white = [' ' 9 10 13];  % space, tab, lf, cr
+        white = setstr(white);
+
+        fid = fopen(file, 'r');
+        if fid < 0,
+                fid = fopen([file '.pgm'], 'r');
+        end
+        if fid < 0,
+                error('Couldn''t open file');
+        end
+                
+        magic = fread(fid, 2, 'char');
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        cols = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        rows = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        maxval = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        if magic(1) == 'P',
+                if magic(2) == '2',
+                        %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)])
+                        I = fscanf(fid, '%d', [cols rows])';
+                elseif magic(2) == '5',
+                        %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)])
+                        if maxval == 1,
+                                fmt = 'unint1';
+                        elseif maxval == 15,
+                                fmt = 'uint4';
+                        elseif maxval == 255,
+                                fmt = 'uint8';
+                        elseif maxval == 2^32-1,
+                                fmt = 'uint32';
+                        end
+                        I = fread(fid, [cols rows], fmt)';
+                else
+                        disp('Not a PGM file');
+                end
+        end
+        fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m
new file mode 100755
index 0000000..0c004fc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m
@@ -0,0 +1,109 @@
+%LOADPPM        Load a PPM image
+%
+%       I = loadppm(filename)
+%
+%       Returns a matrix containing the image loaded from the PPM format
+%       file filename.  Handles ASCII (P3) and binary (P6) PPM file formats.
+%
+%       If the filename has no extension, and open fails, a '.ppm' and
+%       '.pnm' extension will be tried.
+%
+% SEE ALSO:     saveppm loadpgm
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+
+% Peter Corke 1994
+
+function I = loadppm(file)
+        white = [' ' 9 10 13];  % space, tab, lf, cr
+        white = setstr(white);
+
+        fid = fopen(file, 'r');
+        if fid < 0,
+                fid = fopen([file '.ppm'], 'r');
+        end
+        if fid < 0,
+                fid = fopen([file '.pnm'], 'r');
+        end
+        if fid < 0,
+                error('Couldn''t open file');
+        end
+                
+        magic = fread(fid, 2, 'char');
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        cols = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        rows = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        maxval = fscanf(fid, '%d', 1);
+        while 1
+                c = fread(fid,1,'char');
+                if c == '#',
+                        fgetl(fid);
+                elseif ~any(c == white)
+                        fseek(fid, -1, 'cof');  % unputc()
+                        break;
+                end
+        end
+        if magic(1) == 'P',
+                if magic(2) == '3',
+                        %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)])
+                        I = fscanf(fid, '%d', [cols*3 rows]);
+                elseif magic(2) == '6',
+                        %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)])
+                        if maxval == 1,
+                                fmt = 'unint1';
+                        elseif maxval == 15,
+                                fmt = 'uint4';
+                        elseif maxval == 255,
+                                fmt = 'uint8';
+                        elseif maxval == 2^32-1,
+                                fmt = 'uint32';
+                        end
+                        I = fread(fid, [cols*3 rows], fmt);
+                else
+                        disp('Not a PPM file');
+                end
+        end
+        %
+        % now the matrix has interleaved columns of R, G, B
+        %
+        I = I';
+        size(I);
+        R = I(:,1:3:(cols*3));
+        G = I(:,2:3:(cols*3));
+        B = I(:,3:3:(cols*3));
+        fclose(fid);
+   
+   
+   I = zeros(rows,cols,3);
+   I(:,:,1) = R;
+   I(:,:,2) = G;
+   I(:,:,3) = B;
+   I = uint8(I);
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m
new file mode 100755
index 0000000..0d18a62
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m
@@ -0,0 +1,7 @@
+function [m,s] = mean_std_robust(x);
+
+x = x(:);
+
+m = median(x);
+
+s = median(abs(x - m))*1.4836;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m
new file mode 100755
index 0000000..b056661
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m
@@ -0,0 +1,92 @@
+
+if ~exist('I_1'),
+   active_images_save = active_images;
+   ima_read_calib;
+   active_images = active_images_save;
+   check_active_images;
+end;
+
+check_active_images;
+
+if isempty(ind_read),
+   return;
+end;
+
+
+n_col = floor(sqrt(n_ima*nx/ny));
+
+n_row = ceil(n_ima / n_col);
+
+
+ker2 = 1;
+for ii  = 1:n_col,
+   ker2 = conv(ker2,[1/4 1/2 1/4]);
+end;
+
+
+II = I_1(1:n_col:end,1:n_col:end);
+
+[ny2,nx2] = size(II);
+
+
+
+kk_c = 1;
+
+II_mosaic = [];
+
+for jj = 1:n_row,
+
+        
+        II_row = [];
+        
+        for ii = 1:n_col,
+        
+        if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima),
+        
+           if active_images(kk_c),
+              eval(['I = I_' num2str(kk_c) ';']);
+              %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing
+              I = I(1:n_col:end,1:n_col:end);
+           else
+              I = zeros(ny2,nx2);
+           end;
+        
+        else
+        
+        I = zeros(ny2,nx2);
+        
+        end;
+        
+                
+        
+        II_row = [II_row I];
+        
+        if ii ~= n_col,
+      
+        II_row = [II_row zeros(ny2,3)];
+      
+        end;
+        
+        
+        kk_c = kk_c + 1;
+        
+        end;
+   
+   nn2 = size(II_row,2);
+   
+   if jj ~= n_row,
+      II_row = [II_row; zeros(3,nn2)];
+   end;
+   
+   II_mosaic = [II_mosaic ; II_row];
+   
+end;
+
+figure(2);
+image(II_mosaic);
+colormap(gray(256));
+title('Calibration images');
+set(gca,'Xtick',[])
+set(gca,'Ytick',[])
+axis('image');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m
new file mode 100755
index 0000000..6dc7149
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m
@@ -0,0 +1,43 @@
+function [xn] = normalize(x_kk,fc,cc,kc,alpha_c),
+
+%normalize
+%
+%[xn] = normalize(x_kk,fc,cc,kc,alpha_c)
+%
+%Computes the normalized coordinates xn given the pixel coordinates x_kk
+%and the intrinsic camera parameters fc, cc and kc.
+%
+%INPUT: x_kk: Feature locations on the images
+%       fc: Camera focal length
+%       cc: Principal point coordinates
+%       kc: Distortion coefficients
+%       alpha_c: Skew coefficient
+%
+%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix)
+%
+%Important functions called within that program:
+%
+%comp_distortion_oulu: undistort pixel coordinates.
+
+if nargin < 5,
+   alpha_c = 0;
+   if nargin < 4;
+      kc = [0;0;0;0];
+      if nargin < 3;
+         cc = [0;0];
+         if nargin < 2,
+            fc = [1;1];
+         end;
+      end;
+   end;
+end;
+
+
+% First subtract principal point, and divide by the focal length:
+temp = (x_kk(2,:) - cc(2))/fc(2);
+x_distort = [(x_kk(1,:) - cc(1))/fc(1) - alpha_c*temp;temp];
+
+
+%Compensate for lens distortion:
+
+xn = comp_distortion_oulu(x_distort,kc);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m
new file mode 100755
index 0000000..c96ccb7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m
@@ -0,0 +1,26 @@
+function img = pgmread(filename)
+% function img = pgmread(filename)
+%   this is my version of pgmread for the pgm file created by XV.
+%   
+%  this program also corrects for the shifts in the image from pm file.
+
+fid = fopen(filename,'r');
+fscanf(fid, 'P5\n');
+cmt = '#';
+while findstr(cmt, '#'),
+  cmt = fgets(fid);
+   if length(findstr(cmt, '#')) ~= 1,
+      YX = sscanf(cmt, '%d %d');
+      y = YX(1); x = YX(2);
+   end
+end
+
+%fgets(fid);
+
+%img = fscanf(fid,'%d',size);
+%img = img';
+
+img = fread(fid,[y,x],'uint8');
+img = img';
+fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m
new file mode 100755
index 0000000..c5c4a34
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m
@@ -0,0 +1,53 @@
+function [x] = project2_oulu(X,R,T,f,t,k)
+%PROJECT     Subsidiary to calib
+
+%         (c) Pietro Perona -- March 24, 1994
+%         California Institute of Technology
+%         Pasadena, CA
+%         
+%         Renamed because project exists in matlab 5.2!!!
+%         Now uses the more elaborate intrinsic model from Oulu
+
+
+
+[m,n] = size(X);
+
+Y = R*X + T*ones(1,n);
+Z = Y(3,:);
+
+f = f(:); %% make a column vector
+if length(f)==1,
+   f = [f f]';
+end;
+
+x = (Y(1:2,:) ./ (ones(2,1) * Z)) ;
+
+
+radius_2 = x(1,:).^2 + x(2,:).^2;
+
+if length(k) > 1,
+
+   radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2));
+   
+   if length(k) < 4,
+      
+      delta_x = zeros(2,n); 
+      
+   else
+   
+      delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ;
+            k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)];
+      
+   end;
+      
+
+else
+   
+   radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2));
+
+   delta_x = zeros(2,n);
+   
+end;
+
+
+x = (x .* radial_distortion + delta_x).* (f * ones(1,n))  + t*ones(1,n);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m
new file mode 100755
index 0000000..1823490
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m
@@ -0,0 +1,276 @@
+function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)
+
+%project_points.m
+%
+%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)
+%
+%Projects a 3D structure onto the image plane.
+%
+%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
+%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
+%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
+%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
+%       c: principal point location in pixel units (2x1 vector)
+%       k: Distortion coefficients (radial and tangential) (4x1 vector)
+%
+%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
+%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
+%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
+%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix)
+%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
+%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
+%
+%Definitions:
+%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
+%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
+%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
+%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
+%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
+%call r^2 = a^2 + b^2.
+%The distorted point coordinates are: xd = [xx;yy] where:
+%
+%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
+%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
+%
+%The left terms correspond to radial distortion, the right terms correspond to tangential distortion
+%
+%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
+%
+%xxp = f(1)*xx + c(1)
+%yyp = f(2)*yy + c(2)
+%
+%
+%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
+%
+%
+%Important function called within that program:
+%
+%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
+%
+%rigid_motion.m: Computes the rigid motion transformation of a given structure
+
+
+
+if nargin < 6,
+   k = zeros(4,1);
+   if nargin < 5,
+      c = zeros(2,1);
+      if nargin < 4,
+         f = ones(2,1);
+         if nargin < 3,
+            T = zeros(3,1);
+            if nargin < 2,
+               om = zeros(3,1);
+               if nargin < 1,
+                  error('Need at least a 3D structure to project (in project_points.m)');
+                  return;
+               end;
+            end;
+         end;
+      end;
+   end;
+end;
+
+
+[m,n] = size(X);
+
+[Y,dYdom,dYdT] = rigid_motion(X,om,T);
+
+
+inv_Z = 1./Y(3,:);
+
+x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;
+
+
+bb = (-x(1,:) .* inv_Z)'*ones(1,3);
+cc = (-x(2,:) .* inv_Z)'*ones(1,3);
+
+
+dxdom = zeros(2*n,3);
+dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
+dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);
+
+dxdT = zeros(2*n,3);
+dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
+dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
+
+
+% Add distortion:
+
+r2 = x(1,:).^2 + x(2,:).^2;
+
+
+
+dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
+dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
+
+
+r4 = r2.^2;
+
+dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
+dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;
+
+
+% Radial distortion:
+
+cdist = 1 + k(1) * r2 + k(2) * r4;
+
+dcdistdom = k(1) * dr2dom + k(2) * dr4dom;
+dcdistdT = k(1) * dr2dT+ k(2) * dr4dT;
+dcdistdk = [ r2' r4' zeros(n,2)];
+
+
+xd1 = x .* (ones(2,1)*cdist);
+
+dxd1dom = zeros(2*n,3);
+dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
+dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
+coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
+dxd1dom = dxd1dom + coeff.* dxdom;
+
+dxd1dT = zeros(2*n,3);
+dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
+dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
+dxd1dT = dxd1dT + coeff.* dxdT;
+
+dxd1dk = zeros(2*n,4);
+dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
+dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;
+
+
+
+% tangential distortion:
+
+a1 = 2.*x(1,:).*x(2,:);
+a2 = r2 + 2*x(1,:).^2;
+a3 = r2 + 2*x(2,:).^2;
+
+delta_x = [k(3)*a1 + k(4)*a2 ;
+   k(3) * a3 + k(4)*a1];
+
+
+ddelta_xdx = zeros(2*n,2*n);
+aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
+bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
+cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);
+
+ddelta_xdom = zeros(2*n,3);
+ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
+ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);
+
+ddelta_xdT = zeros(2*n,3);
+ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
+ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);
+
+ddelta_xdk = zeros(2*n,4);
+ddelta_xdk(1:2:end,3) = a1';
+ddelta_xdk(1:2:end,4) = a2';
+ddelta_xdk(2:2:end,3) = a3';
+ddelta_xdk(2:2:end,4) = a1';
+
+
+
+xd2 = xd1 + delta_x;
+
+dxd2dom = dxd1dom + ddelta_xdom ;
+dxd2dT = dxd1dT + ddelta_xdT;
+dxd2dk = dxd1dk + ddelta_xdk ;
+
+
+% Pixel coordinates:
+
+xp = xd2 .* (f * ones(1,n))  +  c*ones(1,n);
+
+coeff = reshape(f*ones(1,n),2*n,1);
+
+dxpdom = (coeff*ones(1,3)) .* dxd2dom;
+dxpdT = (coeff*ones(1,3)) .* dxd2dT;
+dxpdk = (coeff*ones(1,4)) .* dxd2dk;
+
+dxpdf = zeros(2*n,2);
+dxpdf(1:2:end,1) = xd2(1,:)';
+dxpdf(2:2:end,2) = xd2(2,:)';
+
+dxpdc = zeros(2*n,2);
+dxpdc(1:2:end,1) = ones(n,1);
+dxpdc(2:2:end,2) = ones(n,1);
+
+
+return;
+
+% Test of the Jacobians:
+
+n = 10;
+
+X = 10*randn(3,n);
+om = randn(3,1);
+T = [10*randn(2,1);40];
+f = 1000*rand(2,1);
+c = 1000*randn(2,1);
+k = 0.5*randn(4,1);
+
+
+[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k);
+
+
+% Test on om: NOT OK
+
+dom = 0.000000001 * norm(om)*randn(3,1);
+om2 = om + dom;
+
+[x2] = project_points(X,om2,T,f,c,k);
+
+x_pred = x + reshape(dxdom * dom,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on T: OK!!
+
+dT = 0.0001 * norm(T)*randn(3,1);
+T2 = T + dT;
+
+[x2] = project_points(X,om,T2,f,c,k);
+
+x_pred = x + reshape(dxdT * dT,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+
+% Test on f: OK!!
+
+df = 0.001 * norm(f)*randn(2,1);
+f2 = f + df;
+
+[x2] = project_points(X,om,T,f2,c,k);
+
+x_pred = x + reshape(dxdf * df,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on c: OK!!
+
+dc = 0.01 * norm(c)*randn(2,1);
+c2 = c + dc;
+
+[x2] = project_points(X,om,T,f,c2,k);
+
+x_pred = x + reshape(dxdc * dc,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
+
+% Test on k: OK!!
+
+dk = 0.001 * norm(4)*randn(4,1);
+k2 = k + dk;
+
+[x2] = project_points(X,om,T,f,c,k2);
+
+x_pred = x + reshape(dxdk * dk,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m
new file mode 100755
index 0000000..5bb1b91
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m
@@ -0,0 +1,312 @@
+function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)
+
+%project_points.m
+%
+%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
+%
+%Projects a 3D structure onto the image plane.
+%
+%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
+%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
+%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
+%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
+%       c: principal point location in pixel units (2x1 vector)
+%       k: Distortion coefficients (radial and tangential) (4x1 vector)
+%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
+%
+%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
+%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
+%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
+%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix)
+%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
+%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
+%
+%Definitions:
+%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
+%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
+%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
+%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
+%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
+%call r^2 = a^2 + b^2.
+%The distorted point coordinates are: xd = [xx;yy] where:
+%
+%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
+%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
+%
+%The left terms correspond to radial distortion, the right terms correspond to tangential distortion
+%
+%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
+%
+%xxp = f(1)*(xx + alpha*yy) + c(1)
+%yyp = f(2)*yy + c(2)
+%
+%
+%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
+%
+%
+%Important function called within that program:
+%
+%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
+%
+%rigid_motion.m: Computes the rigid motion transformation of a given structure
+
+
+if nargin < 7,
+   alpha = 0;
+   if nargin < 6,
+      k = zeros(4,1);
+      if nargin < 5,
+         c = zeros(2,1);
+         if nargin < 4,
+            f = ones(2,1);
+            if nargin < 3,
+               T = zeros(3,1);
+               if nargin < 2,
+                  om = zeros(3,1);
+                  if nargin < 1,
+                     error('Need at least a 3D structure to project (in project_points.m)');
+                     return;
+                  end;
+               end;
+            end;
+         end;
+      end;
+   end;
+end;
+
+
+[m,n] = size(X);
+
+[Y,dYdom,dYdT] = rigid_motion(X,om,T);
+
+
+inv_Z = 1./Y(3,:);
+
+x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;
+
+
+bb = (-x(1,:) .* inv_Z)'*ones(1,3);
+cc = (-x(2,:) .* inv_Z)'*ones(1,3);
+
+
+dxdom = zeros(2*n,3);
+dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
+dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);
+
+dxdT = zeros(2*n,3);
+dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
+dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
+
+
+% Add distortion:
+
+r2 = x(1,:).^2 + x(2,:).^2;
+
+
+
+dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
+dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
+
+
+r4 = r2.^2;
+
+dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
+dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;
+
+
+% Radial distortion:
+
+cdist = 1 + k(1) * r2 + k(2) * r4;
+
+dcdistdom = k(1) * dr2dom + k(2) * dr4dom;
+dcdistdT = k(1) * dr2dT+ k(2) * dr4dT;
+dcdistdk = [ r2' r4' zeros(n,2)];
+
+
+xd1 = x .* (ones(2,1)*cdist);
+
+dxd1dom = zeros(2*n,3);
+dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
+dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
+coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
+dxd1dom = dxd1dom + coeff.* dxdom;
+
+dxd1dT = zeros(2*n,3);
+dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
+dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
+dxd1dT = dxd1dT + coeff.* dxdT;
+
+dxd1dk = zeros(2*n,4);
+dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
+dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;
+
+
+
+% tangential distortion:
+
+a1 = 2.*x(1,:).*x(2,:);
+a2 = r2 + 2*x(1,:).^2;
+a3 = r2 + 2*x(2,:).^2;
+
+delta_x = [k(3)*a1 + k(4)*a2 ;
+   k(3) * a3 + k(4)*a1];
+
+
+ddelta_xdx = zeros(2*n,2*n);
+aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
+bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
+cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);
+
+ddelta_xdom = zeros(2*n,3);
+ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
+ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);
+
+ddelta_xdT = zeros(2*n,3);
+ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
+ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);
+
+ddelta_xdk = zeros(2*n,4);
+ddelta_xdk(1:2:end,3) = a1';
+ddelta_xdk(1:2:end,4) = a2';
+ddelta_xdk(2:2:end,3) = a3';
+ddelta_xdk(2:2:end,4) = a1';
+
+
+
+xd2 = xd1 + delta_x;
+
+dxd2dom = dxd1dom + ddelta_xdom ;
+dxd2dT = dxd1dT + ddelta_xdT;
+dxd2dk = dxd1dk + ddelta_xdk ;
+
+
+% Add Skew:
+
+xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];
+
+% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha
+
+dxd3dom = zeros(2*n,3);
+dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
+dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
+dxd3dT = zeros(2*n,3);
+dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
+dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
+dxd3dk = zeros(2*n,4);
+dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
+dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
+dxd3dalpha = zeros(2*n,1);
+dxd3dalpha(1:2:2*n,:) = xd2(2,:)';
+
+
+
+% Pixel coordinates:
+
+xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
+
+coeff = reshape(f*ones(1,n),2*n,1);
+
+dxpdom = (coeff*ones(1,3)) .* dxd3dom;
+dxpdT = (coeff*ones(1,3)) .* dxd3dT;
+dxpdk = (coeff*ones(1,4)) .* dxd3dk;
+dxpdalpha = (coeff) .* dxd3dalpha;
+
+dxpdf = zeros(2*n,2);
+dxpdf(1:2:end,1) = xd2(1,:)';
+dxpdf(2:2:end,2) = xd2(2,:)';
+
+dxpdc = zeros(2*n,2);
+dxpdc(1:2:end,1) = ones(n,1);
+dxpdc(2:2:end,2) = ones(n,1);
+
+
+return;
+
+% Test of the Jacobians:
+
+n = 10;
+
+X = 10*randn(3,n);
+om = randn(3,1);
+T = [10*randn(2,1);40];
+f = 1000*rand(2,1);
+c = 1000*randn(2,1);
+k = 0.5*randn(4,1);
+alpha = 0.01*randn(1,1);
+
+[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);
+
+
+% Test on om: NOT OK
+
+dom = 0.000000001 * norm(om)*randn(3,1);
+om2 = om + dom;
+
+[x2] = project_points2(X,om2,T,f,c,k,alpha);
+
+x_pred = x + reshape(dxdom * dom,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on T: OK!!
+
+dT = 0.0001 * norm(T)*randn(3,1);
+T2 = T + dT;
+
+[x2] = project_points2(X,om,T2,f,c,k,alpha);
+
+x_pred = x + reshape(dxdT * dT,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+
+% Test on f: OK!!
+
+df = 0.001 * norm(f)*randn(2,1);
+f2 = f + df;
+
+[x2] = project_points2(X,om,T,f2,c,k,alpha);
+
+x_pred = x + reshape(dxdf * df,2,n);
+
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on c: OK!!
+
+dc = 0.01 * norm(c)*randn(2,1);
+c2 = c + dc;
+
+[x2] = project_points2(X,om,T,f,c2,k,alpha);
+
+x_pred = x + reshape(dxdc * dc,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
+
+% Test on k: OK!!
+
+dk = 0.001 * norm(k)*randn(4,1);
+k2 = k + dk;
+
+[x2] = project_points2(X,om,T,f,c,k2,alpha);
+
+x_pred = x + reshape(dxdk * dk,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
+
+
+% Test on alpha: OK!!
+
+dalpha = 0.001 * norm(k)*randn(1,1);
+alpha2 = alpha + dalpha;
+
+[x2] = project_points2(X,om,T,f,c,k,alpha2);
+
+x_pred = x + reshape(dxdalpha * dalpha,2,n);
+
+norm(x2-x)/norm(x2 - x_pred)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m
new file mode 100755
index 0000000..561a7d0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m
@@ -0,0 +1,24 @@
+function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny);
+
+% new formalism using homographies
+
+a00 = [P1;1];
+a10 = [P2;1];
+a11 = [P3;1];
+a01 = [P4;1];
+
+% Compute the planart collineation:
+
+[H] = compute_collineation (a00, a10, a11, a01);
+
+
+% Build the grid using the planar collineation:
+
+x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1);
+y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1);
+
+pts = [x_l(:) y_l(:) ones(nx*ny,1)]';
+
+XX = H*pts;
+
+XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m
new file mode 100755
index 0000000..bb4ef86
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m
@@ -0,0 +1,258 @@
+%%% This code is an additional code that helps doing projector calibration in 3D scanning setup.
+%%% This is not a useful code for anyone else but me.
+%%% I included it in the toolbox for illustration only.
+
+
+load camera_results;
+
+
+proj_name = input('Basename projector calibration images (without number nor suffix): ','s');
+
+
+i = 1;
+
+while (i <= n_ima), % & (~no_image_file),
+   
+   if active_images(i),
+   
+        %fprintf(1,'Loading image %d...\n',i);
+   
+        if ~type_numbering,   
+        number_ext =  num2str(image_numbers(i));
+        else
+        number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i));
+        end;
+        
+      ima_namep = [proj_name  number_ext 'p.' format_image];
+      ima_namen = [proj_name  number_ext 'n.' format_image];
+      
+      if i == ind_active(1),
+         fprintf(1,'Loading image ');
+      end;
+         
+         fprintf(1,'%d...',i);
+         
+         if format_image(1) == 'p',
+            if format_image(2) == 'p',
+               Ip = double(loadppm(ima_namep));
+               In = double(loadppm(ima_namen));
+            else
+               Ip = double(loadpgm(ima_namep));
+               In = double(loadpgm(ima_namen));
+            end;
+         else
+            if format_image(1) == 'r',
+               Ip = readras(ima_namep);
+               In = readras(ima_namen);
+            else
+               Ip = double(imread(ima_namep));
+               In = double(imread(ima_namen));
+            end;
+         end;
+
+        
+                if size(Ip,3)>1,
+                Ip = Ip(:,:,2);
+                In = In(:,:,2);
+                end;
+   
+                eval(['Ip_' num2str(i) ' = Ip;']);
+                eval(['In_' num2str(i) ' = In;']);
+      
+   end;
+   
+   i = i+1;   
+   
+end;
+
+
+fprintf(1,'\nExtraction of the grid corners on the image\n');
+
+disp('Window size for corner finder (wintx and winty):');
+wintx = input('wintx ([] = 5) = ');
+if isempty(wintx), wintx = 5; end;
+wintx = round(wintx);
+winty = input('winty ([] = 5) = ');
+if isempty(winty), winty = 5; end;
+winty = round(winty);
+fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+
+
+disp('The projector you are using is the DLP or Intel');
+nx = 800;
+ny = 600;
+
+dX = input('Size dX in x of the squares (in pixels) [50] = ');
+dY = input('Size dY in y of the squares (in pixels) [50] = ');
+   
+if isempty(dX), dX=50; end;
+if isempty(dY), dY=50; end;
+   
+dXoff = input('Position in x of your reference (in pixels) [399.5] = ');
+dYoff = input('Position in y of your reference (in pixels) [299.5] = ');
+   
+if isempty(dXoff), dXoff=399.5; end;
+if isempty(dYoff), dYoff=299.5; end;
+
+end;
+
+
+
+for kk = ind_active,
+   
+   eval(['Ip = Ip_' num2str(kk) ';']);
+   eval(['In = In_' num2str(kk) ';']);
+   
+        [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(In,wintx,winty,fc,cc,kc,dX,dY);
+   xproj = x;
+   
+   Np_proj = size(x,2);
+   
+        figure(2);
+        image(Ip);
+        hold on;
+        plot(xproj(1,:)+1,xproj(2,:)+1,'r+');
+        title('Click on your reference point');
+        xlabel('Xc (in camera frame)');
+        ylabel('Yc (in camera frame)');
+        hold off;
+        
+        disp('Click on your reference point...');
+        
+        [xr,yr] = ginput2(1);
+        
+        err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2));
+        ind_ref = find(err == min(err));
+        
+        ref_pt = xproj(:,ind_ref);
+        
+        
+        figure(2);
+        hold on;
+        plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off;
+        
+        
+        nn = floor(ind_ref/(n_sq_x+1));
+        off_x = ind_ref - nn*(n_sq_x+1) - 1;
+        off_y = n_sq_y - nn;
+        
+        
+        xprojn = xproj - cc * ones(1,Np_proj);
+        xprojn = xprojn ./ (fc * ones(1,Np_proj));
+   xprojn = comp_distortion(xprojn,kc);
+   
+   eval(['Rc = Rc_' num2str(kk) ';']);
+   eval(['Tc = Tc_' num2str(kk) ';']);
+   
+        Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)])));
+        Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame
+        %Xproj = Rc'* Xcp - (Rc'*Tc)*ones(1,Np); % in the object frame !!! it works!
+        %Xproj(3,:) = zeros(1,Np);
+   
+   eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 
+   
+   x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj));
+   
+   eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the 
+   
+end;
+
+
+
+X_proj = [];
+x_proj = [];
+
+for kk = ind_active,
+   eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']);
+   eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']);
+end;
+
+
+%Save camera parameters:
+fc_save  = fc;
+cc_save = cc;
+kc_save = kc;
+
+omc_1_save = omc_1;
+Rc_1_save = Rc_1;
+Tc_1_save = Tc_1;
+
+
+% Get started to calibrate projector:
+clear fc cc kc
+
+n_ima = 1;
+X_1 = X_proj;
+x_1 = x_proj;
+
+
+% Image size: (may or may not be available)
+
+nx = 800;
+ny = 600;
+
+% No calibration image is available (only the corner coordinates)
+
+no_image = 1;
+
+% Set the toolbox not to prompt the user (choose default values)
+
+dont_ask = 1;
+
+% Do not estimate distortion:
+
+est_dist = [0;0;0;0];
+est_dist = ones(4,1);
+
+center_optim = 1;
+
+% Run the main calibration routine:
+
+go_calib_optim_iter;
+
+% Shows the extrinsic parameters:
+
+dX = 3;
+dY = 3;
+
+ext_calib;
+
+% Reprojection on the original images:
+
+reproject_calib;
+
+
+
+
+%----------------------- Retrieve results:
+
+% Intrinsic:
+
+% Projector:
+fp = fc;
+cp = cc;
+kp = kc;
+
+% Camera:
+fc = fc_save;
+cc = cc_save;
+kc = kc_save;
+
+% Extrinsic:
+
+% Relative position of projector and camera:
+T = Tc_1;
+om = omc_1;
+R = rodrigues(om);
+
+% Relative prosition of camera wrt world:
+omc = omc_1_save;
+Rc = Rc_1_save;
+Tc = Tc_1_save;
+
+% relative position of projector wrt world:
+Rp = R*Rc;
+omp = rodrigues(Rp);
+Tp = T + R*Tc;
+   
+eval(['save calib_cam_proj  R om T fc fp cc cp kc kp Rc Rp Tc Tp']);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m
new file mode 100755
index 0000000..fc1820b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m
@@ -0,0 +1,87 @@
+function [X, map] = readras(filename, ys, ye, xs, xe);
+%READRAS Read an image file in sun raster format.
+%        READRAS('imagefile.ras') reads a "sun.raster" image file.
+%        [X, map] = READRAS('imagefile.ras') returns both the image and a 
+%        color map, so that
+%               [X, map] = readras('imagefile.ras');
+%               image(X) 
+%               colormap(map)
+%               axis('equal')
+%        will display the result with the proper colors.
+%        NOTE: readras cannot deal with complicated color maps.  
+%              In fact, Matlab doesn't quite allow to work with colormaps
+%              with more than 64 entries.
+%
+
+%%
+%%       (C) Thomas K. Leung 3/30/93.
+%%       California Institute of Technology.
+%%       Modified by Andrea Mennucci to deal with color images
+%%
+
+% PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998
+
+dot = max(find(filename == '.'));
+suffix = filename(dot+1:dot+3);
+
+if(strcmp(suffix, 'ras'))                       % raster file format %
+        fp = fopen(filename, 'rb');
+        if(fp<0) error(['Cannot open ' filename '.']), end
+
+        %Read and crack the 32-byte header
+        fseek(fp, 4, -1);  
+
+        width   = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        height  = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        depth   = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        length  = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        type    = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        maplen  = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar');
+
+        maplen = maplen / 3;
+
+        if maptype == 2                                 % RMT_RAW
+                map = fread(fp, [maplen, 3], 'uchar')/255;
+%               if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end;
+        elseif maptype == 1                             % RMT_EQUAL_RGB
+                map(:,1) = fread(fp, [maplen], 'uchar');
+                map(:,2) = fread(fp, [maplen], 'uchar');
+                map(:,3) = fread(fp, [maplen], 'uchar');
+                %maxmap = max(max(map));
+                map = map/255;
+                if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end;
+        else                                            % RMT_NONE
+                map = [];
+        end
+%       if maplen>64,
+%            map=[map',zeros(3,256-maplen)]';
+%       end;
+
+        % Read the image
+
+        if rem(width,2) == 1
+                Xt = fread(fp, [width+1, height], 'uchar');
+                X = Xt(1:width, :)';
+        else
+                Xt = fread(fp, [width, height], 'uchar');
+                X = Xt';
+        end
+        X = X + 1;
+        fclose(fp);
+else
+        error('Image file name must end in either ''ras'' or ''rast''.');
+end
+
+
+if nargin == 5
+
+        X = X(ys:ye, xs:xe);
+
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m
new file mode 100755
index 0000000..0909c69
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m
@@ -0,0 +1,119 @@
+% Re-select te corners after calibration
+
+if ~exist('n_ima')|~exist('fc'),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+check_active_images;
+
+flag = 0;
+for kk = ind_active,
+   if ~exist(['y_' num2str(kk)]),
+      flag = 1;
+   else
+      eval(['ykk = y_' num2str(kk) ';']);
+      if isnan(ykk(1,1)),
+         flag = 1;
+      end;
+   end;
+end;
+
+if flag,
+   fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n');
+   return;
+end;
+
+
+if ~exist(['I_' num2str(ind_active(1))]),
+   n_ima_save = n_ima;
+   active_images_save = active_images;
+   ima_read_calib;
+   n_ima = n_ima_save;
+   active_images = active_images_save;
+   check_active_images;
+   if no_image_file,
+      disp('Cannot extract corners without images');
+      return;
+   end;
+end;
+
+fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n');
+
+disp('Window size for corner finder (wintx and winty):');
+wintx = input('wintx ([] = 5) = ');
+if isempty(wintx), wintx = 5; end;
+wintx = round(wintx);
+winty = input('winty ([] = 5) = ');
+if isempty(winty), winty = 5; end;
+winty = round(winty);
+
+fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1);
+
+ima_numbers = input('Number(s) of image(s) to process ([] = all images) = ');
+
+if isempty(ima_numbers),
+   ima_proc = 1:n_ima;
+else
+   ima_proc = ima_numbers;
+end;
+
+q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ');
+
+fprintf(1,'Processing image ');
+
+for kk = ima_proc;
+   
+   if active_images(kk),
+   
+        fprintf(1,'%d...',kk);
+   
+        if isempty(q_auto),
+           
+           eval(['I = I_' num2str(kk) ';']);
+           
+           eval(['y = y_' num2str(kk) ';']);
+           
+           xc = cornerfinder(y+1,I,winty,wintx); % the four corners
+           
+           eval(['wintx_' num2str(kk) ' = wintx;']);
+           eval(['winty_' num2str(kk) ' = winty;']);
+           
+           eval(['x_' num2str(kk) '= xc - 1;']);
+           
+        else
+           
+           fprintf(1,'\n');
+           
+           click_ima_calib;
+           
+        end;
+      
+   else
+      
+      if ~exist(['omc_' num2str(kk)]),
+         
+         eval(['dX_' num2str(kk) ' = NaN;']);
+         eval(['dY_' num2str(kk) ' = NaN;']);  
+         
+         eval(['wintx_' num2str(kk) ' = NaN;']);
+         eval(['winty_' num2str(kk) ' = NaN;']);
+         
+         eval(['x_' num2str(kk) ' = NaN*ones(2,1);']);
+         eval(['X_' num2str(kk) ' = NaN*ones(3,1);']);
+         
+         eval(['n_sq_x_' num2str(kk) ' = NaN;']);
+         eval(['n_sq_y_' num2str(kk) ' = NaN;']);
+         
+      end;
+      
+   end;
+   
+   
+end;
+
+% Recompute the error:
+
+comp_error_calib;
+
+fprintf(1,'\ndone\n');
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m
new file mode 100755
index 0000000..ccac7a5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m
@@ -0,0 +1,127 @@
+function [Irec] = rect(I,R,f,c,k,alpha,KK_new);
+
+
+if nargin < 5,
+   k = 0;
+   if nargin < 4,
+      c = [0;0];
+      if nargin < 3,
+         f = [1;1];
+         if nargin < 2,
+            R = eye(3);
+            if nargin < 1,
+               error('ERROR: Need an image to rectify');
+               break;
+            end;
+         end;
+      end;
+   end;
+end;
+
+
+if nargin < 7,
+   if nargin < 6,
+                KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1];
+   else
+        KK_new = alpha; % the 6th argument is actually KK_new   
+   end;
+   alpha = 0;
+end;
+
+
+
+% Note: R is the motion of the points in space
+% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame.
+
+
+if ~exist('KK_new'),
+   KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1];
+end;
+
+
+[nr,nc] = size(I);
+
+Irec = 255*ones(nr,nc);
+
+[mx,my] = meshgrid(1:nc, 1:nr);
+px = reshape(mx',nc*nr,1);
+py = reshape(my',nc*nr,1);
+
+rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))];
+
+
+% Rotation: (or affine transformation):
+
+rays2 = R'*rays;
+
+x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)];
+
+
+% Add distortion:
+
+k1 = k(1);
+k2 = k(2);
+
+p1 = k(3);
+p2 = k(4);
+
+r_2 = sum(x.^2);
+
+delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2) ;
+           p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];
+
+xd = (ones(2,1)*( 1 + k1 * r_2 + k2 * r_2.^2)) .* x + delta_x;
+
+
+% Reconvert in pixels:
+
+px2 = f(1)*(xd(1,:)+alpha_c*xd(2,:))+c(1);
+py2 = f(2)*xd(2,:)+c(2);
+
+
+% Interpolate between the closest pixels:
+
+px_0 = floor(px2);
+px_1 = px_0 + 1;
+alpha_x = px2 - px_0;
+
+py_0 = floor(py2);
+py_1 = py_0 + 1;
+alpha_y = py2 - py_0;
+
+good_points = find((px_0 >= 0) & (px_1 <= (nc-1)) & (py_0 >= 0) & (py_1 <= (nr-1)));
+
+I_lu = I(px_0(good_points) * nr + py_0(good_points) + 1);
+I_ru = I(px_1(good_points) * nr + py_0(good_points) + 1);
+I_ld = I(px_0(good_points) * nr + py_1(good_points) + 1);
+I_rd = I(px_1(good_points) * nr + py_1(good_points) + 1);
+
+
+I_interp = (1 - alpha_y(good_points)).*((1 - alpha_x(good_points)).* I_lu + alpha_x(good_points) .* I_ru) + alpha_y(good_points) .* ((1 - alpha_x(good_points)).* I_ld + alpha_x(good_points) .* I_rd);
+
+
+Irec((px(good_points)-1)*nr + py(good_points)) = I_interp;
+
+
+
+return;
+
+
+% Convert in indices:
+
+fact = 3;
+
+[XX,YY]= meshgrid(1:nc,1:nr);
+[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr);
+
+%tic;
+Iinterp = interp2(XX,YY,I,XXi,YYi); 
+%toc
+
+[nri,nci] = size(Iinterp);
+
+
+ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1;
+ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1;
+
+good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m
new file mode 100755
index 0000000..d3ad3d2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m
@@ -0,0 +1,121 @@
+%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%%
+
+if ~exist('n_ima')|~exist('fc'),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+if ~exist('no_image'),
+   no_image = 0;
+end;
+
+if ~exist('nx')&~exist('ny'),
+   fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n');
+   nx = 640;
+   ny = 480;
+end;
+
+
+check_active_images;
+
+
+% Color code for each image:
+
+colors = 'brgkcm';
+
+% Reproject the patterns on the images, and compute the pixel errors:
+
+% Reload the images if necessary
+
+if ~exist(['omc_' num2str(ind_active(1)) ]),
+   fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n');
+   return;
+end;
+
+if ~no_image,
+        if ~exist(['I_' num2str(ind_active(1)) ]'),
+           n_ima_save = n_ima;
+           active_images_save = active_images;
+           ima_read_calib;
+           n_ima = n_ima_save;
+           active_images = active_images_save;
+           check_active_images;
+        if no_image_file,
+           fprintf(1,'WARNING: Do not show the original images\n'); %return;
+        end;
+   end;
+else
+   no_image_file = 1;
+end;
+
+
+if ~exist('dont_ask'),
+   dont_ask = 0;
+end;
+
+
+if ~dont_ask,
+   ima_numbers = input('Number(s) of image(s) to show ([] = all images) = ');
+else
+   ima_numbers = [];
+end;
+
+
+if isempty(ima_numbers),
+   ima_proc = 1:n_ima;
+else
+   ima_proc = ima_numbers;
+end;
+
+
+figure(5);
+for kk = ima_proc, %1:n_ima,
+   if exist(['y_' num2str(kk)]),
+   if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']),
+           eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']);
+      hold on;
+   end;
+   end;
+end;
+hold off;
+axis('equal');
+title('Reprojection error (in pixel)');
+xlabel('x');
+ylabel('y');
+drawnow;
+
+set(5,'Name','error','NumberTitle','off');
+
+
+
+for kk = ima_proc,
+   if exist(['y_' num2str(kk)]),
+   if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']),
+   
+        if exist(['I_' num2str(kk)]),
+           eval(['I = I_' num2str(kk) ';']);
+        else
+           I = 255*ones(ny,nx);
+        end;
+   
+        figure(5+kk);
+        image(I); hold on;
+        colormap(gray(256));
+        title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']);
+        eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']);
+        eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']);
+      zoom on;
+      axis([1 nx 1 ny]);
+        hold off;
+        drawnow;
+
+      set(5+kk,'Name',num2str(kk),'NumberTitle','off');
+      
+   end;
+   end;
+end;
+
+
+err_std = std(ex')';
+
+fprintf(1,'Pixel error:      err = [%3.5f   %3.5f] (all active images)\n\n',err_std); 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m
new file mode 100755
index 0000000..473405c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m
@@ -0,0 +1,66 @@
+function [Y,dYdom,dYdT] = rigid_motion(X,om,T);
+
+%rigid_motion.m
+%
+%[Y,dYdom,dYdT] = rigid_motion(X,om,T)
+%
+%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om).
+%
+%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
+%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
+%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
+%
+%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points)
+%        dYdom: Derivative of Y with respect to om ((3N)x3 matrix)
+%        dYdT: Derivative of Y with respect to T ((3N)x3 matrix)
+%
+%Definitions:
+%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
+%The coordinate vector of P in the camera reference frame is: Y = R*X + T
+%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
+%
+%Important function called within that program:
+%
+%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
+
+
+
+if nargin < 3,
+   T = zeros(3,1);
+   if nargin < 2,
+      om = zeros(3,1);
+      if nargin < 1,
+         error('Need at least a 3D structure as input (in rigid_motion.m)');
+         return;
+      end;
+   end;
+end;
+
+
+[R,dRdom] = rodrigues(om);
+
+[m,n] = size(X);
+
+Y = R*X + T*ones(1,n);
+
+if nargout > 1,
+   
+
+dYdR = zeros(3*n,9);
+dYdT = zeros(3*n,3);
+
+dYdR(1:3:end,1:3:end) =  X';
+dYdR(2:3:end,2:3:end) =  X';
+dYdR(3:3:end,3:3:end) =  X';
+
+dYdT(1:3:end,1) =  ones(n,1);
+dYdT(2:3:end,2) =  ones(n,1);
+dYdT(3:3:end,3) =  ones(n,1);
+
+dYdom = dYdR * dRdom;
+
+end;
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m
new file mode 100755
index 0000000..9d55337
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m
@@ -0,0 +1,217 @@
+function        [out,dout]=rodrigues(in)
+
+% RODRIGUES     Transform rotation matrix into rotation vector and viceversa.
+%               
+%               Sintax:  [OUT]=RODRIGUES(IN)
+%               If IN is a 3x3 rotation matrix then OUT is the
+%               corresponding 3x1 rotation vector
+%               if IN is a rotation 3-vector then OUT is the 
+%               corresponding 3x3 rotation matrix
+%
+
+%%
+%%              Copyright (c) March 1993 -- Pietro Perona
+%%              California Institute of Technology
+%%
+
+%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
+%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!
+
+%% BUG when norm(om)=pi fixed -- April 6th, 1997;
+%% Jean-Yves Bouguet
+
+
+[m,n] = size(in);
+%bigeps = 10e+4*eps;
+bigeps = 10e+20*eps;
+
+if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
+   theta = norm(in);
+   if theta < eps
+      R = eye(3);
+      
+      %if nargout > 1,
+      
+      dRdin = [0 0 0;
+               0 0 1;
+               0 -1 0;
+               0 0 -1;
+               0 0 0;
+               1 0 0;
+               0 1 0;
+               -1 0 0;
+          0 0 0];
+       
+       %end;
+         
+   else
+      if n==length(in)  in=in'; end;    %% make it a column vec. if necess.
+         
+         %m3 = [in,theta]
+
+         dm3din = [eye(3);in'/theta];
+
+         omega = in/theta;
+         
+         %m2 = [omega;theta]
+         
+         dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];
+         
+         alpha = cos(theta);
+         beta = sin(theta);
+         gamma = 1-cos(theta);
+         omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
+         A = omega*omega';
+         
+         %m1 = [alpha;beta;gamma;omegav;A];
+         
+         dm1dm2 = zeros(21,4);
+         dm1dm2(1,4) = -sin(theta);
+         dm1dm2(2,4) = cos(theta);
+         dm1dm2(3,4) = sin(theta);
+         dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
+                             0 0 -1 0 0 0 1 0 0;
+                             0 1 0 -1 0 0 0 0 0]';
+                       
+         w1 = omega(1);
+         w2 = omega(2);
+         w3 = omega(3);
+         
+         dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
+         dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
+         dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];
+         
+         R = eye(3)*alpha + omegav*beta + A*gamma;
+         
+         dRdm1 = zeros(9,21);
+         
+         dRdm1([1 5 9],1) = ones(3,1);
+         dRdm1(:,2) = omegav(:);
+         dRdm1(:,4:12) = beta*eye(9);
+         dRdm1(:,3) = A(:);
+         dRdm1(:,13:21) = gamma*eye(9);
+         
+         dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;
+         
+         
+      end;
+      out = R;
+      dout = dRdin;
+      
+      %% it is prob. a rot matr.
+   elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
+            & (abs(det(in)-1) < bigeps))
+      R = in;
+      
+      
+      
+      tr = (trace(R)-1)/2;
+      dtrdR = [1 0 0 0 1 0 0 0 1]/2;
+      theta = real(acos(tr));
+      
+      
+      if sin(theta) >= 1e-5,
+         
+         dthetadtr = -1/sqrt(1-tr^2);
+         
+         dthetadR = dthetadtr * dtrdR;
+         % var1 = [vth;theta];
+         vth = 1/(2*sin(theta));
+         dvthdtheta = -vth*cos(theta)/sin(theta);
+         dvar1dtheta = [dvthdtheta;1];
+         
+         dvar1dR =  dvar1dtheta * dthetadR;
+         
+         
+         om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';
+         
+         dom1dR = [0 0 0 0 0 1 0 -1 0;
+               0 0 -1 0 0 0 1 0 0;
+               0 1 0 -1 0 0 0 0 0];
+         
+         % var = [om1;vth;theta];
+         dvardR = [dom1dR;dvar1dR];
+         
+         % var2 = [om;theta];
+         om = vth*om1;
+         domdvar = [vth*eye(3) om1 zeros(3,1)];
+         dthetadvar = [0 0 0 0 1];
+         dvar2dvar = [domdvar;dthetadvar];
+         
+         
+         out = om*theta;
+         domegadvar2 = [theta*eye(3) om];
+         
+         dout = domegadvar2 * dvar2dvar * dvardR;
+         
+         
+      else
+         if tr > 0;                     % case norm(om)=0;
+            
+            out = [0 0 0]';
+            
+            dout = [0 0 0 0 0 1/2 0 -1/2 0;
+                  0 0 -1/2 0 0 0 1/2 0 0;
+                  0 1/2 0 -1/2 0 0 0 0 0];
+         else                           % case norm(om)=pi; %% fixed April 6th
+            
+            
+            out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
+            %keyboard;
+            
+            if nargout > 1,
+               fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
+                        dout = NaN*ones(3,9);
+            end;
+         end; 
+      end;
+      
+   else
+      error('Neither a rotation matrix nor a rotation vector were provided');
+   end;
+
+return;
+
+%% test of the Jacobians:
+
+%%%% TEST OF dRdom:
+om = randn(3,1);
+dom = randn(3,1)/1000000;
+
+[R1,dR1] = rodrigues(om);
+R2 = rodrigues(om+dom);
+
+R2a = R1 + reshape(dR1 * dom,3,3);
+
+gain = norm(R2 - R1)/norm(R2 - R2a)
+
+%%% TEST OF dOmdR:
+om = randn(3,1);
+R = rodrigues(om);
+dom = randn(3,1)/10000;
+dR = rodrigues(om+dom) - R;
+
+[omc,domdR] = rodrigues(R);
+[om2] = rodrigues(R+dR);
+
+om_app = omc + domdR*dR(:);
+
+gain = norm(om2 - omc)/norm(om2 - om_app)
+
+
+%%% OTHER BUG: (FIXED NOW!!!)
+
+omu = randn(3,1);   
+omu = omu/norm(omu)
+om = pi*omu;        
+[R,dR]= rodrigues(om);
+[om2] = rodrigues(R);
+[om om2]
+
+%%% NORMAL OPERATION
+
+om = randn(3,1);         
+[R,dR]= rodrigues(om);
+[om2] = rodrigues(R);
+[om om2]
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m
new file mode 100755
index 0000000..87ee2fe
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m
@@ -0,0 +1,23 @@
+function [] = rotation(st);
+
+if nargin < 1,
+   st= 1;
+end;
+
+
+fig = gcf;
+
+ax = gca;
+
+vv = get(ax,'view');
+
+az = vv(1);
+el = vv(2);
+
+for azi = az:-abs(st):az-360,
+   
+   set(ax,'view',[azi el]);
+   figure(fig);
+   drawnow;
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m
new file mode 100755
index 0000000..095e17e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m
@@ -0,0 +1,65 @@
+%%% Program that launchs the complete 
+
+for N_ima_active = 1:30,
+   
+   error_analysis;
+   
+end;
+
+
+
+return;
+
+
+f = [];
+f_std = [];
+
+c = [];
+c_std = [];
+
+k = [];
+k_std = [];
+
+NN = 30;
+
+for rr = 1:NN,
+   
+   load(['Calib_Accuracies_' num2str(rr)]);
+   
+   [m1,s1] = mean_std_robust(fc_list(1,:));
+   [m2,s2] = mean_std_robust(fc_list(2,:));
+   
+   f = [f [m1;m2]];
+   f_std = [f_std [s1;s2]];
+   
+   [m1,s1] = mean_std_robust(cc_list(1,:));
+   [m2,s2] = mean_std_robust(cc_list(2,:));
+   
+   c = [c [m1;m2]];
+   c_std = [c_std [s1;s2]];
+      
+   [m1,s1] = mean_std_robust(kc_list(1,:));
+   [m2,s2] = mean_std_robust(kc_list(2,:));
+   [m3,s3] = mean_std_robust(kc_list(3,:));
+   [m4,s4] = mean_std_robust(kc_list(4,:));
+   
+   k = [k [m1;m2;m3;m4]];
+   k_std = [k_std [s1;s2;s3;s4]];
+   
+end;
+
+figure(1);
+errorbar([1:NN;1:NN]',f',f_std');
+figure(2);
+errorbar([1:NN;1:NN]',c',c_std');
+figure(3);
+errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std');
+
+figure(4);
+semilogy(f_std');
+
+figure(5);
+semilogy(c_std');
+
+figure(6);
+semilogy(k_std');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m
new file mode 100755
index 0000000..a176e39
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m
@@ -0,0 +1,46 @@
+%SAVEINR        Write an INRIMAGE format file
+%
+%       SAVEINR(filename, im)
+%
+%       Saves the specified image array in a INRIA image format file.
+%
+% SEE ALSO:     loadinr
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+% Peter Corke 1996
+
+function saveinr(fname, im)
+
+        fid = fopen(fname, 'w');
+        [r,c] = size(im');
+
+        % build the header
+        hdr = [];
+        s = sprintf('#INRIMAGE-4#{\n');
+        hdr = [hdr s];
+        s = sprintf('XDIM=%d\n',c);
+        hdr = [hdr s];
+        s = sprintf('YDIM=%d\n',r);
+        hdr = [hdr s];
+        s = sprintf('ZDIM=1\n');
+        hdr = [hdr s];
+        s = sprintf('VDIM=1\n');
+        hdr = [hdr s];
+        s = sprintf('TYPE=float\n');
+        hdr = [hdr s];
+        s = sprintf('PIXSIZE=32\n');
+        hdr = [hdr s];
+        s = sprintf('SCALE=2**0\n');
+        hdr = [hdr s];
+        s = sprintf('CPU=sun\n#');
+        hdr = [hdr s];
+
+        % make it 256 bytes long and write it
+        hdr256 = zeros(1,256);
+        hdr256(1:length(hdr)) = hdr;
+        fwrite(fid, hdr256, 'uchar');
+
+        % now the binary data
+        fwrite(fid, im', 'float32');
+        fclose(fid)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m
new file mode 100755
index 0000000..0cee75d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m
@@ -0,0 +1,22 @@
+%SAVEPGM        Write a PGM format file
+%
+%       SAVEPGM(filename, im)
+%
+%       Saves the specified image array in a binary (P5) format PGM image file.
+% 
+% SEE ALSO:     loadpgm
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+
+% Peter Corke 1994
+
+function savepgm(fname, im)
+
+        fid = fopen(fname, 'w');
+        [r,c] = size(im');
+        fprintf(fid, 'P5\n');
+        fprintf(fid, '%d %d\n', r, c);
+        fprintf(fid, '255\n');
+        fwrite(fid, im', 'uchar');
+        fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m
new file mode 100755
index 0000000..ece092b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m
@@ -0,0 +1,45 @@
+%SAVEPPM        Write a PPM format file
+%
+%       SAVEPPM(filename, I)
+%
+%       Saves the specified red, green and blue planes in a binary (P6)
+%       format PPM image file.
+%
+% SEE ALSO:     loadppm
+%
+%       Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab
+
+
+% Peter Corke 1994
+
+function saveppm(fname, I)
+
+I = double(I);
+
+if size(I,3) == 1,
+   R = I;
+   G = I;
+   B = I;
+else
+   R = I(:,:,1);
+   G = I(:,:,2);
+   B = I(:,:,3);
+end;
+
+%keyboard;
+
+        fid = fopen(fname, 'w');
+        [r,c] = size(R');
+        fprintf(fid, 'P6\n');
+        fprintf(fid, '%d %d\n', r, c);
+   fprintf(fid, '255\n');
+   R = R';
+   G = G';
+   B = B';
+        im = [R(:) G(:) B(:)];
+   %im = reshape(im,r,c*3);
+   im = im';
+   %im = im(:);
+   fwrite(fid, im, 'uchar');
+   fclose(fid);
+   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m
new file mode 100755
index 0000000..3f98a8b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m
@@ -0,0 +1,95 @@
+
+if ~exist('n_ima')|~exist('fc'),
+   fprintf(1,'No calibration data available.\n');
+   return;
+end;
+
+check_active_images;
+
+if ~exist('solution_init'), solution_init = []; end;
+
+for kk = 1:n_ima,
+   if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end;
+   if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end;
+end;
+
+if ~exist('param_list'),
+   param_list = solution;
+end;
+
+if ~exist('wintx'),
+   wintx = [];
+   winty = [];
+end;
+
+if ~exist('dX_default'),
+   dX_default = [];
+   dY_default = [];
+end;
+
+if ~exist('alpha_c'),
+   alpha_c = 0;
+end;
+
+for kk = 1:n_ima,
+   if ~exist(['y_' num2str(kk)]),
+        eval(['y_' num2str(kk) ' = NaN*ones(2,1);']);
+   end;
+   if ~exist(['n_sq_x_' num2str(kk)]),
+        eval(['n_sq_x_' num2str(kk) ' = NaN;']);
+        eval(['n_sq_y_' num2str(kk) ' = NaN;']);
+   end; 
+   if ~exist(['wintx_' num2str(kk)]),
+        eval(['wintx_' num2str(kk) ' = NaN;']);
+        eval(['winty_' num2str(kk) ' = NaN;']);
+   end; 
+end;
+
+save_name = 'Calib_Results';
+
+if exist([ save_name '.mat'])==2,
+   disp('WARNING: File Calib_Results.mat already exists');
+   pfn = -1;
+   cont = 1;
+   while cont,
+      pfn = pfn + 1;
+        postfix = ['_old'num2str(pfn)];
+      save_name = [ 'Calib_Results' postfix];
+      cont = (exist([ save_name '.mat'])==2);
+   end;
+   copyfile('Calib_Results.mat',[save_name '.mat']);
+        disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']);
+end;
+
+
+save_name = 'Calib_Results';
+
+if exist('calib_name'),
+   
+   fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']);
+
+   string_save = ['save ' save_name ' center_optim param_list active_images ind_active center_optim est_alpha est_dist fc kc cc alpha_c ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY'];
+   
+   for kk = 1:n_ima,
+        string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)];
+   end;
+   
+else
+   
+   fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']);
+
+   string_save = ['save ' save_name ' center_optim param_list active_images ind_active center_optim est_alpha est_dist fc kc cc alpha_c ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY'];
+   
+   for kk = 1:n_ima,
+        string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)];
+   end;
+   
+end;
+
+   
+
+%fprintf(1,'To load later click on Load\n');
+
+eval(string_save);
+
+fprintf(1,'done\n');
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m
new file mode 100755
index 0000000..c5e5430
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m
@@ -0,0 +1,39 @@
+
+      satis_distort = 0;
+      
+      disp(['Estimated focal: ' num2str(f_g) ' pixels']);
+      
+      while ~satis_distort,
+
+         k_g = input('Guess for distortion factor kc ([]=0): ');
+         
+         if isempty(k_g), k_g = 0; end;
+      
+         xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g);
+         
+         xu = xy_corners_undist(1,:)';
+         yu = xy_corners_undist(2,:)';
+         
+         [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid
+         
+         XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu;
+         XX(1,:) = f_g*XX(1,:)+c_g(1);
+         XX(2,:) = f_g*XX(2,:)+c_g(2);
+         
+         figure(2);
+         image(I);
+         colormap(map);
+         zoom on;
+         hold on;
+         %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro');
+         plot(XX(1,:),XX(2,:),'r+');
+         title('The red crosses should be on the grid corners...');
+         hold off;
+         
+         satis_distort = input('Satisfied with distortion? ([]=no, other=yes) ');
+         
+         satis_distort = ~isempty(satis_distort);
+         
+         
+      end;
+      
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m
new file mode 100755
index 0000000..aad0fa4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m
@@ -0,0 +1,9 @@
+% Main camera calibration toolbox:
+
+calib_gui;
+
+%calib_gui;
+
+path(pwd,path);
+
+format compact
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m
new file mode 100755
index 0000000..d9a7574
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m
@@ -0,0 +1,193 @@
+%%% INPUT THE IMAGE FILE NAME:
+
+if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'),
+   fprintf(1,'No intrinsic camera parameters available.\n');
+   return;
+end;
+
+KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1];
+
+disp('Program that undistorts images');
+disp('The intrinsic camera parameters are assumed to be known (previously computed)');
+
+fprintf(1,'\n');
+
+quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? ');
+
+if isempty(quest),
+   quest = 0;
+end;
+
+if ~quest,
+
+        if ~exist(['I_' num2str(ind_active(1))]),
+        ima_read_calib;
+   end;
+   
+   check_active_images;   
+   
+   format_image2 = format_image;
+   if format_image2(1) == 'j',
+      format_image2 = 'bmp';
+   end;
+   
+   for kk = 1:n_ima,
+      
+      if exist(['I_' num2str(kk)]),
+      
+        eval(['I = I_' num2str(kk) ';']);
+        [I2] = rect(I,eye(3),fc,cc,kc,KK);
+        
+                if ~type_numbering,   
+                number_ext =  num2str(image_numbers(kk));
+                else
+                number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk));
+                end;
+                
+        ima_name2 = [calib_name '_rect' number_ext '.' format_image2];
+        
+        fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']);
+         
+         
+         if format_image2(1) == 'p',
+            if format_images2(2) == 'p',
+               saveppm(ima_name2,uint8(round(I2)));
+            else
+               savepgm(ima_name2,uint8(round(I2)));
+            end;
+         else
+            if format_image2(1) == 'r',
+               writeras(ima_name2,round(I2),gray(256));
+            else
+               imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2);
+            end;
+         end;
+         
+         
+      end;
+      
+   end;
+   
+   fprintf(1,'done\n');
+
+else
+   
+   dir;
+   fprintf(1,'\n');
+
+        image_name = input('Image name (full name without extension): ','s');
+
+        format_image2 = '0';
+
+while format_image2 == '0',
+   
+   format_image2 =  input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s');
+        
+        if isempty(format_image2),
+        format_image2 = 'ras';
+        end;
+   
+   if lower(format_image2(1)) == 'm',
+      format_image2 = 'ppm';
+   else
+      if lower(format_image2(1)) == 'b',
+         format_image2 = 'bmp';
+      else
+         if lower(format_image2(1)) == 't',
+            format_image2 = 'tif';
+         else
+            if lower(format_image2(1)) == 'p',
+               format_image2 = 'pgm';
+            else
+               if lower(format_image2(1)) == 'j',
+                  format_image2 = 'jpg';
+               else
+                  if lower(format_image2(1)) == 'r',
+                     format_image2 = 'ras';
+                  else  
+                     disp('Invalid image format');
+                     format_image2 = '0'; % Ask for format once again
+                  end;
+               end;
+            end;
+         end;
+      end;
+   end;
+end;
+
+ima_name = [image_name '.' format_image2];
+
+
+%%% READ IN IMAGE:
+
+if format_image2(1) == 'p',
+   if format_image2(2) == 'p',
+      I = double(loadppm(ima_name));
+   else
+      I = double(loadpgm(ima_name));
+   end;
+else
+   if format_image2(1) == 'r',
+      I = readras(ima_name);
+   else
+      I = double(imread(ima_name));
+   end;
+end;
+
+if size(I,3)>1,
+   I = I(:,:,2);
+end;
+
+
+if (size(I,1)>ny)|(size(I,2)>nx),
+   I = I(1:ny,1:nx);
+end;
+
+
+        %% SHOW THE ORIGINAL IMAGE:
+        
+        figure(2);
+        image(I);
+        colormap(gray(256));
+        title('Original image (with distortion) - Stored in array I');
+        drawnow;
+        
+        
+        %% UNDISTORT THE IMAGE:
+        
+        fprintf(1,'Computing the undistorted image...')
+        
+   [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK);
+   
+   fprintf(1,'done\n');
+        
+        figure(3);
+        image(I2);
+        colormap(gray(256));
+        title('Undistorted image - Stored in array I2');
+   drawnow;
+   
+   
+   %% SAVE THE IMAGE IN FILE:
+   
+   ima_name2 = [image_name '_rect.' format_image2];
+   
+   fprintf(1,['Saving undistorted image under ' ima_name2 '...']);
+   
+   if format_image2(1) == 'p',
+      if format_images2(2) == 'p',
+              saveppm(ima_name2,uint8(round(I2)));
+      else
+              savepgm(ima_name2,uint8(round(I2)));
+      end;
+   else
+      if format_image2(1) == 'r',
+         writeras(ima_name2,round(I2),gray(256));
+      else
+         imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2);
+      end;
+   end;
+   
+   fprintf(1,'done\n');
+      
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m
new file mode 100755
index 0000000..8946349
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m
@@ -0,0 +1,89 @@
+function [fc,cc,kc,Rc,Tc,omc,nx,ny] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2);
+
+%Conversion from Reg Willson's calibration format to my format
+
+% Conversion:
+
+% Focal length:
+fc = [sx/dpx ; 1/dpy]*f;
+
+% Principal point;
+cc = [Cx;Cy];
+
+% Extrinsic parameters:
+Rx = rodrigues([Rx;0;0]);
+Ry = rodrigues([0;Ry;0]);
+Rz = rodrigues([0;0;Rz]);
+
+Rc = Rz * Ry * Rx;
+
+omc = rodrigues(Rc);
+
+Tc = [Tx;Ty;Tz];
+
+
+% More tricky: Take care of the distorsion:
+
+Nfy = round(Nfx * 3/4);
+
+nx = Nfx;
+ny = Nfy;
+
+% Select a set of DISTORTED coordinates uniformely distributed across the image:
+
+[xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy);
+
+xp_dist = xp_dist(:)';
+yp_dist = yp_dist(:)';
+
+
+% Apply UNDISTORTION according to Willson:
+
+xp_sensor_dist = dpx*(xp_dist - Cx)/sx;
+yp_sensor_dist = dpy*(yp_dist - Cy);
+
+dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2);
+
+xp_sensor = xp_sensor_dist .* dist_fact;
+yp_sensor = yp_sensor_dist .* dist_fact;
+
+xp = xp_sensor * sx / dpx  + Cx;
+yp = yp_sensor / dpy  + Cy;
+
+ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1));
+
+xp = xp(ind);
+yp = yp(ind);
+xp_dist = xp_dist(ind);
+yp_dist = yp_dist(ind);
+
+
+% Now, find my own set of parameters:
+
+x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)];
+x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)];
+
+k = [0;0;0;0];
+
+for kk = 1:5,
+        
+        [xd,dxddk] = apply_distortion(x,k);
+
+        err = x_dist - xd;
+
+        %norm(err)
+   
+   k_step = inv(dxddk'*dxddk)*(dxddk')*err(:);
+   
+   k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:);
+   
+   %norm(k_step)/norm(k)
+   
+   if norm(k_step)/norm(k) < 10e-10,
+      break;
+   end;
+   
+end;
+
+
+kc = k;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m
new file mode 100755
index 0000000..bbde63c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m
@@ -0,0 +1,59 @@
+% Read in Reg Willson's data file, and convert it into my data format:
+
+%dir;
+
+%calib_file = input('Reg Willson calibration file name: ','s');
+
+if exist(calib_file),
+
+        
+        load(calib_file);
+        
+        inddot = find(calib_file == '.');
+        
+   if isempty(inddot),
+      varname = calib_file;
+   else
+      varname = calib_file(1:inddot(1)-1);      
+        end;
+        
+        eval(['calib_params = ' varname ';'])
+        
+        Ncx = calib_params(1);
+   Nfx = calib_params(2);
+   dx = calib_params(3);
+   dy = calib_params(4);
+   dpx = calib_params(5);
+   dpy = calib_params(6);
+   Cx = calib_params(7);
+   Cy = calib_params(8);
+   sx = calib_params(9);
+   f = calib_params(10);
+   kappa1 = calib_params(11);
+   Tx = calib_params(12);
+   Ty = calib_params(13);
+   Tz = calib_params(14);
+   Rx = calib_params(15);
+   Ry = calib_params(16);
+   Rz = calib_params(17);
+   p1 = calib_params(18);
+   p2 = calib_params(19);
+   
+   % Conversion:
+   [fc,cc,kc,Rc_1,Tc_1,omc_1,nx,ny] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2);
+   
+   KK = [fc(1) 0 cc(1);0 fc(2) cc(2) ; 0 0 1];
+   
+   n_ima = 1;
+   
+   X_1 = [NaN;NaN;NaN];
+   x_1 = [NaN;NaN];
+   
+   map = gray(256);
+   
+else
+   
+   disp(['WARNING: Calibration file ' calib_file ' not found']);
+   
+end;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m
new file mode 100755
index 0000000..c7eb7bc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m
@@ -0,0 +1,105 @@
+function writeras(filename, image, map);
+%WRITERAS Write an image file in sun raster format.
+%         WRITERAS('imagefile.ras', image_matrix, map) writes a 
+%         "sun.raster" image file.
+
+%         Written by Thomas K. Leung 3/30/93.
+%         @ California Institute of Technology.
+
+
+% PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998
+
+dot = max(find(filename == '.'));
+suffix = filename(dot+1:dot+3);
+
+if nargin < 3,
+   map = [];
+end;
+
+if(strcmp(suffix, 'ras'))
+        %Write header
+
+        fp = fopen(filename, 'wb');
+        if(fp < 0) error(['Cannot open ' filename '.']), end
+
+        [height, width] = size(image);
+        image = image - 1;
+        mapsize = size(map, 1)*size(map,2);
+        %fwrite(fp, ...
+        %       [1504078485, width, height, 8, height*width, 1, 1, mapsize], ...
+   %       'long');
+   
+   
+   zero_str = '00000000';
+   
+   % MAGIC NUMBER:
+   
+
+        fwrite(fp,89,'uchar');
+   fwrite(fp,166,'uchar');
+   fwrite(fp,106,'uchar');
+   fwrite(fp,149,'uchar');
+
+   width_str = dec2hex(width);
+        width_str = [zero_str(1:8-length(width_str)) width_str];
+   
+   for ii = 1:2:7,
+        fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar');
+        end;
+   
+   
+   height_str = dec2hex(height);
+        height_str = [zero_str(1:8-length(height_str)) height_str];
+   
+   for ii = 1:2:7,
+        fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar');
+   end;
+   
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,8,'uchar');
+   
+   ll = height*width;
+   ll_str = dec2hex(ll);
+   ll_str = [zero_str(1:8-length(ll_str)) ll_str];
+   
+   for ii = 1:2:7,
+        fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar');
+        end;
+  
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,1,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,0,'uchar');
+   fwrite(fp,~~mapsize,'uchar');
+
+   mapsize_str = dec2hex(mapsize);
+   mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str];
+   
+   %keyboard;
+   
+   for ii = 1:2:7,
+        fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar');
+        end;
+ 
+   
+        if mapsize ~= 0
+                map = min(255, fix(255*map));
+                fwrite(fp, map, 'uchar');
+        end
+        if rem(width,2) == 1
+                image = [image'; zeros(1, height)]';
+                top = 255 * ones(size(image));
+                fwrite(fp, min(top,image)', 'uchar');
+        else
+                top = 255 * ones(size(image));
+                fwrite(fp, min(top,image)', 'uchar');
+        end
+        fclose(fp);
+else
+        error('Image file name must end in either ''ras'' or ''rast''.');
+end