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.

525 files changed, 31550 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
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/README b/SD-VBS/common/toolbox/toolbox_basic/affine/README
new file mode 100755
index 0000000..e578a74
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/README
@@ -0,0 +1,5 @@
+Top level program is "compute_AD.m".  Use "compute_AD_disp.m" if one
+wants to display results as program runs.
+
+The testing programs are called "simulation.m" for synthetic images,
+and "test_affine.m" for real images.
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/carve_it.m b/SD-VBS/common/toolbox/toolbox_basic/affine/carve_it.m
new file mode 100755
index 0000000..1a44f89
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/carve_it.m
@@ -0,0 +1,25 @@
+function img = carve_it(I,center,window_size_h)
+
+[size_y,size_x]= size(I);
+min_x = round(center(1)-window_size_h(1));
+max_x = round(center(1)+window_size_h(1));
+min_y = round(center(2)-window_size_h(2));
+max_y = round(center(2)+window_size_h(2));
+window_size = window_size_h*2 +1;
+
+if (min_x <1)|(max_x > size_x)|(min_y<1)|(max_y>size_y),
+  disp('window too big');
+  center
+  window_size_h
+  img = zeros(window_size(2),window_size(1));
+  n_min_x = max(1,round(min_x));
+  n_min_y = max(1,round(min_y));
+  n_max_x = min(size_x,round(max_x));
+  n_max_y = min(size_y,round(max_y));
+  img(1+(n_min_y-min_y):window_size(2)-(max_y-n_max_y),1+(n_min_x-min_x):window_size(1)-(max_x-n_max_x))=I(n_min_y:n_max_y,n_min_x:n_max_x);
+else
+  img = I(center(2)-window_size_h(2):center(2)+window_size_h(2),...
+        center(1)-window_size_h(1):center(1)+window_size_h(1));
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD.m b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD.m
new file mode 100755
index 0000000..a39acd6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD.m
@@ -0,0 +1,90 @@
+function [A,D,mask] =...
+compute_AD(img_i,img_j,center_i,center_j,window_size_h,num_iter,w,num_trans,Dest,mask)
+%
+% function [A,D,mask] = ...
+%     compute_AD(img_i,img_j,center_i,center_j,window_size_h,num_iter,w,
+%                mask,num_trans)
+%
+%  A:    Affine motion;
+%  D:    Displacement;
+%  
+%  img_i, img_j: the two image(in full size);
+%  center_i, center_j:  the centers of the feature in two images;
+%  window_size_h: half size of the feature window;
+%  num_iter:    number of iterations;
+%  w:           parameter used in "grad.m" for computing gaussians used for
+%               gradient estimation;
+%
+%  num_trans:   OPTIONAL, number of translation iteration; default = 3;
+%  mask:        OPTIONAL, if some area of the square shaped feature window should
+%                         be weighted less;
+%
+
+%
+%    Jianbo Shi
+%
+
+if ~exist('Dest'),
+  Dest = [0,0];
+end
+
+if ~exist('mask'),
+   mask = ones(2*window_size_h+1)';
+end
+
+% set the default num_trans
+if ~exist('num_trans'),
+   num_trans= 3;
+end
+
+% normalize image intensity to the range of 0.0-1.0
+img_i = norm_inten(img_i);
+img_j = norm_inten(img_j);
+
+window_size = 2*window_size_h + 1;
+I = carve_it(img_i,center_i,window_size_h);
+J = carve_it(img_j,center_j,window_size_h);
+
+% init. step
+J_computed = I;
+D_computed = Dest;
+A_computed = eye(2);
+J_computed = compute_J(A_computed,D_computed,img_i,center_i,window_size_h);
+
+%% level of noise
+sig = 0.1;
+
+records = zeros(num_iter,6);
+errs = zeros(1,num_iter);
+
+k = 1;
+% iteration
+while k <= num_iter,
+  [A,D] = iter_AD(J_computed,J,mask,w,k,num_trans);
+
+  A_computed = A*A_computed;
+  D_computed = (A*D_computed')' + D;
+
+  % compute the warped image
+  J_computed = compute_J(A_computed,D_computed,img_i,center_i,window_size_h);
+
+  % compute the SSD error
+  errs(k) = sqrt(sum(sum((mask.*(J_computed-J)).^2)))/prod(size(J));
+
+  % update the mask, discounting possible occlusion region
+  if (k>num_trans),
+    mask = exp(-abs(J_computed-J)/sig);
+  end
+
+  % record the A and D
+  records(k,:) = [reshape(A_computed,1,4),reshape(D_computed,1,2)];
+
+  k = k+1;
+end  
+
+[tmp,id] = min(errs);
+A = reshape(records(id,1:4),2,2);
+D = reshape(records(id,5:6),1,2);
+
+J_computed = compute_J(A,D,img_i,center_i,window_size_h);
+mask = exp(-abs(J_computed-J)/sig);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD_disp.m b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD_disp.m
new file mode 100755
index 0000000..f2e6c62
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_AD_disp.m
@@ -0,0 +1,103 @@
+function [A,D,mask] =...
+compute_AD_disp(img_i,img_j,center_i,center_j,window_size_h,num_iter,w,fig_disp,num_trans,Dest,mask)
+%
+% function [A,D,mask] = ...
+%     compute_AD_disp(img_i,img_j,center_i,center_j,window_size_h,num_iter,w,
+%                fig_disp,mask,num_trans)
+%  
+%  Computing affine transform for matching to image patches.  Display results
+%  as program runs.
+%
+%  A: Affine motion;
+%  D: Displacement;
+%  
+%
+%  img_i, img_j: the two image(in full size);
+%  center_i, center_j:  the centers of the feature in two images;
+%  window_size_h: half size of the feature window;
+%  num_iter:    number of iterations;
+%  w:           parameter used in "grad.m" for computing gaussians used for
+%               gradient estimation;
+%  fig_disp:    figure for display;
+%
+%  num_trans:   OPTIONAL, number of translation iteration;
+%  mask:        OPTIONAL, if some area of the square shaped feature window should
+%                         be weighted less;
+%
+
+
+%
+%    Jianbo Shi
+%
+
+if ~exist('mask'),
+   mask = ones(2*window_size_h+1)';
+end
+
+if ~exist('Dest'),
+  Dest = [0,0];
+end
+
+% set the default num_trans
+if ~exist('num_trans'),
+   num_trans= 3;
+end
+
+% normalize image intensity to the range of 0.0-1.0
+img_i = norm_inten(img_i);
+img_j = norm_inten(img_j);
+
+window_size = 2*window_size_h + 1;
+I = carve_it(img_i,center_i,window_size_h);
+J = carve_it(img_j,center_j,window_size_h);
+
+% init. step
+D_computed = Dest;
+A_computed = eye(2);
+J_computed = compute_J(A_computed,D_computed,img_i,center_i,window_size_h);
+
+
+
+figure(fig_disp);subplot(1,3,1);imagesc(I);colormap(gray);axis('image');
+subplot(1,3,3);imagesc(J);axis('image');
+drawnow;
+
+sig = 0.1;
+
+records = zeros(num_iter,6);
+errs = zeros(1,num_iter);
+
+k = 1;
+% iteration
+while k <= num_iter,
+  [A,D] = iter_AD(J_computed,J,mask,w,k,num_trans);
+
+  A_computed = A*A_computed;
+  D_computed = (A*D_computed')' + D;
+
+  % compute the warped image
+  J_computed = compute_J(A_computed,D_computed,img_i,center_i,window_size_h);
+
+  % compute the SSD error
+  errs(k) = sqrt(sum(sum((mask.*(J_computed-J)).^2)))/prod(size(J))
+
+  % update the mask, discounting possible occlusion region
+  if (k>num_trans+1),
+    mask = exp(-abs(J_computed-J)/sig);
+  end
+
+  % record the A and D
+  records(k,:) = [reshape(A_computed,1,4),reshape(D_computed,1,2)];
+
+  figure(fig_disp);subplot(1,3,2);imagesc(J_computed);axis('image');
+  title(sprintf('iter:%d: dx=%3.3f, dy = %3.3f',k,D_computed(1),D_computed(2)));drawnow; 
+
+  k = k+1;
+end  
+
+[tmp,id] = min(errs);
+A = reshape(records(id,1:4),2,2);
+D = reshape(records(id,5:6),1,2);
+
+J_computed = compute_J(A,D,img_i,center_i,window_size_h);
+mask = exp(-abs(J_computed-J)/sig);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/compute_J.m b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_J.m
new file mode 100755
index 0000000..80db273
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/compute_J.m
@@ -0,0 +1,31 @@
+function [JJ,mask] = compute_J(A,D,I,center,window_size_h)
+%%  function J = compute_J(A,D,I,center,window_size_h)
+%
+
+[size_y,size_x] = size(I);
+
+center_x = center(1);
+center_y = center(2);
+
+[XX,YY] = meshgrid(1:size_x,1:size_y);
+x = reshape(XX,size_x*size_y,1);
+y = reshape(YY,size_x*size_y,1);
+index(:,1) = x-center_x;
+index(:,2) = y-center_y;
+
+position_new = A*index'+ [D(1),0;0,D(2)]*ones(2,size_x*size_y);
+position_new(1,:) = position_new(1,:)+center_x;
+position_new(2,:) = position_new(2,:)+center_y;
+
+position_new_x = reshape(position_new(1,:),size_y,size_x);
+position_new_y = reshape(position_new(2,:),size_y,size_x);
+
+[J,mask]= m_interp4(I,position_new_x,position_new_y);
+
+JJ = J(center(2)-window_size_h(2):center(2)+window_size_h(2),...
+       center(1)-window_size_h(1):center(1)+window_size_h(1));
+mask = mask(center(2)-window_size_h(2):center(2)+window_size_h(2),...
+       center(1)-window_size_h(1):center(1)+window_size_h(1));
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/find_AD.m b/SD-VBS/common/toolbox/toolbox_basic/affine/find_AD.m
new file mode 100755
index 0000000..3cccefb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/find_AD.m
@@ -0,0 +1,82 @@
+function [A,D] = find_AD(I,J,mask,w)
+%
+%  [A,D] = find_AD(I,J,mask,w)
+%
+%    find the matrix affine transform A and displacement D,
+%    such that SSD difference of  I(Ax-d)-J(x) is minimized,
+%   
+
+%
+% Jianbo Shi
+%
+
+
+[gy1,gx1] = grad(I,w);
+[gy2,gx2] = grad(J,w);
+
+gx = 0.5*(gx1+gx2);
+gy = 0.5*(gy1+gy2);
+
+[size_y,size_x] = size(I);
+[center_y,center_x] = find_center(size_y,size_x);
+mask = mask(w+1:size_y-w,w+1:size_x-w);
+
+[x,y] = meshgrid(1:size_x,1:size_y);
+x = x- center_x;
+y = y-center_y;
+
+x = x(w+1:size_y-w,w+1:size_x-w);
+y = y(w+1:size_y-w,w+1:size_x-w);
+
+gx_sqr = gx.*mask.*gx;
+gx_gy = gx.*mask.*gy;
+gy_sqr = gy.*mask.*gy;
+
+x_sqr = x.*x;
+x_y = x.*y;
+y_sqr = y.*y;
+
+T= zeros(6,6);
+T(1,1) = 0.5*trapz(trapz(gx_sqr.*x_sqr));
+T(2,1) = trapz(trapz(gx_gy.*x_y));
+T(3,1) = trapz(trapz(gx_sqr.*x_y));
+T(4,1) = trapz(trapz(gx_gy.*x_sqr));
+T(5,1) = trapz(trapz(gx_sqr.*x));
+T(6,1) = trapz(trapz(gx_gy.*x));
+T(2,2) = 0.5*trapz(trapz(gy_sqr.*y_sqr));
+T(3,2) = trapz(trapz(gx_gy.*y_sqr));
+T(4,2) = trapz(trapz(gy_sqr.*x_y));
+T(5,2) = trapz(trapz(gx_gy.*y));
+T(6,2) = trapz(trapz(gy_sqr.*y));
+T(3,3) = 0.5*trapz(trapz(gx_sqr.*y_sqr));
+T(4,3) = trapz(trapz(gx_gy.*x_y));
+T(5,3) = trapz(trapz(gx_sqr.*y));
+T(6,3) = trapz(trapz(gx_gy.*y));
+T(4,4) = 0.5*trapz(trapz(gy_sqr.*x_sqr));
+T(5,4) = trapz(trapz(gx_gy.*x));
+T(6,4) = trapz(trapz(gy_sqr.*x));
+T(5,5) = 0.5*trapz(trapz(gx_sqr));
+T(6,5) = trapz(trapz(gx_gy));
+T(6,6) = 0.5*trapz(trapz(gy_sqr));
+
+T = T+T';
+
+J = J(w+1:size_y-w,w+1:size_x-w);
+I = I(w+1:size_y-w,w+1:size_x-w);
+
+
+diff = (J-I).*mask;
+b(1) = trapz(trapz(diff.*gx.*x));
+b(2) = trapz(trapz(diff.*gy.*y));
+b(3) = trapz(trapz(diff.*gx.*y));
+b(4) = trapz(trapz(diff.*gy.*x));
+b(5) = trapz(trapz(diff.*gx));
+b(6) = trapz(trapz(diff.*gy));
+
+a = inv(T)*b';
+
+A = [1+a(1), a(3);
+a(4),1+a(2)];
+
+D= [a(5),a(6)];
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/find_D.m b/SD-VBS/common/toolbox/toolbox_basic/affine/find_D.m
new file mode 100755
index 0000000..1e42cb2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/find_D.m
@@ -0,0 +1,65 @@
+function D = find_D(I,J,mask,w)
+%
+%  function D = find_D(I,J,mask,w)
+%
+%  find the vector D such that it minimizes then
+%  difference between I(Ax-d)-J(x).
+%
+%  mask: the weight matrix,
+%  w: window size for estimating gradiant, use a large value
+%     when A,D are large.
+%
+
+%
+%  NOTE:  Because gradient values on the boarder regions of
+%         I and J can not be computed accuately when using
+%         a gaussian of large support, those boarder regions
+%         of width w are not used in computing D.
+%
+
+%
+%  Jianbo Shi
+%
+
+[gy1,gx1] = grad(I,w);
+[gy2,gx2] = grad(J,w);
+
+gx = 0.5*(gx1+gx2);
+gy = 0.5*(gy1+gy2);
+
+[size_y,size_x] = size(I);
+[center_y,center_x] = find_center(size_y,size_x);
+mask = mask(w+1:size_y-w,w+1:size_x-w);
+
+[x,y] = meshgrid(1:size_x,1:size_y);
+x = x- center_x;
+y = y-center_y;
+
+x = x(w+1:size_y-w,w+1:size_x-w);
+y = y(w+1:size_y-w,w+1:size_x-w);
+
+gx_sqr = gx.*mask.*gx;
+gx_gy = gx.*mask.*gy;
+gy_sqr = gy.*mask.*gy;
+
+
+T= zeros(2,2);
+
+T(1,1) = 0.5*trapz(trapz(gx_sqr));
+T(2,1) = trapz(trapz(gx_gy));
+T(2,2) = 0.5*trapz(trapz(gy_sqr));
+
+T = T+T';
+
+J = J(w+1:size_y-w,w+1:size_x-w);
+I = I(w+1:size_y-w,w+1:size_x-w);
+
+
+diff = (J-I).*mask;
+b(1) = trapz(trapz(diff.*gx));
+b(2) = trapz(trapz(diff.*gy));
+
+a = inv(T)*b';
+
+D= [a(1),a(2)];
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/find_center.m b/SD-VBS/common/toolbox/toolbox_basic/affine/find_center.m
new file mode 100755
index 0000000..b12ac7b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/find_center.m
@@ -0,0 +1,4 @@
+function [center_x,center_y] = find_center(size_x,size_y)
+
+center_x = 0.5*(size_x +1);
+center_y = 0.5*(size_y +1);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/gen_feature_s.m b/SD-VBS/common/toolbox/toolbox_basic/affine/gen_feature_s.m
new file mode 100755
index 0000000..3c113e9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/gen_feature_s.m
@@ -0,0 +1,17 @@
+function I = gen_feature_s(size_of_feature)
+% function I = gen_feature(size_of_feature)
+%  generates a spherical features with size
+% of "size_of_feature"
+%
+
+ss = round(0.4*size_of_feature);
+[X,Y,II] = hemisphere_s(ss);
+
+II = abs(II);
+II = 1/max(max(II))*II;
+
+I = zeros(size_of_feature,size_of_feature);
+
+t = round((size_of_feature-ss)/2);
+
+I(1+t:1+t+ss,1+t:1+t+ss) = II;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/grad.m b/SD-VBS/common/toolbox/toolbox_basic/affine/grad.m
new file mode 100755
index 0000000..53bab55
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/grad.m
@@ -0,0 +1,24 @@
+% gradient of an image
+% coordinates (r, c) follow matrix convention;
+% the gaussian is truncated at x = +- tail, and there are samples samples
+% inbetween, where samples = hsamples * 2 + 1
+
+function[gr,gc] = gradient(image, hsamples)
+
+tail=4;
+samples = hsamples * 2 + 1;
+
+x = linspace(-tail, tail, samples);
+gauss = exp(-x.^2);
+n = gauss * ones(samples,1);
+gauss = gauss/n;
+
+gaussderiv = -x.*gauss;
+n = -gaussderiv*linspace(1,samples,samples)';
+gaussderiv = gaussderiv/n;
+
+gr = conv2(conv2(image, gaussderiv','valid'), gauss,'valid');
+gc = conv2(conv2(image, gaussderiv,'valid'), gauss','valid');
+
+%gr = conv2(conv2(image, gaussderiv','same'), gauss,'same');
+%gc = conv2(conv2(image, gaussderiv,'same'), gauss','same');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/hemisphere_s.m b/SD-VBS/common/toolbox/toolbox_basic/affine/hemisphere_s.m
new file mode 100755
index 0000000..5300183
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/hemisphere_s.m
@@ -0,0 +1,27 @@
+function [x,y,z] = hemisphere(r)
+%HEMISPHERE     Generate sphere and transform from spherical coordinates.
+%
+%       [X,Y,Z] = HEMISPHERE(N) generates three (n+1)-by-(n+1)
+%       matrices so that SURF(X,Y,Z) produces a sphere.
+%
+%       [X,Y,Z] = HEMISPHERE(R,N) generates three (n+1)-by-(n+1)
+%       matrices so that SURF(X,Y,Z,R) produces a sphere colored by R
+%
+%       [X,Y,Z] = HEMISPHERE(R,THETA,PHI) converts from spherical coordinates
+%       to cartesian coordinates.
+
+% Modified from
+%       Clay M. Thompson 4-24-91
+%       Copyright (c) 1991-92 by the MathWorks, Inc.
+% by Carlo Tomasi
+
+error(nargchk(1,3,nargin));
+
+n = r;
+% 0 <= theta <= 2*pi and 0 <= phi <= pi/2
+[theta,phi] = meshgrid((pi/n/2)*[-n:2:n],(pi/2/n)*[-n:2:n]);
+r = ones(n+1,n+1);
+
+x = r .* cos(phi) .* sin(theta);
+y = r .* sin(phi);
+z = r .* cos(phi) .* cos(theta).*phi.*theta;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/im.m b/SD-VBS/common/toolbox/toolbox_basic/affine/im.m
new file mode 100755
index 0000000..6450120
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/im.m
@@ -0,0 +1,3 @@
+function im(I)
+
+imagesc(I);axis('image');drawnow;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/iter_AD.m b/SD-VBS/common/toolbox/toolbox_basic/affine/iter_AD.m
new file mode 100755
index 0000000..50bdae1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/iter_AD.m
@@ -0,0 +1,26 @@
+function [A,D] = iter_AD(I,J,mask,w,k,num_trans)
+%
+%  function [A,D] = iter_AD(I,J,mask,w,k,num_trans)
+%
+%  find the affine motion A, and displacement D,
+%  such that difference between I(Ax-D) and J(x) is minimized.
+%  If k <= num_trans, only translation is computed.  This is useful
+%  in practice, when translation is relative large.
+%
+%  mask: the weight matrix,
+%  w: window size for estimating gradiant, use a large value
+%     when A,D are large.
+%
+
+%
+%  Jianbo Shi
+%
+
+
+if k <= num_trans,
+  D = find_D(I,J,mask,w);
+  A = eye(2);
+else
+  [A,D] = find_AD(I,J,mask,w);
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/m_interp4.m b/SD-VBS/common/toolbox/toolbox_basic/affine/m_interp4.m
new file mode 100755
index 0000000..314f140
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/m_interp4.m
@@ -0,0 +1,49 @@
+function [F,mask] = m_interp4(z,s,t)
+%INTERP4 2-D bilinear data interpolation.
+%       ZI = INTERP4(Z,XI,YI) assumes X = 1:N and Y = 1:M, where
+%       [M,N] = SIZE(Z).
+%
+%       Copyright (c) 1984-93 by The MathWorks, Inc.
+%       Clay M. Thompson 4-26-91, revised 7-3-91, 3-22-93 by CMT.
+%
+%  modified to 
+
+ 
+[nrows,ncols] = size(z);
+
+
+if any(size(z)<[3 3]), error('Z must be at least 3-by-3.'); end
+if size(s)~=size(t), error('XI and YI must be the same size.'); end
+
+% Check for out of range values of s and set to 1
+sout = find((s<1)|(s>ncols));
+if length(sout)>0, s(sout) = ones(size(sout)); end
+
+% Check for out of range values of t and set to 1
+tout = find((t<1)|(t>nrows));
+if length(tout)>0, t(tout) = ones(size(tout)); end
+
+% Matrix element indexing
+ndx = floor(t)+floor(s-1)*nrows;
+
+% Compute intepolation parameters, check for boundary value.
+d = find(s==ncols); 
+s(:) = (s - floor(s));
+if length(d)>0, s(d) = s(d)+1; ndx(d) = ndx(d)-nrows; end
+
+% Compute intepolation parameters, check for boundary value.
+d = find(t==nrows);
+t(:) = (t - floor(t));
+if length(d)>0, t(d) = t(d)+1; ndx(d) = ndx(d)-1; end
+d = [];
+
+% Now interpolate, reuse u and v to save memory.
+F =  ( z(ndx).*(1-t) + z(ndx+1).*t ).*(1-s) + ...
+     ( z(ndx+nrows).*(1-t) + z(ndx+(nrows+1)).*t ).*s;
+
+mask = ones(size(z));
+
+% Now set out of range values to zeros.
+if length(sout)>0, F(sout) = zeros(size(sout));mask(sout)=zeros(size(sout));end
+if length(tout)>0, F(tout) = zeros(size(tout));mask(tout)=zeros(size(tout));end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/norm_inten.m b/SD-VBS/common/toolbox/toolbox_basic/affine/norm_inten.m
new file mode 100755
index 0000000..8e8865b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/norm_inten.m
@@ -0,0 +1,11 @@
+function I = norm_inten(J)
+%
+% I = norm_inten(J)
+%   
+%     normalize image intensity to the range of 0.0-1.0
+%
+
+max_J = max(max(J));
+min_J = min(min(J));
+
+I = (J-min_J)/(max_J-min_J);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/pan.0.pgm b/SD-VBS/common/toolbox/toolbox_basic/affine/pan.0.pgm
new file mode 100755
index 0000000..2e7b5f6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/pan.0.pgm
@@ -0,0 +1,53 @@
+P5
+# CREATOR: XV Version 3.10a  Rev: 12/29/94
+128 96
+255
+qquvvvvwzyz{}~€�‚‚‚ƒ„††…†‡ˆˆŠŠŽ�Œ��Ž�‘’“•–”–——›��œ��ž� ¡ ­©¤¢£¥¥¥¥§¥§©««¬­°±²²¶¶µ´³²´³³¶¹»ÀÀ¿ÁÃÄÂÄÄÃÄÃÅÆÊÇÆÆÅÊÉÈÊÏÞËÄÃÄÂÂÃÃÀÁ»qrrsvxwxyzzz{{|~~€‚ƒƒ…………†‡†‡ˆ‰ŠŒŽŽŽ�‘’’“”•–•“•˜™šœœ�ŸŸŸ¡¡Ÿª°¥¤¦§¥©«ªªª«¬­®°³´¶¶¹·¶··¸¸¹¹º½½¾¿»¼¿ÀÀÂÂÃÈÇÈÊËÉÈÊÊËÎÍÉØ×ÈÆÄÄÃÅÅÃÂÃÀtuwuruzzyzz{}}}�‚„„„†‡‡‰‰‰‰‹‹���‘”’’““•—–˜——™š›žžŸŸ¡¤¥¦¤¦·¬§¨©ª¬ª­¬­¯¯²³³¶·¹¹¹½¼½½¼¼¼¾¿ÀÁÃÃÁÄÄÃÆÇÇÈÉÊÉÉÊÊÎÍËËÌËÍÞÌÂÄÂÂÃÅÅÃÁÃÂtvwxyxwwz{{z|{|}��‚„„…‡ˆˆ‰‰Š‹‹Œ���‘‘’“•—˜˜™™š›œ��žžž ¢¢¡¢£¢±º¬©ªª«¬®®®°³´µ¸¹¹¹»¿¾¾½¾¾¾½¿¿ÂÂÁÀÀ¿¿¿ÂÆÈËÎÒÔÖ×ÖÔÓÐÎÎÎßâÒÕØÜááááàÜÙÕtvwwyz{ywz}}�‚�‚�ƒ†‰‰ˆˆ‡‡ˆ�Œ��‘“”’“–—˜™š™šš›œœ›œžœœž Ÿ ��š ´œ•™š¡¥¨¬®®®­­­ª©ªª­¬ª¨¦¥¢ÇÕÃÄÅÅÅÇÅÅÅÂÅÆÇÇÊËËÍÊÍÐÐÑÐÑâÖÓÓÔÖ×Ô×ÖÖ××Ðm„zuuvvvurvuwxz{{{}€�ƒ„ƒ„††‡‰Šˆ‰ŠŒ�Ž�“•™šœœ›œžž¡¢££¤¤¦¦©¨§¨§µ³¢¦ª®±³®¢�žžžŸŸŸŸ ¤§©©ªª§ËÚÁÁÀÂÅÄÅÇÆÇËÍÎÏÐÏÍÏÑÒÔ×ÙÙãèÝÛÜÝÜÛÛÝÚØÙØÓBUw…xz}~}~{{€€‚„„ƒ…‡†ˆ‡Š‹‹�Ž���‘��“”““•—˜˜š›��Ÿ¡ Ÿ¢¤¦¦§¦ªª«®®±Á®­­±³µ°¡šš™š™™›��¡¥§¨©«¥ÃàÈÇÊÎÍËÎÐÑÒÔÖÖÕØÖÖÚÞàáãâáîéâæåçéåâÞàâààÝGcZW�„}}~}�~ƒƒ„…„†‰‰‰‹‹‹���‘‘’’’“••–—˜œ�ŸŸžŸ ¡££¥¨§§ªª«®°°°¯ÅÀ¨­³·µ­ š�¡¢¡  ŸŸ ¤ª«­¯¨ÄèÑÏÑÓÔ×ØØ×ÛßÞßââãæéêêêêéíòéëììîíëíîïïííêGeoxae“�~€‚‚ƒ~„ˆ‡‰‰ˆ‰‰‹Š�����’“”—šššœ ¡¢¡¡£¥¦¦¦¨¬­®¯®¯°²²²²³´»Î·°¸¹µ­¥ œš›œœž¢¦ª¬®®²¸²ÇëØ×××ÝàáåæéêìëïîìïîðððóóöôðñóôóôôööõõóòîFakswvey�‚ƒ„……„€ƒˆ‰Š‹‹ŒŒ��‘‘’”•–˜˜™š›�Ÿ ¢£¥¨©ª¬¬¬®°°³´µ¹ºº½¾ÁÁ¿ÊÓº¿Ã¹±«¥§­±²±®©©­±´²³´±ËñáßáãæèìðòõõõöøùøõîæßØÏÓõðïñóõ÷ùõôöîëççïIbjnpu|x_‹Ž„††‡‰ˆ„‰��Ž��‘‘’”–———›œ��ž ¢¤¥§ª©ª¬®¯°²¯®³´µ³²´¸º¼¿ÅÎâ¡nbb_eq†¦Â¿Ìå÷ýýýýýüüüüýýýýþþþþþþþþýýýýüüüüçæ¼­®ã–½øúú§>¸áq±}̶Gbjkloqw|pn‹rcbbehq�…‡ˆ‰‹Ž�‘’”•–ššœžŸ¢¤¦©ª«®°¥Œ› “ »ÂÄÅÅÆÊÍÏÓ×ÙÜç«Õûúúü÷‹.ÃýýýýýýýüüøíüýÚÞîØüÝÈþþ«¨î–Ù»“ô臞‰fšb€�’Žf0^xF_VYqH^ehkoopqy€f†ðþþþûä‹|�”–˜–‹“¢£ ŸŸŸ¢¤§¨ª¬²±°°x;33>AV¨ËÐÒ×ÞáÌÆçñúã•ÄyÂýýýÉi5!q¸¬�ŽcMi<2<3WWB=XFoZMqeHNtMdfV„wNi…v\uat}ˆŽc*d�HY]ZtG]aced`C233++%)(*59:@7B>F>FEA=<\g^gb\^Ya][\PD83.2<Fgua?>X_=Hj?42)Wh3%GkgpD8€…€xb`vJQjFktVR|LvtTytLU{OdoU…}N]‡sSwZl‚Š‘[+e€IVYRqI[^___W?519+&'112>;9G<@;E8BG<:5V[FYYLPIHHMA4=A83:PMd{m@BdmUO�i?$ByC!'Q_pS$l„‚|hSrJMqG^ugp‡Os~Rq|QRN\wM}€SYŒyQuPe‡�’X#_€NQ_RpFX\]^YK512:)&(032>?9B=E:G7AJ@B;W\GRWHOHEJIB5>C@:4NQ^xp<5[njF€lJ, %pIQk]L*&X€w{qReNBnIGe’Ô¨Yj‡KayPR€XQ€Qu‚XQ�OtUb‰Ž”^%[�QPcRoGUY]YNC445=)%+.01?98EAG=E6>H@B<S\KPY?LFKOKG9GA8>7BFRwvN\r>itK2WRJmRZ9(AepvPZ[BpVCf‡Î¨U]ŠN[zSK�gU‡Pj‰bJ‘�QpX\‹Ž”d%U…VPhNmFUYYTG>26;?*'+247><;HCM@I7<LCE:T[JKWAJJMVJMAGB394<GLny]#HpGY‡Y:!Eh-(ekhG'2zli{_UkBgfAXl³§YLˆWP~cO€qQ‚T_�rM…†TlaZŒŽ”m'LƒYMiMrGVYVNC?368@.)+123?<9JEL@K;;K?D;QXNL\FOTQYOVKB@3:26FHg}o/
+3oXKŒrJ*4r=UzhI'-%lzp|mWtF[n<GZŸžhI�bO„nV�Sx]X�~M�ŽUgkZŽŽ“u,H†^MoPqHTTPH?</4<?*),.+2><<I>J@H:;JAF>O[SM_PR]XVLSJ8>8565BD_v>)hiE�wU-"lLGpcaHA$U~o~wRjLJrCCZ†ŒuDtsF}yPq„SmlT�…Ms�Y`tZŒŽ“~2CƒeMrRqIRSMGMXPF>A.**0.<C>APIR>L?9K;DEQ]XP`VS]\[OML5?C8?7BDX|~R[sBm‚_7[_"=cqfj_W;I�s|~T\[HvRCa^DxGj…Hq�Pg…W\yX‰ŠSe”e^�ZŠ�•„9@„kLrPmGOOIM[\MC>@+(*-,7>;<MDM=G<7G>GAJYUU_ZU^ZOKGP?IH2?5>GRu€e&KuK]”gH#DoNXhj]FE]jb|rvZRmIp_?fbCxV[‘NdƒQ\…[U…Qy‹W`”o[Š\‚‘”ˆA<qNvPmHOKFM[\NB>?)(,,+>BBDSHP@FA8ICG@F\WV]bS_\KLISOED2:4:FNpˆt85teTœhQ+*ENWOG6*+;PVm|x…kPpK`pVp–zuoZ–YW…_Z‹hP…PmŽ`[•{[“\}‘—ŽL;~wSvQiFIHDLXVJ@<>(''+)7AEESGO<GE;HBG@B_a[^^RTURVNWUGC3789KNh„}F*lxM‚zV3)7/5FHD:*"7JKTy~†yOiKQn69.bƒR„hR‡oW‡uT†Ve“m[–ˆ^‹]s‘–‘Q3t|PuRdDFECJTM?3 
+
+")+8ABBSFN=FD8HFHFB`a]^UONRRYRWVH=:787AA_‚ƒ`!aMd‘gN,!*5?>4*))(ATq~†„Q[VIt<6:R�Tq}U�‚VV|a^˜x]��^af‘–“V+n�OoS]EFEELOB/&5?ABREK=CI;KLIH=b_\YVPMT[cZVVGDA4<6>BT}‡m+N|\[šf/"$');GGC7,%2W]l€ƒ^UhHk:+DE‡Z`ŽWw†Tr‡Vmn]˜ƒXŠ”byeb”•–\%h†QlV]GGHDJL;+.%%1'1-!<?BRIH>?L@GMIN@\aYTVVQWgaWPMKIC1:6<ES}�|><€pY‚;%%"&/BPF<>9* ,NUVzƒmV{GW5 #FBuhZ�Uj‹\c‹XiX“‹W}•epmc–•—c(^†Pf[[EEFDJH4$&(COEZsW.<CCNEFEAN?FQHM?Z_UTSZYc_`^QVTNI3726BKo‹ƒO+t}O<*-,2<@=71-*($%9GJn„}XlKD2";Dc}X‹^]‹b\ˆ^]ŒZˆ�Uv—ijwd••˜k'XˆTbaXDDECJD+3N<.Av‚h:8B?NIDE>Q=GQDR:V]U^]`_`\^[M]VOI8868CIfˆ‰_!a‚_.')/3:1-35.+)#,DPFcƒƒbdY:1,;M[ŠXv\�m\‹k[”\~•Wo˜pg‚a�“–s(TŠUafUBCGBF:&&""$.ReV4 8A?NHDJ>O>DPISAP][dcb[ZSZfX`QBH?37/AD\„�u0F~T.31./:?:'9DBGJW}…dVj@5"3@XXŒWo„^Ž}_…xY�[n™bjžyjŒdŽ”—|+RŠ[ZlWDFGDA3+,!12'6FDIKJO>RBGUMUCQdcjfaWWT^gZXYFHK291<FW}’‚D6sC%8LPFFpmM,39j†t]qH'1LfT_d–\�„_ŒY~egœlh¤�g�f�‘”„4Q‡a[s[CDECB.'+-3'7EFLNKR?QDGRDMDJadh[UVY_ecUMXPTM18*2FQw�ˆU+^..?Xga\o„�d4+8i„~aoN$RvUpsaœasŠ\qŽ^wmcŸzj¥Ši’g‡”–Œ>K‰hZuWHGDDE.#           
+!"9HGJOLUCPEHRKMFJaf`XW^_e`^aUa^KI5:.,FLk��d$I#:WRQXXiy|wz‹��s;1/]�ƒaf]%! DuZg…\˜g`ŒWhŒ_i}e™‚c¡Žj�e|—˜’ED�iXzVEGFDC1-)%0ZdH')3;=HGEMKVBQJJTJNHHfc`WOjg_X]dXabOK674+EId‹�p6+!Njv�„†„}}‚��›�v>-"S~ƒvŠ�‚zˆ�{šc…sbŽb`ˆfd„g˜�a�“o‹hx–••XA€s\[GFFFC/44,-ZkL+*0=AGFCILUHQLJPAMHA^ZX[OkcVXdfYX]LLA3:1>K^‡“„C
$Tgmt{vuz{}‡”�–|>%R�|ž¦¦©¨«­¯²©pn‚hŠs^ˆqb‰m‹’Zz”kfr–“”]<vw[}_EGIJSK>AOUaga^bcehnppprvvvtw{xz{x~€ƒƒ„‚„‡‡‡‰‰‡ŠŠŠ‹Œ�‘”š�œ˜’v)+UL>516HZU?8?QhsE!cª¯°®¯®¯°²±²²²´³´´³³³´µ³²µ³´µ±°±¯±°¯¯®®­««¬«ªFFIL[bcfjlnoqruwyyz|~~}}�„€€€€€€�„……ƒƒƒƒ„………„‚ƒƒ‚ƒ„ƒ‚‚‚�€‚zE:\?+)69:“|=.0BTx^28^nmkijiiijfffdbcb`aa`_]^^``_\\\\]^`__a`b`bdb_GHJM[XPNOQTTVVWYZZ[[\\^]]^_`aabbbbbbcdfgfggghikllmoppqqrqqrrsrB>eSLIE:[²˜C<Lah�jDZ~‡Š‹‹ŒŽŽ���‘�‘���‘’’“”“”••——––———˜˜™˜˜˜˜˜˜—–•IJKK_kmmoqrsuwyz}~‚ƒ‚„†ˆˆ‰ˆ‰Œ‹‹ŒŒ������’��’‘’““”••—™›š››œ›ššž�W.Ex†�wft‹ £„aq‰œœg8Xž©¨«ª¬¬­®­®¯¬««­«¬®­®®¬««­«ªª¬ª««ª©ªª©©ª¨¨¦¥¢HHJKZddcfhkmopttvxyz}~}}~�‚�‚ƒ‚„†……†‡‡ˆˆ‡ŠŠŠŠŠŠŠŒŒŒŒŒŠŠŠ‹�Œs@;k�ž˜ŸŸ‰¥±®¬¢´·�bNp‰‰‰‰ˆ‡††††…†„„ƒ�€~}{zxwwwttrrppmmihhfdb`_][VIKMLI5

+
+                                
+              
+
+
+                       
+                            
+               

+        5H]vˆŸ§˜‚‹Ž�´¸®¢‰bW* #$$&&%'&%$&'%$##&$$$#$$$##$$#!##$"IIIHG=*
+                  
+                      (KUf~•žv1GCV¨´§—ƒvN$!-01*   ! !  ! ILLJLE2
     
+
+
+    
+
+
+
+    ;R]qŠ–vHE`…¨¨™ŠsF'&0/!=?B'


JJLJGB8(3>2,94>BB.7AB11<
''
+"5$EXg€‘Œ~p€š©¤•ˆ|W5((..!!%GHN)






<YT:EBMXS\jcIM\kfKJJJIE?)5B42@;FKH3@IG9;D
+
+#nf$!"
+;Xfx�€’µ­š˜š‘†|yr:T¤‰47A3,MSQ*
$2
TigRMHYc]]ieMLbonIKLLIGB4DR;;HAVXY9M]^CN^'
+
"^Z%!"<iTIªœ<h–G:’œ™�W7YcmZ<).+1LtŠ„{€‚=F†€Cy—o6…šs'
        
Bk*B]RSKSr|qWSRhxqnvq\et€ƒLKJJKGE:VeCK\TkokBcruL[q/
+
+
*m`%06$?hRJ§žBn—EQÃÈÈ΂0W`adw•–¥¡�‡„�otp5RŠt9^X7£Ê‡jßï�­Ùæ�1@1;QOQP`Š“|`d`„™‹ƒ“‰xŠž‘JLMLIHG;YoJUg]t}pEn}~L^o(*n[,PdC
M©™?q—DT½ÄÄÌ�!EY\bYQZ^hy}|wcql3T“u.5ªÏ�_¾Õ¨º¿×€/C5LPTXNi‘—‰qpoš£ŒŽ•Šˆ�š¨ŽIKMKKIG<\rGZg^zmDpz~Mcn%
+      /td0g|K

O¬—<xœBYÅÉÈÊ{?UXWL<4C_v{yyixo1\—w.;¯ÏzX º®È½áz>5*U{`<S“”€qs{¦ �‹’Š†Œš¬ˆJLMNNKG>_uH\h_xfDt|~Ngk!    /t_$$&T¬’={ž@^ÇÈÊËz @U\iux†�’’‚xzx—s3Z”s-AµËyj«´­ÄÁÛvO@2RRIQMx™“xst¢ž�–�Œ‚�¦¬~KNNNOO_WesG_jbx‚fHw}Nnj                     

+5|]$##Z²’?�—?bËÊÎÍpQdPY`inr}‡„|u}e¨š7V”u%OÀÏvl�§¶¿»Ë�z8HwgMeZƒ˜•xvr„£›•£¥•Œ�¤¢rLNNNVgjYgvKegcy„gJx}~Qsi
                   !#3_O#>EL™¥­§\cOONWWRT]kqkozdÀ¯:1><"2LTSq«·¹ÐÙÀgM=FNHFIUŽ£‘qrq‰¥œ‘ œ–Ž–£œpOPRRdmkXfuQjhg~ˆjLzPxf


+
+
+#&!]½mOKIOY_badin\‚ùø¨G:6$<WZ_y• Â¿ÔµH^\EKEAA[“¦‘rrsš±˜“£™•”›¦›oMMPTellUewWqfa€‰lLz€~S|b  
+"$5—ÆÁŸOMMJFMXafdVlãùûûöÃs1#">WZb�§ÇÇܬNRJDKB>Bg›¨�qvv•­–˜« •—œ¥�oMNQVgolUiv[sdc€‰qN}ƒ€S|b


+
+%%Ÿôêۼ׎GOPMKQ\\SSÖøùûûûûûû¸=&FX[f€�¦ÍÉá¶\<R^MVL?s¤¬�rwwŸ®”žªœ–�©§‡pMOPWhnmNez]we_€ŠtO|‚|Zƒ^

+
+$%J×ÚÛôè»ÄàwHMMLNOHZ«óøúûûùöûûûý¾l\]o•¡­×çíˆ>KWZOUOF|ª­‰uzz£­”¡¨›™œ¥¨~tMPRXjqlJb|e{eby†zV~„|[†\

+
                     
+ !$' �÷ôãäóòص»¢nILJG;q¦Vªöüüüû÷úûûúúúÉfqœ²kCY‚aDGZbOVWQ“²©†z�ƒ¬°˜©¦ž›¡ª§wzKMPXinkC[}awhbˆ|Y‡|\‡Z
+        $%>¬ôüüôèæôõë·—\{€S@F‡•b„å÷üüüûôõûûúúúúûç¾¥lY\Z<=5czK^nb�³¥{}†­©–­®§Ÿ¢§ t{KMQXjojFW€fyqf„Œ…`�‡|_ŽY

+    '&KåôøüüüôêçôöôØž�h]j<w¤‰v¾ïýüüüüùùûûúúúúûûûû°SPO5>7PŠwO|w©¸¤~€‚�´ª˜¬­ª¢¡¨›pzLNOZloh>N‚csyh†�†[|‰}a�Y          %&NÐýûùüüüüôîìôööï·œˆexh´òú¿Îüýüüüûøùûûúúù÷úûûûôôôºG?5Lp^;^zµ·˜s€~•À« µ³­©««‘qwLMOZloh>F…cpƒm†�’c~‹€b”_
      '%8Ìûøøöõ÷÷÷÷÷ïï÷õõõ鼸ª�—åúìãÊ÷úüüüüùùúúýýýóóûûûýýý§9><Sa`FZž¾¿˜sƒ�¤Ë¨¨¶´±¬¬¶ŽvnLNPYmpi@AƒjhŒl�‘’]{�‡^�j

+$%6Ðûûøø÷óö÷÷÷÷ðï÷õõõõéÒßàãííúöìóúüüüûõöúúýýüòõûûûýúø±RJTXOFKEˆÁÁ‘w…‚«Ñ±«º³±®°´„}qIMNYknhG>~p^”k}—žbz‘’\ˆw!
+      )' Ÿûûûøøóð÷÷÷÷÷óîõõõõõåÖñòýýýýõéôúüüüûöøúúýýüòôûûúûúúÈxŒf\WVIH”ÅÇŒz…|²Ï´´¿º²«²³{…kMOOZoqhE9~`�pwšžbv’•a€|*        

+ ()aÚäêûøøóðö÷÷÷÷óïõõõõõïÙîøýýýýøõ÷úüüüû÷÷úúýýõçóûúøýøú÷Ã`‰µ“›´�£ÂÊ|„�³É°¸Ã½µ°´¬q‚bNNP^ppiC0{�a„yu�¡jq“™l…4
%
+"*(�ÙÚØñùùöõöööö÷ôòöööööêÜîõúúúúùö÷ùûûûûøøûûüüùëñøóñýêòûÕga�G]uV­Á¸‚‚‡‹ÀÅ­ºÇù¶º¦q‚SMNQ^qohI&j�Yfˆu•œqd“—szˆ6       
+! !)+ >ÌïáÒßùù÷òôööö÷ôóöööööïçïôúúúúøõ÷ùûûûû÷÷ûûüüûêñøîïýòîöÑS‡Å_˜¨s¾Â²{†Š�Á¿£¼ÈÁ¸µ´–p~JMMO_wwpcVapXWkWr{OKƒ‘fpk'
$!#..!^ÄóáÈÙùùùóóööö÷õñõööööòëðõúúúúøö÷ùûûûûøøûûüüûðôøíñýðèñáef}rWh„ÅÂ¥x†„’¸¤ÅÅ¿¸·²‘x{DKOQbrrtwy|€„ˆ‹�’“–——™œ�¡¤¥¨ª«ª«ª©¨§¨¨¥£¡£¡ž�š”¥»ìéÂÑøù÷ìîööö÷ôðôööööôïïî÷úúúôñ÷ùûûûû÷÷ûûüü÷ëóøööýûáßû„GNBF7s»±‰gkwšÅ­¨Â¼·´�¥†s?MPRYF997:<><>>><<;;:<<?@BCGKQTV[_bhmuz�…‰���¥ÊÅáÚ¸Ëøøõëñ÷÷÷÷òíöööööõëè×ðùùùôð÷øúúúùóöûûûûùîóøðòýýÖÎýîÔÖÖÕ×ÕÕÕÙÛÛÙÙÙÙÖÕÖÖÕØÚÛÚÜÚLNR_jkhilmmoqtttuuvussrpoommlkjigedbba^\ZXXUC�ҿȸÏÂóøöïñ÷÷÷÷óåðööööôéåÕóùùùôðöøúúúúøøûûûûúëò÷îóýüÚØýë—€€�ƒƒ„††‡ˆ‰‹Œ��‘’‘’““”•——LLQbrsuvy{}‚†‰ŠŽ‘’“”••–˜˜™šž ž�žž ¡¤¤¢¥§¥§§˜Ì×ÁÇ´íÃîøòçóöö÷÷öäîööööõéêâùùùùòòøøúúúúùùûûûûûîòøéðýüßåýüµz{ywvvtstuqpqponmlkkjljigHMMH2
"%*,17;>AEINS[aflqtx}v¹Í½ÉÄÖÅìøõéï÷÷÷÷ôäïööööõéãìøøøùíî÷øúúúúùúûûûûûù÷ôÁãú÷ÙåýýëÌÈÈÈÇÇÆÇÇÆÆÅÄÃÁÁÀ¾½¾»º¸¸¶ILLMB$
LÒâÎØÝÔ²æøøíî÷÷÷õòæî÷÷÷÷öïàòøøøøëéøøúúú÷÷ûûû÷÷÷÷ûôºåðåØñýý÷žƒƒˆ‹��”˜žŸ£¦ª®°³´·¹»½¿Á¿IKKJD-

TÐæÕÐä׶Ù÷÷êé÷÷÷õóæë÷÷÷÷öðäîøøøøéå÷øúúúôóùûû÷÷÷÷úùÜçßÖÑ÷ýýý“#&$# ! !    !!JKLIF7

PÐóèÎßÓ»Ööøîî÷÷÷õõêç÷÷÷÷öïàîøøøøòîøøúúúòíùûû÷÷÷òùòÎÕÕÔàýýýýÎC&$#"     !""!"""!"#"%$EJLHC;'


]ÔöñÔÚñÖõøñð÷÷÷õõéç÷÷÷÷öíÝðøøø÷îìøøúúúðç÷ûû÷÷õéïðÙÈÍÖáüýýýõw%&%##"!#"!"#"#""""#$$&'IJIHF?.uÚùôÜØ›Çôøòíöøøõõëæ÷÷÷÷øîÌéøøø÷íð÷÷úúúïÝöýýúú÷ìòàľÒÜàöüüüü¨2)&$"#$#!"""#"#&%%&)(()HJKGDA7$


&‹ØùöâÖƯÅñøóæôøøõõçÞõ÷÷÷÷êÞöøøøöëñ÷÷úúúïÞóøýúúôÞïß²«Óß×èüüüüâW,(%%%$"#$####$&&()(+.0GHJJHB=,@¯áùøé×À¥¸êøóéôøøõóåÅë÷÷ôðæÜóøøøöéï÷÷úúúèÙ÷ýýúúõàò衤Üèßäüüüüú‡$('''%%&''&%&&'(),*,/2FGFGEB?2
[ÌëùùïÔ¯œ¤Üòñçôøøòïã¿ßóòìßÝòøøøøòåñ÷÷øöö×âýýýúúñßôâ��ÖðêàúüüüúÀ9-))('%'''&&((+---.022GHHGFB?7&
oÜóøùíϦœ—ÍëïêöøöóïÛ»ÛïðæÛäöøùùùõåóûûüüûæíùùùûûèÑéÊ™©ÔöóÖêûûûüîn+++(()'&%&'*+--.04333FFFGFB?9*~Ýó÷ùóק¤œÂÙáØòøõðëÖ·ÑëçÞßðö÷ùùùôäöûûüûéÈçùùùûùÔ×õ§°ËùñØêûûûüüŸ.-+*((')()+,-./013444FGJIGDA;0 
"—âùùùõݵ±£ÁÖÑÎóøöïêâÃËæÞÕäñóôùùùïÞ÷ûûüûêÐëùùùûùäîð³·½ÚùùÜãûûûüüÛS5/++,+++-/./10362251DGGGEB@<4&
B»ñ÷óôõÒ°³¢¹ËÊ´àøôâÔÖÁÆáââðêðòøùùïÛ÷ûûüüïÜóùùùùóØî⡶ÛõùùíÛóûûüüü…)-.,,+*,.00122244444FFEEDA@=8- _Öôôíëôླྀ£´À±ÇÞÖÍÏÒźÔÜêóáìòùùùïÖòøøüüìëûûûúøèØõÑ›´à÷ùúõÞêúúûûû¹60,,-.../0334555553/BBDDCB?;6."
yÐéèæÞÝìܵ›°À̹ÀÚÞÔÊÒǹÑàïðâîñòñ÷íÙõøøüüíéúûúúúçÛøÄ‘µæöùùöàáøúûûûîn-.-/../112333335421EDDEB@?<80%6–á÷÷÷öèÃÓÑ•¨¸ÆÂÀÚÏÈÈÑIJÌäòíÛëóùùùíÚôøøüøàãûûûú÷ÔÙú¿Œ¶èøúúúèÌéúûûûû¥6.,/121134464543244ADECAA?<93'N—Ñö÷÷÷øëÖⲫ½ÈĹÒÓÍÐØ̬ÉëôçØîò÷õöÜÐôøøüôÓâúúúùïÓãô¸‰©ÑôùúúïÙìúûùùûßZ20012/233121132122@?B@A?A=:4+!d‡†ÏÍ·¼¼œ¥½®¬ÂÌȵÈÒО¿¨ÁæòãÕïñö÷÷ãÃñøøüöÑåùûöôïÜìö°˜¹ñøðíñæàýýýýû™/44314442034532441BABBCB@><82&
‚ÆÞô÷õÑ™�”˜†˜³ÂĬ´³›‘Ÿ««¾ëòáÏßðòò÷îËîøøüøÔàø÷ðøîÝíö¤}Ÿ°äêæçîõòûýýýýúÐQ84443444222333121>@CDDCB?<93(DÀúÝ÷÷÷ùúÝŠb‚¯¬™‹“§�n‚ž®ÃìòáÓêð÷÷öíËáøøüüÝäüüûûöàìç€t¡¸¼áúúýúìöýýýýû÷…88432421255422122=>@AABA@<94+"nݱ³Ùô÷캙”™vkwwy„œ¯¢yWp’¥ÐòÜÍéò÷÷õçÈØôøüøæîñìêéæÍæævp›±òúúúýý÷öüýð½‹U973222122233220252@AABCB@><97.#(�®rH 9ltvpmb\ZZr™³¢T !&¡ËÊñòìô÷ïÎÓïöýîÞíåàëìçÊëÞnkœ¥ÖúúúûÝs2.BS@78423322312311221/11>@ABABA=<;72'(0&;_ggea\SQU]i™¯žSˆÆÌÝæôõõíÎÏëöýùè÷üûúûöâñÖfl–¡�ÆÅV7@CDK<3443223322334443233110=>CCBBAB@<94)!    ""!!"""##%"#&'!(AW^_``\XY^XWb‚Ÿ“P%¤µÈñøöøøðÐÍáôýýýýüüûùôæñÒxmŒ—€Z8:>FNI:899:7878557677766777554>?@BABB@?<95. !!"""##$%%##$%%&((''(-5@LSWYXVTRKDI^`hz`5*, /“©´øüøøøóÑÑßðüýüüüüüøíÛîÎuaƒ‹X=;=DOYI889:9::999879::999:98886
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/pan.1.pgm b/SD-VBS/common/toolbox/toolbox_basic/affine/pan.1.pgm
new file mode 100755
index 0000000..9d57f70
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/pan.1.pgm
@@ -0,0 +1,59 @@
+P5
+# CREATOR: XV Version 3.10a  Rev: 12/29/94
+128 96
+255
+prsrxxwxz{{||||}~~�‚‚‚„ƒ„…ˆ‡‰‰‰Š�ŠŠ‹���‘��’’’””–™ššœžœœ�ž�ž¡ «§££¤¦¦§¨¨§¨««¬¬«¯°±³µ·µ³´´µ´·¸¹¼¿¾¿ÂÅÄÄÄÄÂÂÃÃÄÉÈÇÆÆÈÊÈÈÌØſÿÀ¿¾¸prsttuyxyyz{|{{|~�ƒƒƒ„†…††‡‡‰‰Š‰Œ�ŽŽ‘‘�’“’“””“—ššš›�žŸŸ¡¢¢Ÿ«®£¤¤§§¨©««¬­®­¯°°°µ·¹¸·¸¸¹¸¶º»½¾¾¿»¼¾¿ÀÀÂÄÇÇÇÊËËÉÊÊÊÌÌÉ×ØÇÅÂÁÀÂÁÀ¼tuuuwwvwyxzz{|}€€���ƒ‚„†‡ˆˆŠ‹‹ŒŽŒ����’’�’”•–•˜”•š›™š�Ÿž¢££¤£¦·ª¨©©ªªª­¬®°­²²±³¶¹¹¹¾½½¿¼¼¼ÀÃÂÂÂÁÃÃÆÄÄÅÆÇÊÉÆÇÊÈËÍÌËÊÊÆÚÊÀÀ¿¿¾ÁÀ¿¼tuwwxxzyy{{{{||}~�ƒ†…„†‡ˆ‰ŠŠŒ��ŽŽ‘““••”•–—˜™šœœžŸŸŸ  ¢£££¥¡°¹¬ª«ª¬®®­¯±³µµ···º¼¾¾¿½¼½¾¿¿¿ÃÁÁÀ¿À¿ÁÄÇËÍÓÖÖ×ÖÕÔÒÑÑÑÏÞäÕÖÖÙÛØÕÓÒËuvuwyyz{~{z~~~€€‚�ƒ……†ˆ‰‰ˆ‡ŠŠ‹Œ����“••—˜˜š›œ›œ�œœŸŸŸŸ  ¡¡¡Ÿ››¡³œ—˜œ¢£¦ª®­­¯¬¬¬ª¨©«ª©§¦¥¥ÌÕÄÃÁÂÅÉÅÄÆÃÅÇÊÊÍÍÌÎÊÍÎÏÑÑÓâ×ÕÔÓÕ×ÓÔÖÕÏ_`k€{x{{zzywzyz{{{}~~€‚„……†††‡‡‰‰‰ŠŠ‹��“•™šœ���žŸ¡¢££¤¥¦§©¨©ª¦¶µ£§©®³µ®£œœ�žžŸŸŸ¡£¦©¨«¬ªÎÚÁÂÁÂÅÅÇÈÈÇËÍÏÏÐÍÍÑÒÓÕ×ØÙâèÞÜÛÜÜÛÚÚ×ØÕ8:E[‡{|€‚‚�}…„ƒ„…‡…‡‰‰‹ŒŒ��ŽŽ����’’’””˜˜˜˜š›Ÿ � £Ÿ¢¤¨§¨ªªª«¬­­¾¬­®±µµ¯¢™™šš›››œ›Ÿ¤¨¨©¬§ÆâÊÇÌÎÌÊÍÐÒÒÒÕÕÕÕÔÕÛÝáááááïæâäãäåãàßááÛ57@_WY…„€€�‚‚€ƒƒ……‡ˆˆˆŠŒŒ�Œ���‘‘’“”•––™š�žž   ¡¡¢£¥§¨¨ª««­¯¯²±Ä¿¨­´·´­¢š�¢¡ ¡ŸžŸ £ª¬­°©ÅéÑÏÑÓÓ×ØØØÛÞÞàäããåèèèëéèìóéèééëìêìííîê:9D`ny_i”ƒ€�‚ƒ†‚‡‰‡‡‡Š‹Š‰‹ŒŽŽŽ‘’“•–™™™›œž ¡¤¤£¥§¨©©®¯°¯®±³´³³´µ¾Î´¯·¹³®¨ �›žŸžŸ¡¦¬¯°¯´¸°ÈëÖØÙÙßßàèèëìëëïííññðððòîïôòôóôóõöö÷öóí8:B^kryxc}�„……††ˆ„†ŠŠ‹ŒŒ��Ž�‘‘’••˜˜˜™š›��Ÿ¡£¦§©«­¬¬­°±³´¶»½½¿¿¿Á¼ÈÑ·¿Â¸°¬§¨¯³³²¯¨ª®±´³´´¯ÇïÜÜßâæéìòôôöøøøøøóìæâÜÒÉ÷õóô÷ø÷÷îëôÝÛÚ8;C[gorv}wb‘�…ˆŠŠ‹ˆ†‹�Ž�‘’“““’”——˜š›�žŸ¡£¤¤¦ª«­°±²²²´²³·¸·µ¶¹»¹½ÄÊà ned_gx�­ÇÊÙñüýýýüüüüüüüüþþþþýýýýýýýýüüüüûûÕÑÀ®¥ß�·øúø¥0­çmšt9;D^iloquz~nq’xifdefq€„…ˆŠ‹���’“•—˜™›�Ÿ¡¢¦§§ª­°ª’Ÿ¥—¤½ÂÄÅÆÉËÍÐÓØÙÜç¨Õûûûü÷†1ÅýýýýýüüüüõèûüÚÞíÕûÛÀüýª¦ï‘ͺŒäØ�x—„^Ž[r‚‡ˆk4\|JSZ87A[gjknqtw{€f”öýýýûä�€��”–˜”•œŸžž ¢¡£¨¨«­°¯°°y962??XªÉÏÑÔÛßÍÇîñúà•ÁvÃøøüÃb0 n® ’WGa7-84Z`HA`Hu_NulKQwIaiTƒxNc‡w]uZm|‡�i&[~KQ_8:BZdeffcK5850)%)09::B6BAIAGIEB>]linhbc[e_^aRC71/4=Eera?=Y_=He<30(Wd0$FkenD9�„y_]wKQlImuVS|LvvSvrKT{MbqSƒ|R[ŠvSvUb€‰�]$\KM`;;CX`cba[A2/41)&2-/=74C<?9G9AE=;5U`BVTDJBDEJB6<A7,4RNc{l?BenWO�i?"Bu@#$M[lP'mƒ�yfWtLNsK`vkv‡St}NmSQ€MZ~Nz}UT‹{OrQcƒ‰�b$XRIb:;CU^__\Q:226-%'/03=75C<C:D7@J??8V]DPVBIE@DI@2=A@;5NS^xn;9`lgH‚kH+$lBSi]I((Z~tzpNaQErKGe”Ú¨Wj‡L`wPQ€ZP�Op�ZNŽ�OqV\ˆŒ’e$Q�VKl;=EVZ]\RF7559.+)32/:;9C>C8H8?H<?<TYFMY=MFHPLF9FB9?6DHRsuK ]p?lqJ5\UIiNX9'C‚fnuPYZBpWCg‡ÐªUZˆNYyVN�jV†Qf‹kK‹ƒNi]YŠŽ”n(I…^Go<=DQYZVLA579<*'+1-4=66E@H=D7;LCE<TZGGU=GJLULL@DA5:4=FLpyZ HqH\‡X:!Fe)$agiG&0yjgz]TlCgfDZk³¥ZK†WM}fQ€tQ€W^�tK‚ˆSgeW‹Œ“w.D€aJp>?EUYVQF>7:>A,%(2-6A:;N@KBL:7L?G@UYJ@XEMQP\GTF@@4:58EHh}m/6nXO�mG*3q<S{jJ$)&lwqnUvG[n>FU�™hJƒcLƒqV|ƒStbW�…Mz�TcoY‹Œ’~3C‚iLq??ESURJA=46<?,)+2,6A<=J?H=H;:J?G=OVMK`NQ]YWIRI8?8875CF`u=)kiGƒqO,"nL*Vn]T)*T~n~xNiLKsDC\ˆŠuErtG|yOm„ShpUŽ‡Lm’_\{V‡�”†::}oLtA@FSSNFFTSFA@+*,.-;=?AOBO:F=6L?IDQ^WN]RP_YTKIJ2@D8<6FFY|zN]vFpƒ^6$^cDSYjlt;0@‚s|T]^GuRBb`FyGh‡Ko}NgƒWV|T„‹Wd—jY‹[€�”ŠJ6xsMq@BFQQKL[[QD>A+)*+*7>>>LEL>I?7G<GBL\ZQ_YS\XKHBL;GG3?8@HSw€c$ItP_•hG4RgqgRJ]q{G&%5|rv€[RlHoaAfcBxWZ’Oa�R[†`V†Pu‹[`—u[ŽZyŽ”�J2vzPu=@GNLJN]]QD<>+,,0+<>>ES@KDGC;GBHBF^^X]aP[WEKBRNGC0969GKo„q45teW�jJERTOC3,7O_K*#h}{‡oRqJanUi†rwqW—]U„`W�kR†Qn�e[–…Z�]sŽ”�T1t~PrACEJIFKYWLB>>'#(-*8>>@REM<EF:HBEEC`a\\[MOPMSITPED6847KMhƒ{E+mzO}_53CMKB.#6FKH3+Vƒ€‡{PiJOm69.aƒO�jP†oT†xVƒXb•s[”�\…]j‘••T)l‚Nl@?DFFDISQG>)
+ &':=@CSDJ>GH<HFHI@ab\VPJFNMVSVXG<<696AA_�…^a~S6(#'3BJ</(*'>UC/ Gˆ~„ƒPZWIr:37Q“Sn�V}�V~�Tyf_˜{\�”b|ca•”•\&a‚MeACEHGELUF6&6<@BREH;?K<JLII>caYUSNLRX`YWUEDB4;7?EVˆm*O{P'$&)8DIG;.'.S[E'2}�„^RiGh:*ED„[^�Vt†To‡Tjr\–…V‚•cqj`’“•b"[‡Oa?CHIGELQ?/1) -#+'&7=DTHI==L>GOFO=^\WRPUSYd^PKIHHC062;EP{‹}7
+=S*# (>RG9?9/ @VJ3'j~~ƒmUyGX1!FCujYŽUf†Za�XgƒY�ŽVz“ffub”•—k'UˆR]ACDEFFKL8'&#@UERnX-9BCNEDC=L=FPFL:X\RRSZYc]]YKPPOI3616CLn‰ƒL".),*-8A@;53/*%$8HH;R„}„~YmLC/#;Ec~YŒa[ˆ`[‰b\ŽY‚�Vp˜mgc““—r%RˆWXCCEFHFKG-+OA3;t„n98@@QFEJAS?GRGR:S`W]\_b`W[[P]VNH8845BIf…ˆ`" #&+/40/78/,+%!8NE>#B„�„‚^aW;2.9NY‹W~v]�q^ˆq^•Ys•^l�ui‡dŽ–˜{+O†_UACEEFCE>''')',JeZ=!7A<JHFL>Q@EPEP?P^Ycdc]WOXd\cSDG>46.CF[„�s4!!,42-.7;8)-<CJKJ26z|~„dXmB5$6CYXŒXn†]‹`…}Z‹]h™igŸ}h�g‹”–�0K‡eWEFFHHGE7..!%&34$2C>EKEP@THIULTAMbajebUUV`h[XUFIN2;-;FW}‘�F%(4HPH=blQ49:1$f}w‡uZrF%.MjX€ee˜^�‡^{�]hc›ul¥…h�f†“•‰?G„lVBDDEHFC1(-
+)2%0@DEKHS>SFIUFLCHachZYXW`fbUOYPUO19*2GPu�„B'8Od`Ue€�l<)51U†zƒ}`qN&QuTpwaœaoŠYm�]tr`�f¡�i’e~”—’EAƒqWEEGIHEG0!
+#"6FEHQKTEQDERKMDEae^UR^^d\V[P`^KI3</.GNmŒŠB/LMEMTcu{zv…‘ŽzJ53/Bˆy€ƒ\aY  Dw[eŒ]–fa�Vd‹`c‚h˜Š_š‘jŒcy——–J<|tXEFGIIIG3+(%+O`H'(2:>IIFOLU@NIHTKKDCe_ZTMkg\UXcW^^JH6:6+DGbŠ‰<@es€…††€~‚�žŸ“|I0*3.2y}ƒs‡Œ…„|ˆ‚z›a„ue‘eaŠe_ˆh•’`Œ—k„hr–”—]:z}YFEFIIJI323()UkO*(.9AIDBKGOCNJKRCNHA^UT[Jg`WVbdZV[JNA5<0>I\†.Ehpx}yxz{‹™£ž„I'!,$#g~‚�z�£¦«¬®¬©®¥nl„gŠt[†qcŠh€�Yu™fwii”�”i;oYEGIKLIPI;:GM[c^X[]acgklmmqpoqswuvwt{zy}z€‚†………‡†ˆ‡‰‹Œ��’›“BKSG<69G[ZI@EUlwT(I¦±¯¯±²°®®®°±±±¯¯°²´µ¶µµ´³²±±³³±²°°¯¯°¯¯®­¬¬ªª§FEFILMYgiikoprvwy}€}‚……†…†„…††…††‡†‡‡ˆ††‡‡‡‰‰‰ˆŠˆ‡‡‡ˆ‰ˆ†…„ƒY-UM2)-88{}B/'8Xsi4(Imnnmnmlhhigeccecb__`a^^\^]\\\]^]]][\^^``bbceebFGGJLLZYOMNOOQRTTSUVYYYZ[[\\^^___^^^`abbcegffffgijiknnnonooqY(2`TGCA=K¦©K9>V_xwHPo…†ˆ‰‰‰‹‹��ŽŽŽ�����“’’““•••———™˜—˜™™˜™š›šš™š›š˜HIJMMN^lqopqrsuwzz}ƒ‚‚„†‰�Œ�ŒŒ����ŽŽ��‘’“”””““”–••–˜››œ›œœšn4Aj�ymeiƒœ¨†`_v“ž�M@‚¦©©ªªª¬«­¬¬­®®«¬¯­®®®®°°¯­¬®®®®­ªª©¨©¨©©ªªª©¨§FGHJKKZghgilnoruwwz|}~‚ƒ‚ƒ„………………†‡ˆ‰ˆˆ‰Š‹Œ��Ž����ŽŽ�ŽŽ���Ž~M;aŠ�—˜¡��­¬«š­º¨wWj‚ŠŠ‰ˆ‰ˆˆ‡……‚ƒ„ƒ‚€€~}|{yxwvtrpomlkhffebaaa_\[YWTKMMLNMK8"

+                 
+
+

+        
+
+
+
+
+
+                                            
+



,?Qt‰œ§š†�˜ž·»´«”taA!!"$&%$&'''&(&'&'(&%%#%$"$$%#!"##"HKKJJKI?+       
+        
+                               
+
+ASd|“ ƒ>AGR¡¶®ž‰ve5"++!$"
.12,!! "! ! JJLMLLKD5

+
    
+

+   
+3P\m†•|E?Ww¤®Ÿ��z¦ƒ*11''1/ =AB)

KKNMKKID;%->5-:3:@C.5AC52=      (+3

!?Wf{Ž�~o{˜©¨™‹�²~(00&%-. #$EDN+









8WX=GCMZTZfdIL]IJLJKKKJA+1D3,@8FKL.9GG64F         

 kj(#$4S`u‚“¿Â£šž”ˆ|œÄx3qq;S¤Š69G6*MQQ(

+
+NeeRMDRd^]feGG[JJKLONMKE2>S@9LAVXY9H[]CG_)
+        
+
+      \[%"$<hSF¨š=j—F:�|\bmdJ1005QuŒ…|ŸÀt:ƒ?Fˆ�Dz™r6…œu'
+        <l,=^SUMOq{rXUNdvpmwu^_qKKKKONMJE6JgLEbSmopC]rtORo,
+
       &kb'-4%@gTI¦›:k˜CNÄ©caa[exp���„„„�´Ås:vq6RŒt8[|Z0ŸË‰gÝò›ªÜé”3A2:QPQU_ˆ•€hd]�™‡‘Žy{ƒIKLLNMLJH:QoOOm`s}xFg|NWt-
+
      (l`*QfDK§˜;o˜ES¼¹|[_d_Xccfx€x‡ÁËt;vl2T–x-4ªÑ‚_¾Ö¦·¿×„,D5JOSYQf�–ˆrqj˜¤‹‹”‰ˆ�”LLJKOLLJH=TsKToc{�vFm{€N[r(
++rc.g€NP­—<vŸG[ÆÉ™YWUPB9C]pwxv’ÅÆp<yq3Y—w-:­ÕY¡¸©É¿å~;6(Nt_<Rz’–€nur�ŸŠ‹“ŠƒŠ—LLLMNOOKI=XuITk_x€pEo{€Ods% 
+
+
-tb%&)!T­“;{ŸB^ÆÈŸZ[eqr~‰�‘…xxo¥Åp=u3X˜u,AµÎye«³ªÄ¾ÞuK=.QQDOKv™“xrv{ŸžŽ•�‰†žLLNPRQPO[V\zJWi`{‚qGu|„Rlq"
     
+3{`$"%Y±“>˜>cÊÃ�UYbksw‚ŒŠwx_’Þp:{s3O–v-I¿Óvj�¥°¾¼Î€z7GuhLc[�–˜{ts£Ÿ”¡¤ŽŠ’žJLNNOOUfkW`zL\ia|ƒoHv~�Soo 

          #&
5bR$AHM˜¡pMNUXSSZhqlkx^›ër0QK-.<:#2NUQmª´µÏÖÀeO:FMGCHR�¥”ssq†¥�ŽŸž’Œ–¡PORRRSdnmYb|Naj_}†rJx„Tsj
+$%  ˆyFLIQY]dhbgnvlÚú±=77*,43$8UZ\lŽ”½¼Ï¸IY\DNHBF\”¥�oop’³š‘ š–“— MOOOORdnmV`|Sgk^~‡uKw‚Tuh      
#%+€¸¦UKKHEJS^dda[Ëûûýñ°^/1:8':VX`|˜¤ÆÃÖªKPICMD?Abš¨Žnrq�«—•ª¡“–˜¢NNMQSUgpoU^SgiZ�‰{Mx‚„Sye
+

+
+
+%%!¤çٺɒBJNMKQ[\WL·ûûûýýýýû«G9$ ! >W[bx˜¡ËÅÛ³Z5G_LZN@q¢«Šnvs˜°”™¬ž˜œ¥¤MNOQQTgpnP^Zni\~†}Qu��U|c
+
+
+$&
_×ÒáäÆÂÛyFKLKLNKO­ùúûûýýýýûûù¹K'%%EX[k�œª×èîˆ<HR`RTODx¨®‰pwt™¬“œ¨™•ž¤©OONOQWiqoJR�bul[}‡„Vzƒ„Z€a
 
+    &'‡øõäîîÞ¹ÙÎpHLKFB[�`©õúúüüüüýýýýü½<)HY[o™±g?W�_BCUgGMZJŒ±¬‡wvz¥®•¡¦œ™¡¨¥NOOPQViqmEQ‚_nk\‰„Xz†„[†a
 
+&'
-”ðúúöêóóê™]svM>Az�qv×ôúúüüùùýýýýüüüÊ…^^vœ¢jT[[;;*UzKYn^›´ª}x{©§’¥©£ž£¦ŸLMNOQViqkDN„bqt_…‹�^z‰ƒ_‹_

%&AÒïúúúúõèóõóà¢�f]kAjŸ†n¯êúúúüüûûýýýýüüüüùø§‚Ç¡nOOP7</H‰yKyt¢¹¦z}ƒ‰­¬”¨¯¨¡ ¤œLNNPQYkplFHƒbk~e‚Ž‘\v‹„`Œ`       
+%'SÔý÷ùýúúú÷ìóõõó½—ˆhzg«òúÓÖúúúúüüüüýýýýüüüüùùùõÖ�Àìë»D>.Do[6^t³º�r��½«š¯°ª¦§£“PMMPRVjokIB�ilŽn~Ž•]x�Ša�d 
+%'!?Ñøø÷ô÷÷öööôñôôôõ渲Ÿ‹‘ÝüííÛîûûûýýýýüüüüúùùúýýýýüÄÇØá¨7<3La^CV”·¼—l€šÇ¨¥´´±««µ”MNOQRYlpmK>€rc‘lx�–^v�Ž[ˆo

+')`êøøø÷õö÷öööóïóôôõõëÔÖÝäðë÷üúúûûûýýýýüüüüúøøúýýýýüßÉÒì¨QEMTQFHB�¿Á“s‚~¢Ñ³¨¹´²®­µ‰NMPOOYmpkN8|~`‘kv–�at�•^„z'
+'&:Éøøøø÷òó÷öööõñóôôõõîåõõøúüüüüûûûûýýýýüüüüúúúúýýýýüùñùûYƒ[[UTG@‡ÅÊŽw…zªÏ´®¼¶³ª¯µƒLMNPPYnrlO6zŠ]‡tp›Ÿcn’—`z€- 
+        
 ))–ìïõøø÷ôõ÷öööõñóôôõõñæóùùúüüüüûûûûýýýýüüüüù÷÷úýýýüûü˵٠­a€²�˜²ˆ—ÀÇ�zƒ|­Ç±´Â¼µ±³²uNPQPQ]qtlN+o‘`{}t›žmg•šmw‹7&
+! !/MÈàáè÷÷ø÷÷øùùùøòôööõõóìðöööûûûûûûûûûûûûùùùùù÷÷ùúùúøùû¸OHnµlZ�FZ}Q¤Â¾„€†·Æª³Ã¿·´¸ªmNNMNQ]qtlP$a�^]Žr��vb•™xv�9       !!xúæÙâ÷÷øõöøùùùøõõööõõôóôöööûûûûûûûûûûûûùùùùùøøùõóúø÷úàkD\‹P|Ã]’­p¸À¸~„‰‰»Ã¡µÇÁµ±²›lLNPRR_wytgYbs]XpYk~UF{�ees,$!!6�æèÕØ÷÷ø÷÷øùùùùóôööõõõóõöööûûûûûûûûûûûûùùùùùøùùôõúúùøùû¶™–RazrVi‡ÇƬxˆ‡’ļ¡½ÅÀ¸³­‘nKLMPS`rssvy|ƒ†‹�‘“•™šœ�Ÿ£¥§©©«¬­««ª¨§¦¥¤£ ›°¼ÖðÛÏó÷øõõøùùùùôôööõõõôôôõöûûúúûûûûûûûûùùùùùøøùúúúúõëøð†LZ?HKDG:r¾±�kny•Æ³§Â½¶´�¥‹wKLNPRZH9:99<=<>=@=::<;;==?@BGLORUY_ciosx~ƒ„€±ÄÏãоï÷øôóøöööõóöööõõõñóéó÷øøø÷ûûûûûûûûøøøøûûûû÷÷ùùôÓùùÜÑÑÒÔÔÕÖÖÕÔÔ×ÙÞÜÛÙ×ÖÖÙØ×ÙÚÜÙLKMOP^lnlllmnopsuuuwwvutsqpopnmmjhgeeb`__]\_»ÀÐÇÛÂç÷øõõøöööõðòööõõôïëÞô÷øøø÷ûûûûûûûûøøøøûùúûîñøù÷Ùúü¶x{}}~€��ƒ…‡ŠŠ‹�Œ���“”••–—˜–JLMKN\nrsru{}€…‡‹ŒŽ’“•”•—˜—˜™›�Ÿž  Ÿ¡££££¢£¯ßÆÚÂÚÐß÷øôôøõõöôîóööõõõðìä÷÷øøøøûûûûûûûûøøøøûûûûðòùùøâüüÕ…{zvvtrrqpprqpoommmmkkihgheLKJNOK:
+
+
 $',048:>CHNTX^chd—æÉÍÇÂÇáöøôôøöööôíñööõõõîëñ÷÷ø÷õöûûûûûûûûøøøøûûûûÑßøõêàüüùØÉÇÅÅÄÄÄÄÅÃÄÄÄÄÂÁÁ¿¿¿½»º¸·´LMLLMNH*
}èÖËÚÕ¸Éøùõôùõõõóíñ÷÷öööðêöùùööòôûûûûúúúúùùùùûûûûÅÖöñÝäùùý¶o{�…ˆ��•™� ¢¥©®´¸·¸¹¼ÀÃÆÂLJJLJJG4
+

†ëæÇâá½É÷ùõñøõõõôïð÷÷öööïé÷ùùööíðûûûûúúúúùùùùûûûûáêëáÍäùùýÐE(%!KKLLIIH>%ˆíïÓÚÙ¼Îõù÷ñ÷õõõõðñ÷÷öööïç÷ùùööóôûûûûúùùúùùùùûûûúãâåâÞôùùýõ{&#!!    !!!!" !#LLKLKIE?-




�êóàÒÏ­Éóùøõøõõõôðð÷÷öööðçõùùööòóûûûûúøøúùùùùûö÷÷æÙÍÚÜôùùýý¯2&$#""!!!  !!""!!"!#""$MKLMJFGB5 
4¬ðöèÓÖ¥»ðøöôøõõõõñï÷÷÷÷÷ðâòøøööòôûûûûûùôúûûûûüüüæÏÊËÞàôüüûûì`+$#$$!$$""#!   ######%'&KIJJIGEB:(

N¾ñöìØζ¿ëø÷òõõõõõïêö÷÷÷õëæ÷ø÷öôñôûûûûûöðùúûûûùíöâÉ»Îåéïûüûûû�#)'%$"###""#####!#%%&%&)KHHJIIFC?/

lÒò÷ñßɪ­áööô÷õõõòãÈì÷÷õðéê÷øøöôòöûûûûûìåúûûûûùêùá°¡ÚíöðûüûûûÃ<+($$$'&&$#$$%$#$%&&&),+KIHJJHGD?4#

…Þô÷ôäÁ¦ ÒñõòöõóïëÛÃâóòíááóøøøöòîöûú÷øõäòûûûûûîáùâ«—Îìøìöüûûûós(*(&'&&&&&&&&&'('&)*,..IHHIJFECA9(-¢æö÷õฤžÅéòðõôïëèÚÄÙñòçßç÷øøøõðíóýýýýùôúûùùù÷ßÓëÚ¬§Éïûåèüúúúú¨7-,)(''&'''')(&'*-./123IIIHEFECA<0 =¹ìõ÷÷뾬¢¸×èæñóïëçÖ½Óëèßãòøøøøõíêôýýýüïãöûùùù÷ØØóЫºÈóüïðûúúúúã\0-*)(''''('')*+,--0123HHGHHIGEC>4%UÙùöö÷îó¥ºÖÜàôõòìçÜÉÏåÞ×çòøøøøõìèõýýýýéÜöûùùùõâèôÁ¦ÀÓúüòçùúúúúû�(,*+)))*+***+,-/223365GEHIFFDDB?7)kàúóñõí¶¡®ËÓÆæôòåÒÑÇÌáâäñòö÷øøôçäõýýýýöñúûùùøðÚïì°¯ÏëüüúïõúúúúûÈD0,**((*+++++./0344223GGHGGFIFCA</"�ìøðëïïÑ»¤®ÂɾÍßÍ¿ÅËÈÈÙßêïîùøúúöæâöüüüü÷÷ûûúúøèßöÛ¡®Õõýûûòðûûûûüòz-,-.-)+,,-./131343343EDDFEEEDC@<2&3¬åìêæÜèëÆŸ©ÀÎÃÃÕ×ÍÅÐÊÃÕãðñïùõòóõåâöüüüüõöûûúúøãàøЙ¬×öýûûòíúûûûüü­6.,---,,..0/023332222CCDFGHGEA><6-!FÁúúööóÚËØ©£»ÌÐÉÐÇÃÇÔÌÀÓçòñí÷ùúúöãßöüüüûïõûûúúöÜçú‘¬Ùöýûû÷éóûûûüüîk0/-/.,-./01212342243DDDEGGECB@<7."`¯÷úöööóáÞĪ»ÆɼÊÎÈÍÚλÓëõïí÷÷ø÷óÕßöüüüùèóûúùøîÛêò¹‹¦Ìðüûûûìîûûúûüü 1.-.--.-0/0223310110CBDDECDB??<80$,{w³Ù¼µ¿® ¯°§ºÅʶ¶¿Ç¼²º±ÆçóëèñóùùôÕÍôüüü÷âòûûòóòèõ÷¶‚”­àûúúúïÙòûûüüüÜZ111/./0110111310110BABDEEFC@?=;4) 
7§Ïðúûí²‘�–Ž�¬ºÆº°¥”Š™§³ÉìôæÚêöôõõâÑòüüüøãíõôðõõñ÷÷ªy˜¤ÐííçÜÛìûûûüüüú’0200010011331110121D?ACEDEECA>;7,#lͺÎ÷ûûûûô®qy¢ ‘‰“§›qw’¢¼ìóßÜíôùùóâÌíüüüüòöûöóúúãîð–n›´È½ÝøúúûûûûüüüüÌM5331121/1220100220?=@BDDDDB@=97.%!¢Ì¯ÐñûŒ™ˆjrut˜¯¥~UhŠ¢ÆìÕÕëõùøðÝÇæüüüùëîìêíìäÚðìˆj’¤Ôýúúúúûûûûü÷ÍŸ`9431121212111100/00@=AAEHGDC@>:62&R¯…A*\qsrmaYWUk•±¤T#’ÊÚòóñøõçÉß÷úúñéñíï÷öíÚòæ€j� Àþøøøøû¿j;=UG7322243122112100110-.?=>@CDEDCA><83* ,0$,Rfgeb_URU]h�¨™S
+ŽÈ×ÜïõöóåËÚóùúöôûüøøüöîøæ|b‘«›Çð¹X5?BIL;2222201232332111221011/?<=>BDDA@BA>;4+!  !"   !""!$$%$'&(6Q\\_b]ZXZXR^yœ Y(œ¬ÁíõùùõçÍÓîúúúüüüûúú÷ñøÜŠo‡ Œ|Z:;AJOE7355356866544565565553311>=??BBCDCA@>950! "!!#"!""$$%#$$%%&*29GQUWYUTTLDEVWZeX9180>Ž£°òøùùöìÒÕæöúúüüüüùöñìõךry•�M==AJTS<6799788768987677886676765
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/readpgm.m b/SD-VBS/common/toolbox/toolbox_basic/affine/readpgm.m
new file mode 100755
index 0000000..a5fd7f2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/readpgm.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/affine/simulation.m b/SD-VBS/common/toolbox/toolbox_basic/affine/simulation.m
new file mode 100755
index 0000000..2186a6d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/simulation.m
@@ -0,0 +1,42 @@
+clear
+
+figure(1);colormap(gray);
+
+%------------ Parameters --------------------------
+window_size_h = 40;
+window_size = 2*window_size_h+1;
+noise_level = 40/256;
+
+% define A and D
+x_ext = -0.423;
+ext = 1.232;
+A = [ext+x_ext, 0.2534; 0.3423,ext];
+
+D = [3,1];
+
+%------------- compute image I and J ---------------
+disp('generating I')
+I_init = gen_feature_s(window_size);
+[size_y,size_x] = size(I_init);
+
+%define image center
+[center_x,center_y] = find_center(size_x,size_y);
+
+%  adding noise to image I
+I = I_init+noise_level*rand(size_y,size_x); 
+% make sure all intensities are positive
+I = I.*(I>0);
+
+disp('computing J')
+J_init = compute_J(A,D,I_init,[center_x,center_y],[window_size_h,window_size_h]);
+J = J_init+noise_level*rand(size_y,size_x); 
+J = J.*(J>0);
+
+
+%------------- compute A and residue ----------------
+c = [center_x,center_y];
+num_iter = 8; w = 9;win_h = [window_size_h,window_size_h];
+
+fig_disp = 1;
+[Ac,Dc,mask] = compute_AD_disp(I,J,c,c,win_h,num_iter,w,fig_disp);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/sports1_11_28.jpeg b/SD-VBS/common/toolbox/toolbox_basic/affine/sports1_11_28.jpeg
new file mode 100755
index 0000000..39ebed5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/sports1_11_28.jpeg
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affine/test_affine.m b/SD-VBS/common/toolbox/toolbox_basic/affine/test_affine.m
new file mode 100755
index 0000000..41b48b9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affine/test_affine.m
@@ -0,0 +1,33 @@
+%%% This is a test program for Affine tracker %%%%
+
+disp(sprintf('This is a test program of Affine tracker'));
+
+%% read in images
+
+disp(sprintf('read in images'));
+I = readpgm('pan.0.pgm');
+J = readpgm('pan.1.pgm');
+
+figure(1); im(I); colormap(gray);
+figure(2); im(J); colormap(gray);
+
+
+figure(1);disp(sprintf('click on the center of a image window'));
+c = round(ginput(1));
+
+%% compute the displacement of that image window
+disp(sprintf('computing...'));
+
+win_hsize_temp = [8,8];
+w = 3;
+num_iter = 6;
+
+disp_flag = 1;
+
+win_h = win_hsize_temp + [w,w];
+if disp_flag == 1,
+  figure_id = 3;
+  [A,D,mask] = compute_AD_disp(I,J,c,c,win_h,num_iter,w,figure_id);
+else
+  [A,D,mask] = compute_AD(I,J,c,c,win_h,num_iter,w);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/affinityic.c b/SD-VBS/common/toolbox/toolbox_basic/affinityic.c
new file mode 100755
index 0000000..e48762a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/affinityic.c
@@ -0,0 +1,186 @@
+/*================================================================
+* function w = affinityic(emag,ephase,pi,pj,sigma)
+* Input:
+*   emag = edge strength at each pixel
+*   ephase = edge phase at each pixel
+*   [pi,pj] = index pair representation for MALTAB sparse matrices
+*   sigma = sigma for IC energy
+* Output:
+*   w = affinity with IC at [pi,pj]
+*
+
+% test sequence
+f = synimg(10);
+[i,j] = cimgnbmap(size(f),2);
+[ex,ey,egx,egy] = quadedgep(f);
+a = affinityic(ex,ey,egx,egy,i,j)
+show_dist_w(f,a);
+
+* Jianbo Shi, Stella X. Yu, Nov 19, 2001.
+*=================================================================*/
+
+# include "mex.h"
+# include "math.h"
+
+void mexFunction(
+    int nargout,
+    mxArray *out[],
+    int nargin,
+    const mxArray *in[]
+)
+{
+    /* declare variables */
+    int nr, nc, np, total;
+    int i, j, k, ix, iy, jx, jy, ii, jj, iip1, jjp1, iip2, jjp2, step;
+    double sigma, di, dj, a, z, maxori, phase1, phase2, slope;
+        int *ir, *jc;
+        unsigned long *pi, *pj;
+        double *emag, *ephase, *w;
+    
+    /* check argument */
+    if (nargin<4) {
+        mexErrMsgTxt("Four input arguments required");
+    }
+    if (nargout>1) {
+        mexErrMsgTxt("Too many output arguments");
+    }
+
+    /* get edgel information */
+        nr = mxGetM(in[0]);
+        nc = mxGetN(in[0]);
+        if ( nr*nc ==0 || nr != mxGetM(in[1]) || nc != mxGetN(in[1]) ) {
+            mexErrMsgTxt("Edge magnitude and phase shall be of the same image size");
+        }
+    emag = mxGetPr(in[0]);
+    ephase = mxGetPr(in[1]);
+    np = nr * nc;
+    
+    /* get new index pair */
+    if (!mxIsUint32(in[2]) | !mxIsUint32(in[3])) {
+        mexErrMsgTxt("Index pair shall be of type UINT32");
+    }
+    if (mxGetM(in[3]) * mxGetN(in[3]) != np + 1) {
+        mexErrMsgTxt("Wrong index representation");
+    }
+    pi = mxGetData(in[2]);
+    pj = mxGetData(in[3]);    
+
+    /* create output */
+    out[0] = mxCreateSparse(np,np,pj[np],mxREAL);
+    if (out[0]==NULL) {
+            mexErrMsgTxt("Not enough memory for the output matrix");
+        }
+        w = mxGetPr(out[0]);
+        ir = mxGetIr(out[0]);
+        jc = mxGetJc(out[0]);
+        
+    /* find my sigma */
+        if (nargin<5) {
+            sigma = 0;
+        for (k=0; k<np; k++) { 
+            if (emag[k]>sigma) { sigma = emag[k]; }
+        }
+        sigma = sigma / 10;
+        printf("sigma = %6.5f",sigma);
+        } else {
+            sigma = mxGetScalar(in[4]);
+        }
+        a = 1.0/ (sigma);
+        
+    /* computation */ 
+    total = 0;
+    for (j=0; j<np; j++) {            
+
+        jc[j] = total;
+        jx = j / nr; /* col */
+        jy = j % nr; /* row */
+        
+        for (k=pj[j]; k<pj[j+1]; k++) {
+        
+            i = pi[k];
+          
+            if (i==j) {
+                maxori = 1;
+            
+            } else {
+        
+                ix = i / nr; 
+                iy = i % nr;
+
+                /* scan */            
+                di = (double) (iy - jy);
+                dj = (double) (ix - jx);
+            
+                maxori = 0.;
+                    phase1 = ephase[j];
+
+                       
+                /* sample in i direction */
+                if (abs(di) >= abs(dj)) {  
+                    slope = dj / di;
+                    step = (iy>=jy) ? 1 : -1;
+                
+                    iip1 = jy;
+                    jjp1 = jx;
+        
+                       
+                        for (ii=0;ii<abs(di);ii++){
+                            iip2 = iip1 + step;
+                            jjp2 = (int)(0.5 + slope*(iip2-jy) + jx);
+                  
+                            phase2 = ephase[iip2+jjp2*nr];
+               
+                            if (phase1 != phase2) {
+                                z = (emag[iip1+jjp1*nr] + emag[iip2+jjp2*nr]);
+                                if (z > maxori){
+                                    maxori = z;
+                                }
+                            } 
+                     
+                            iip1 = iip2;
+                            jjp1 = jjp2;
+                            phase1 = phase2;
+                        }
+                    
+                    /* sample in j direction */    
+                } else { 
+                        slope = di / dj;
+                        step =  (ix>=jx) ? 1: -1;
+
+                    jjp1 = jx;
+                        iip1 = jy;                 
+            
+         
+                        for (jj=0;jj<abs(dj);jj++){
+                            jjp2 = jjp1 + step;
+                            iip2 = (int)(0.5+ slope*(jjp2-jx) + jy);
+                  
+                            phase2 = ephase[iip2+jjp2*nr];
+             
+                            if (phase1 != phase2){
+                                z = (emag[iip1+jjp1*nr] + emag[iip2+jjp2*nr]);
+                                if (z > maxori){ 
+                                    maxori = z; 
+                                }
+                                
+                            }
+          
+                            iip1 = iip2;
+                            jjp1 = jjp2;
+                            phase1 = phase2;
+                        }
+                }            
+            
+                maxori = 0.5 * maxori*a;
+                maxori = exp(-maxori*maxori);
+            }       
+                    ir[total] = i;
+
+                    w[total] = maxori + 0.005;
+                    total = total + 1;
+                        
+                } /* i */
+    } /* j */
+        
+    jc[np] = total;
+}  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib/TOOLBOX_calib.tar b/SD-VBS/common/toolbox/toolbox_basic/calib/TOOLBOX_calib.tar
new file mode 100755
index 0000000..92ab3a0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib/TOOLBOX_calib.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Distor2Calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Distor2Calib.m
new file mode 100755
index 0000000..a82f583
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/Multi_Calib_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Multi_Calib_oulu.m
new file mode 100755
index 0000000..62ca9ae
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Multi_Calib_oulu.m
@@ -0,0 +1,12 @@
+
+% enter image names, numbers, ...
+data_calib;
+
+%read images from files
+ima_read_calib;
+
+click_calib;
+
+%go_calib; % the original version
+
+go_calib_optim;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Rectangle2Square.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Rectangle2Square.m
new file mode 100755
index 0000000..a6bbbe5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/UnWarpPlane.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/UnWarpPlane.m
new file mode 100755
index 0000000..8addf52
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/add_suppress.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/add_suppress.m
new file mode 100755
index 0000000..b9bcc57
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/add_suppress.m
@@ -0,0 +1,91 @@
+   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/calib_bouguetj/analyse_error.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/analyse_error.m
new file mode 100755
index 0000000..5bfa3b5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/analyse_error.m
@@ -0,0 +1,104 @@
+% Color code for each image:
+
+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 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;
+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]\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;
+
+
+end;
+
+end;
+
+disp('done');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m
new file mode 100755
index 0000000..5b0fdac
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m
@@ -0,0 +1,74 @@
+if ~exist('instructions'), instructions = 1; end;
+
+if instructions,
+   
+fprintf(1,'\n');
+fprintf(1,'*----------------------------------------------------------------------------------------------------*\n');
+fprintf(1,'|                         Main Calibration toolbox (2D and 3D rigs)                                  |\n');
+fprintf(1,'|                        (c) Jean-Yves Bouguet - September 9th, 1999                                 |\n');
+fprintf(1,'*----------------------------------------------------------------------------------------------------*\n\n\n');
+
+fprintf(1,'CLICK ON:\n\n');
+
+fprintf(1,'2D:   To perform camera calibration from multiple views of a 2D planar grid. \n');
+fprintf(1,'      Set default size of grid (in dX_default and dY_default) in click_calib.m.\n');
+fprintf(1,'3D:   To perform camera calibration from multiple views of a 3D grid corner. \n');
+fprintf(1,'      Set default size of grids (in dX_default and dY_default) in click_calib3D.m.\n');
+fprintf(1,'Exit: To close the calibration tool. \n');
+
+end;
+
+instructions = 0;
+
+fig_number = 1;
+
+n_row = 1;
+n_col = 3;
+
+string_list = cell(n_row,n_col);
+callback_list = cell(n_row,n_col);
+
+x_size = 40;
+y_size = 20;
+
+title_figure = 'Calibration tool';
+
+string_list{1,1} = '2D rig';
+string_list{1,2} = '3D rig';
+string_list{1,3} = 'Exit';
+
+callback_list{1,1} = 'calib_gui;';
+callback_list{1,2} = 'calib3D_gui;';
+callback_list{1,3} = ['disp(''Bye. To run again, type calib.''); close(' num2str(fig_number) ');'];
+
+
+figure(fig_number); clf;
+pos = get(fig_number,'Position');
+
+fig_size_x = x_size*n_col+(n_col+1)*2;
+fig_size_y = y_size*n_row+(n_row+1)*2;
+
+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');
+
+
+for i=n_row:-1:1,
+   for j = n_col:-1:1,
+      if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})),
+        uicontrol('Parent',fig_number, ...
+                'Units','points', ...
+                                'Callback',callback_list{i,j}, ...
+                                'ListboxTop',0, ...
+                                'Position',[(2+(j-1)*(x_size+2))   (fig_size_y - i*(2+y_size))  x_size   y_size], ...
+                                'String',string_list{i,j}, ...
+            'Tag','Pushbutton1');
+      end;  
+        end;
+end;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m
new file mode 100755
index 0000000..ff24f6b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m
@@ -0,0 +1,115 @@
+if ~exist('instructions3D'), instructions3D = 1; end;
+
+if instructions3D,
+   
+fprintf(1,'\n');
+fprintf(1,'*----------------------------------------------------------------------------------------------------*\n');
+fprintf(1,'|           Canera calibration from multiple images of the Intel 3D calibration rig                  |\n');
+fprintf(1,'|                         (c) Jean-Yves Bouguet - September 2nd, 1999                                |\n');
+fprintf(1,'*----------------------------------------------------------------------------------------------------*\n\n\n');
+
+fprintf(1,'LIST OF CALIBRATION COMMANDS (to be executed from 1 to 5):\n\n');
+
+fprintf(1,'1- Image names:          Lets the user enter the file names of the calibration images (max = 30 images).\n');
+fprintf(1,'                         It includes basename, image type (''tif'', ''bmp'' or ''ras''), numbering scheme.\n');
+fprintf(1,'                         Automatically launchs the next step (Read images).\n');
+fprintf(1,'2- Read images:          Reads in the calibration images from files.\n');
+fprintf(1,'                         Does not automatically launch the next step (Extract grid corners).\n');
+fprintf(1,'3- Extract grid corners: Extracts the grid corners from the image.\n');
+fprintf(1,'                         Based six maual clicks per image.\n');
+fprintf(1,'                         The calibration data is saved under ''calib_data.mat''.\n');
+fprintf(1,'                         Automatically launchs the next step (Run calibration).\n');
+fprintf(1,'4- Run calibration:      Main calibration procedure.\n');
+fprintf(1,'                         Optimization of intrinsic and extrinsic parameters to minimize\n');
+fprintf(1,'                         the reprojection error (in the least squares sense.\n');
+fprintf(1,'                         Estimated parameters: 2 focal lengths, principal point,\n');
+fprintf(1,'                         radial (2 coeff. -> 4 degree model) and tangential (2 coeff.) distortion,\n');
+fprintf(1,'                         and extrinsic parameters (6 parameters per image).\n');
+fprintf(1,'                         The final solution is saved under ''Calib_Results.mat''.\n');
+fprintf(1,'                         For a description of the intrinsic camera model, refer to the reference:\n');
+fprintf(1,'                         "A Four-step Camera Calibration Procedure with implicit Image Correction"\n');
+fprintf(1,'                         Janne Heikkila and Olli Silven, Infotech Oulu and Department of EE\n');
+fprintf(1,'                         University of Oulu, Appeared in CVPR''97, Puerto Rico.\n');
+fprintf(1,'                         Visit http://www.ee.oulu.fi/~jth/calibr/Calibration.html\n');
+fprintf(1,'                         Automatically launchs the next step (Graphic out).\n');
+fprintf(1,'5- Graphic out:          Generates the graphical output associated to the current calibration solution.\n');
+fprintf(1,'                         It shows the 3D locations of the grids, and reprojects the 3D patterns on the\n');
+fprintf(1,'                         original calibration images.\n');
+fprintf(1,'6- sol. with center:     Lets the user select the calibration solution with computed principal point.\n');
+fprintf(1,'                         This is the default case (solution retained after Run calibration).\n');
+fprintf(1,'                         Automatically (re)generates the graphical output associated to that solution.\n');
+fprintf(1,'7- sol. without center:  Lets the users select the calibration solution without computed principal point.\n');
+fprintf(1,'                         In that case, the principal point is assumed at the center of the image.\n');
+fprintf(1,'                         Automatically generates the graphical output associated to that solution.\n');
+fprintf(1,'                         This option is sometimes useful when the principal point is difficult to\n');
+fprintf(1,'                         estimate (in particular when the camera field of view is small).\n');
+fprintf(1,'8- Back to main:         Goes back to the main calbration toolbox window.\n\n\n');
+
+end;
+
+instructions3D = 0;
+
+global X_1 x_1 X_2 x_2 X_3 x_3 X_4 x_4 X_5 x_5 X_6 x_6 X_7 x_7 X_8 x_8 X_9 x_9 X_10 x_10 X_11 x_11 X_12 x_12 X_13 x_13 X_14 x_14 X_15 x_15 X_16 x_16 X_17 x_17 X_18 x_18 X_19 x_19 X_20 x_20 X_21 x_21 X_22 x_22 X_23 x_23 X_24 x_24 X_25 x_25 X_26 x_26 X_27 x_27 X_28 x_28 X_29 x_29 X_30 x_30 
+
+
+fig_number = 1;
+
+n_row = 2;
+n_col = 4;
+
+string_list = cell(n_row,n_col);
+callback_list = cell(n_row,n_col);
+
+x_size = 85;
+y_size = 20;
+
+title_figure = 'Camera calibration tool (3D rig)';
+
+string_list{1,1} = 'Image names';
+string_list{1,2} = 'Read images';
+string_list{1,3} = 'Extract grid corners';
+string_list{1,4} = 'Run calibration';
+string_list{2,1} = 'Graphic out';
+string_list{2,2} = 'sol. with center';
+string_list{2,3} = 'sol. without center';
+string_list{2,4} = 'Back to main';
+
+callback_list{1,1} = 'data_calib;';
+callback_list{1,2} = 'ima_read_calib;';
+callback_list{1,3} = 'click_calib3D;';
+callback_list{1,4} = 'go_calib_optim3D;';
+callback_list{2,1} = 'graphout_calib3D;';
+callback_list{2,2} = 'select_sol_with_center3D;';
+callback_list{2,3} = 'select_sol_no_center3D;';
+callback_list{2,4} = 'calib;';
+
+
+figure(fig_number); clf;
+pos = get(fig_number,'Position');
+
+fig_size_x = x_size*n_col+(n_col+1)*2;
+fig_size_y = y_size*n_row+(n_row+1)*2;
+
+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');
+
+
+for i=n_row:-1:1,
+   for j = n_col:-1:1,
+      if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})),
+        uicontrol('Parent',fig_number, ...
+                'Units','points', ...
+                        'Callback',callback_list{i,j}, ...
+                        'ListboxTop',0, ...
+                        'Position',[(2+(j-1)*(x_size+2))   (fig_size_y - i*(2+y_size))  x_size   y_size], ...
+                        'String',string_list{i,j}, ...
+         'Tag','Pushbutton1');
+      end;
+        end;
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m
new file mode 100755
index 0000000..62a45dd
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m
@@ -0,0 +1,81 @@
+fig_number = 1;
+
+n_row = 5;
+n_col = 4;
+
+string_list = cell(n_row,n_col);
+callback_list = cell(n_row,n_col);
+
+x_size = 85;
+y_size = 20;
+
+title_figure = 'Camera calibration tool (2D rig)';
+
+string_list{1,1} = 'Image names';
+string_list{1,2} = 'Read images';
+string_list{1,3} = 'Extract grid corners';
+%string_list{1,4} = 'Initialization';
+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{5,1} = 'Comp. Extrinsic';
+string_list{5,2} = 'Undistort image';
+
+
+callback_list{1,1} = 'data_calib;';
+callback_list{1,2} = 'ima_read_calib;';
+callback_list{1,3} = 'click_calib;';
+%callback_list{1,4} = 'init_calib_param;';
+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{5,1} = 'extrinsic_computation;';
+callback_list{5,2} = 'undistort_image;';
+
+
+figure(fig_number); clf;
+pos = get(fig_number,'Position');
+
+fig_size_x = x_size*n_col+(n_col+1)*2;
+fig_size_y = y_size*n_row+(n_row+1)*2;
+
+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) ');']);
+
+
+for i=n_row:-1:1,
+   for j = n_col:-1:1,
+      if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})),
+        uicontrol('Parent',fig_number, ...
+                'Units','points', ...
+                                'Callback',callback_list{i,j}, ...
+                                'ListboxTop',0, ...
+                                'Position',[(2+(j-1)*(x_size+2))   (fig_size_y - i*(2+y_size))  x_size   y_size], ...
+                                'String',string_list{i,j}, ...
+            'Tag','Pushbutton1');
+        end;
+        end;
+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
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m
new file mode 100755
index 0000000..4f09b62
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m
@@ -0,0 +1,14 @@
+
+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);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m
new file mode 100755
index 0000000..8602c39
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m
@@ -0,0 +1,17 @@
+%%% Replay the set of solution vectors:
+
+N_iter = size(param_list,2);
+
+for nn = 1:N_iter,
+   
+   solution = param_list(:,nn);
+   
+   extract_parameters;
+   comp_error_calib;
+   
+   ext_calib;
+   
+   drawnow;
+   
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m
new file mode 100755
index 0000000..be0410b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m
@@ -0,0 +1,41 @@
+% Check the planarity of a structure:
+
+X = X_1;
+N = size(X,2);
+
+%X(3,:) = 0.1*randn(1,N);
+
+om = rand(3,1);
+T = 10*rand(3,1);
+R = rodrigues(om);
+
+X = R * X  + T*ones(1,N);
+
+
+
+
+
+
+N = size(X,2);
+X_mean = mean(X')';
+
+Y = X - (X_mean*ones(1,N));
+
+YY = Y*Y';
+
+[U,S,V] = svd(YY);
+
+r = S(3,3)/S(2,2);
+
+% if r is less than 1e-4:
+
+R_transform = V';
+T_transform = -(V')*X_mean;
+
+
+% Thresh for r: 1e-4
+
+X_new = R_transform*X + T_transform*ones(1,N);
+
+
+% If Xc = Rc * X_new + Tc, then Xc = Rc * R_transform * X + Tc + T_transform
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m
new file mode 100755
index 0000000..047cc7b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m
@@ -0,0 +1,99 @@
+
+if ~exist('I_1'),
+   ima_read_calib;
+   if no_image_file,
+      disp('Cannot extract corners without images');
+      return;
+   end;
+end;
+
+check_active_images;
+
+%wintx = 10; % neigborhood of integration for
+%winty = 10; % the corner finder
+
+fprintf(1,'\nExtraction of the grid corners on the images\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);
+
+if ~exist('map'), map = gray(256); end;
+
+
+disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!')
+
+
+% 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;
+
+% Useful option to add images:
+kk_first = input('Start image number ([]=1=first): ');
+
+if isempty(kk_first), kk_first = 1; end;
+
+for kk = kk_first:n_ima,
+   if active_images(kk),
+      click_ima_calib;
+   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;
+
+
+
+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/calib_bouguetj/click_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib3D.m
new file mode 100755
index 0000000..e761cd1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib3D.m
@@ -0,0 +1,79 @@
+
+if ~exist('I_1'),
+   ima_read_calib;
+   if no_image_file,
+      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');
+
+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('WARNNG!!! Do not forget to change dX_default and dY_default in click_calib.m!!!')
+
+
+% 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;
+
+% Useful option to add images:
+kk_first = input('Start image number ([]=1=first): ');
+
+if isempty(kk_first), kk_first = 1; end;
+
+for kk = kk_first:n_ima,
+   click_ima_calib3D; %Simple version
+   %init_calib; %advanced vesion (more messy)
+end;
+
+
+
+string_save = 'save calib_data 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) ' Hl_' num2str(kk) ' nl_sq_x_' num2str(kk) ' nl_sq_y_' num2str(kk) ' Hr_' num2str(kk) ' nr_sq_x_' num2str(kk) ' nr_sq_y_' num2str(kk)];
+end;
+
+eval(string_save);
+
+go_calib_optim3D;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m
new file mode 100755
index 0000000..5197870
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m
@@ -0,0 +1,218 @@
+        % Cleaned-up version of init_calib.m
+   
+   fprintf(1,'\nProcessing image %d...\n',kk);
+   
+        eval(['I = I_' num2str(kk) ';']);
+        
+        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) '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;
+   
+   % 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)
+   
+   
+   % Global Homography from plane to pixel coordinates:
+   
+ 
+   
+   
+   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/calib_bouguetj/click_ima_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib3D.m
new file mode 100755
index 0000000..7718268
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/comp_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion.m
new file mode 100755
index 0000000..a0f03de
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/comp_distortion2.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion2.m
new file mode 100755
index 0000000..532ee9a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/comp_distortion_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion_oulu.m
new file mode 100755
index 0000000..67d02d5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/comp_error_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_error_calib.m
new file mode 100755
index 0000000..f8d6fde
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_error_calib.m
@@ -0,0 +1,40 @@
+%%%%%%%%%%%%%%%%%%%% 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
+
+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) '  = project2_oulu(X_' num2str(kk) ',Rkk,Tckk,fc,cc,kc);']);
+      
+      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);']);
+
+           eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']);
+      
+   end;
+   
+end;
+
+err_std = std(ex')';
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_collineation.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_collineation.m
new file mode 100755
index 0000000..809c309
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/compute_extrinsic.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic.m
new file mode 100755
index 0000000..4b4d7dd
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic.m
@@ -0,0 +1,123 @@
+function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,MaxIter,thresh_cond),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,refine)
+%
+%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
+%       refine: set to 1 for refining the extrinsic parameters iteratively
+%               [OPTIONAL: Default value: 1]
+%
+%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 < 7,
+   thresh_cond = inf;
+end;
+
+
+if nargin < 6,
+   MaxIter = 20;
+end;
+
+
+
+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;
+
+
+% Initialization:
+
+[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc);
+
+% Refinement:
+
+[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,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_points(X_kk,omckk,Tckk,fc,cc,kc);
+
+ex = x_kk - x;
+
+
+% Converts the homography in pixel units:
+
+KK = [fc(1) 0 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/calib_bouguetj/compute_extrinsic_init.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_init.m
new file mode 100755
index 0000000..207ea30
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_init.m
@@ -0,0 +1,149 @@
+function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc)
+%
+%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
+%
+%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 < 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;
+
+
+
+% Compute the normalized coordinates:
+
+xn = normalize(x_kk,fc,cc,kc);
+
+
+
+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/calib_bouguetj/compute_extrinsic_refine.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_refine.m
new file mode 100755
index 0000000..69474c4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_refine.m
@@ -0,0 +1,110 @@
+function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,MaxIter,thresh_cond),
+
+%compute_extrinsic
+%
+%[omckk,Tckk,Rckk] = compute_extrinsic_refine(x_kk,X_kk,fc,cc,kc,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
+%       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 < 9,
+   thresh_cond = inf;
+end;
+
+
+if nargin < 8,
+   MaxIter = 20;
+end;
+
+
+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;
+
+
+% 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_points(X_kk,omckk,Tckk,fc,cc,kc);
+      
+   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/calib_bouguetj/compute_homography.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_homography.m
new file mode 100755
index 0000000..fcc9003
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/convert_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/convert_oulu.m
new file mode 100755
index 0000000..726806e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/convert_oulu.m
@@ -0,0 +1,35 @@
+%% Converts data file from oulu to mine:
+
+load cademo,
+n_ima = 0;
+
+no_error = 1;
+
+ii = 1;
+
+while no_error,
+   
+   dataname = ['data' num2str(ii)];
+   
+   if exist(dataname),
+      
+      n_ima = n_ima +1;
+      
+      eval(['x_' num2str(ii) '= ' dataname '(:,4:5)'';'])
+      eval(['X_' num2str(ii) '= ' dataname '(:,1:3)'';'])
+      
+   else
+      no_error = 0;
+   end;
+   
+   ii = ii + 1;
+   
+end;
+
+nx = 500;
+ny = 500;
+
+no_image = 1;
+no_grid = 1;
+
+save data  n_ima x_1 X_1 x_2 X_2 x_3 X_3 nx ny no_image no_grid
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/cornerfinder.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/cornerfinder.m
new file mode 100755
index 0000000..9bfa51f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/count_squares.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/count_squares.m
new file mode 100755
index 0000000..0e226c0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/data_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/data_calib.m
new file mode 100755
index 0000000..318ec15
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/data_calib.m
@@ -0,0 +1,89 @@
+%%% This script alets the user enter the name of the images (base name, numbering scheme,...
+
+dir;
+
+%disp('Camera Calibration using multiple images of a planar checkerboard pattern');
+%disp('Model: 2 focals, 2 radial dist. coeff., 2 tangential dist. coeff. and principle point');
+%disp('       => 8DOF intrinsic model ([Heikkila and Silven, University of Oulu])');
+
+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'') ','s');
+
+        if isempty(format_image),
+        format_image = 'ras';
+        end;
+
+        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;
+
+
+n_ima = 1000;
+while n_ima > 30,
+   n_ima = input('Number of calibration images: ');
+   n_ima = round(n_ima);
+end;
+
+type_numbering = input('Type of numbering (ex: []=4,other=04): ');
+
+type_numbering = ~isempty(type_numbering);
+
+if type_numbering,
+   
+   N_slots = input('Number of spaces for numbers? (ex: 2 -> 04, 3 -> 004), ([]=3) ');
+
+   if isempty(N_slots), N_slots = 3; end;
+
+else
+   
+   N_slots = 1; % not used anyway, but useful for saving
+   
+end;
+
+
+first_num = input('First image number? (0,1,2...) ([]=0) ');
+ 
+if isempty(first_num), first_num = 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);
+
+%string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num';
+
+%eval(string_save);
+
+% Reading images:
+
+ima_read_calib;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/error_analysis.m
new file mode 100755
index 0000000..85feac5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/ext_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ext_calib.m
new file mode 100755
index 0000000..d41d068
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ext_calib.m
@@ -0,0 +1,130 @@
+
+%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%%
+
+check_active_images;
+
+if ~exist(['omc_' num2str(ind_active(1))]),
+   fprintf(1,'Need to calibrate before showing extrinsic results. Maybe need to load Calib_Results.mat file.\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);
+      
+        eval(['XX_kk = X_' num2str(kk) ';']);
+        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;
+         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;
+
+figure(4);rotate3d on;
+axis('equal');
+title('Extrinsic parameters');
+%view(60,30);
+view(a,b);
+hold off;
+
+set(4,'Name','3D','NumberTitle','off');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_grid.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_grid.m
new file mode 100755
index 0000000..1e3cbdb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_grid.m
@@ -0,0 +1,227 @@
+function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc);
+
+map = gray(256);
+
+        figure(2);
+        image(I);
+   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;
+   
+   
+   % Enter the size of each square
+   
+   dX = input(['Size dX of each square along the X direction ([]=3cm) = ']);
+        dY = input(['Size dY of each square along the Y direction ([]=3cm) = ']);
+        if isempty(dX), dX = 3; end;
+        if isempty(dY), 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_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(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;
+   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters.m
new file mode 100755
index 0000000..8e0e1f1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters.m
@@ -0,0 +1,46 @@
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+check_active_images;
+
+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,
+   
+   if active_images(kk),   
+      
+      omckk = solution(4+6*(kk-1) + 3:6*kk + 3);   
+      Tckk = solution(6*kk+1 + 3:6*kk+3 + 3);
+      
+        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/calib_bouguetj/extract_parameters3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters3D.m
new file mode 100755
index 0000000..841c6ab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/extrinsic_computation.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extrinsic_computation.m
new file mode 100755
index 0000000..8cf10db
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extrinsic_computation.m
@@ -0,0 +1,173 @@
+%%% INPUT THE IMAGE FILE NAME:
+
+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'') ','s');
+
+        if isempty(format_image2),
+        format_image2 = 'ras';
+        end;
+
+        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;
+
+ima_name = [image_name '.' format_image];
+
+
+
+%%% READ IN IMAGE:
+
+if format_image(1) == 'p',
+   I = double(pgmread(ima_name));
+else
+   if format_image(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);
+
+
+%%% Reproject the points on the image:
+
+[x_reproj] = project_points(X_ext,omc_ext,Tc_ext,fc,cc,kc);
+
+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_points(Basis,omc_ext,Tc_ext,fc,cc,kc);
+
+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/calib_bouguetj/ginput3.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ginput3.m
new file mode 100755
index 0000000..56fe496
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/go_calib_optim.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim.m
new file mode 100755
index 0000000..6eb1c82
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim.m
@@ -0,0 +1,60 @@
+%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
+
+
+desactivated_images = [];
+
+
+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/calib_bouguetj/go_calib_optim3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim3D.m
new file mode 100755
index 0000000..8cc5e30
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim3D.m
@@ -0,0 +1,264 @@
+% Simplified version of go_calib.m
+
+
+if ~exist('x_1'),
+   click_calib;
+end;
+
+
+fprintf(1,'\nMain calibration procedure\n');
+
+% initial guess for principal point and distortion:
+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 lentgh 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,
+   
+   % left Pattern:
+   
+   eval(['Hlkk = Hl_' num2str(kk) ';']);
+   
+   Hlkk = Sub_cc * Hlkk;
+
+   % Extract vanishing points (direct and diagonals):
+   
+   Vl_hori_pix = Hlkk(:,1);
+   Vl_vert_pix = Hlkk(:,2);
+   Vl_diag1_pix = (Hlkk(:,1)+Hlkk(:,2))/2;
+   Vl_diag2_pix = (Hlkk(:,1)-Hlkk(:,2))/2;
+   
+   Vl_hori_pix = Vl_hori_pix/norm(Vl_hori_pix);
+   Vl_vert_pix = Vl_vert_pix/norm(Vl_vert_pix);
+   Vl_diag1_pix = Vl_diag1_pix/norm(Vl_diag1_pix);
+   Vl_diag2_pix = Vl_diag2_pix/norm(Vl_diag2_pix);
+   
+   al1 = Vl_hori_pix(1);
+   bl1 = Vl_hori_pix(2);
+   cl1 = Vl_hori_pix(3);
+   
+   al2 = Vl_vert_pix(1);
+   bl2 = Vl_vert_pix(2);
+   cl2 = Vl_vert_pix(3);
+   
+   al3 = Vl_diag1_pix(1);
+   bl3 = Vl_diag1_pix(2);
+   cl3 = Vl_diag1_pix(3);
+   
+   al4 = Vl_diag2_pix(1);
+   bl4 = Vl_diag2_pix(2);
+   cl4 = Vl_diag2_pix(3);
+   
+   % right Pattern:
+   
+   eval(['Hrkk = Hr_' num2str(kk) ';']);
+   
+   Hrkk = Sub_cc * Hrkk;
+
+   % Extract vanishing points (direct and diagonals):
+   
+   Vr_hori_pix = Hrkk(:,1);
+   Vr_vert_pix = Hrkk(:,2);
+   Vr_diag1_pix = (Hrkk(:,1)+Hrkk(:,2))/2;
+   Vr_diag2_pix = (Hrkk(:,1)-Hrkk(:,2))/2;
+   
+   Vr_hori_pix = Vr_hori_pix/norm(Vl_hori_pix);
+   Vr_vert_pix = Vr_vert_pix/norm(Vl_vert_pix);
+   Vr_diag1_pix = Vr_diag1_pix/norm(Vr_diag1_pix);
+   Vr_diag2_pix = Vr_diag2_pix/norm(Vr_diag2_pix);
+   
+   ar1 = Vr_hori_pix(1);
+   br1 = Vr_hori_pix(2);
+   cr1 = Vr_hori_pix(3);
+   
+   ar2 = Vr_vert_pix(1);
+   br2 = Vr_vert_pix(2);
+   cr2 = Vr_vert_pix(3);
+   
+   ar3 = Vr_diag1_pix(1);
+   br3 = Vr_diag1_pix(2);
+   cr3 = Vr_diag1_pix(3);
+   
+   ar4 = Vr_diag2_pix(1);
+   br4 = Vr_diag2_pix(2);
+   cr4 = Vr_diag2_pix(3);
+   
+   
+   % Collect all the constraints:
+   
+   A_kk = [al1*al2  bl1*bl2;
+      al3*al4  bl3*bl4;
+      ar1*ar2  br1*br2;
+      ar3*ar4  br3*br4;
+      al1*ar1  bl1*br1];
+   
+   b_kk = -[cl1*cl2;cl3*cl4;cr1*cr2;cr3*cr4;cl1*cr1];
+   
+
+   A = [A;A_kk];
+   b = [b;b_kk];
+   
+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
+
+%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) 0 c_init(1);0 f_init(2) c_init(2); 0 0 1];
+inv_KK = inv(KK);
+
+        
+% Computing of the extrinsic parameters (from the collineations)
+
+for kk = 1:n_ima,
+   
+   eval(['Hlkk = Hl_' num2str(kk) ';']);
+   
+   Hl2 = inv_KK*Hlkk;
+      
+   sc = mean([norm(Hl2(:,1));norm(Hl2(:,2))]);
+   
+        Hl2 = Hl2/sc;
+    
+   eval(['Hrkk = Hr_' num2str(kk) ';']);
+   
+   Hr2 = inv_KK*Hrkk;
+      
+   sc = mean([norm(Hr2(:,1));norm(Hr2(:,2))]);
+   
+   Hr2 = Hr2/sc;
+   
+   omcl = rodrigues([Hl2(:,1:2) cross(Hl2(:,1),Hl2(:,2))]);
+   Tcl = Hl2(:,3);
+   
+   %omcr = rodrigues([Hr2(:,1:2) cross(Hr2(:,1),Hr2(:,2))]);
+   %Tcr = Hr2(:,3);
+   
+   
+        omckk = omcl; %rodrigues([H2(:,1:2) cross(H2(:,1),H2(:,2))]);
+        Tckk = Tcl; %H2(:,3);
+   
+        eval(['omc_' num2str(kk) ' = omckk;']);
+        eval(['Tc_' num2str(kk) ' = Tckk;']);
+        
+end;
+
+
+      
+% Initialisation of the parameters for global minimization:
+
+init_param = [f_init;k_init];
+
+for kk = 1:n_ima,   
+   eval(['init_param = [init_param; omc_' num2str(kk) '; Tc_' num2str(kk) '];']);
+end;
+
+if ~exist('lsqnonlin'),
+
+        options = [1 1e-4 1e-4 1e-6  0 0 0 0 0 0 0 0 0 6000 0 1e-8 0.1 0];
+
+        if exist('leastsq'),
+        sss = ['[param,opt] = leastsq(''multi_error_oulu'',init_param,options,[],n_ima,c_init);'];
+        else
+        sss = ['[param,opt] = leastsq2(''multi_error_oulu'',init_param,options,[],n_ima,c_init);'];
+        end;
+
+else
+   
+   options = optimset('lsqnonlin');
+   options.MaxIter  = 6000;
+   options.Display = 'iter';
+        sss = ['[param,opt] = lsqnonlin(''multi_error_oulu'',init_param,[],[],options,n_ima,c_init);'];
+
+end;
+
+
+fprintf(1,'\nOptimization not including the principal point...\n')
+eval(sss);
+
+history  = [[init_param;c_init] [param;c_init]];
+
+sol_no_center = [param;c_init];
+
+init_param = sol_no_center;
+
+fprintf(1,'\nOptimization including the principal point...\n')
+
+eval(sss);
+
+history = [history param];
+
+
+sol_with_center = param;
+
+
+
+
+%%% Extraction of the final intrinsic and extrinsic paramaters (in the no-center case):
+
+solution = sol_no_center;
+extract_parameters3D;
+
+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:      [click on ''sol. without center'']\n']); 
+
+
+
+
+% Pick the solution with principal point
+%%% NOTE: At that point, the user can choose which solution to pick: with or without
+%%% principal point estimation. By default, we pick the solution with principal point.
+
+solution = sol_with_center;
+
+
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters3D;
+
+
+fprintf(1,'\n\nCalibration results with 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);   
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+graphout_calib3D;
+
+
+
+fprintf(1,'Note: If the solution is not satisfactory, select solution without center estimation.\n\n');
+
+
+%%%%%%%%%%%%%% Save all the Calibration results:
+
+disp('Save calibration results under Calib_Results.mat');
+
+string_save = 'save Calib_Results fc kc cc ex x y solution sol_with_center sol_no_center history 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) ' Tc_' num2str(kk) ' Hl_' num2str(kk) ' nl_sq_x_' num2str(kk) ' nl_sq_y_' num2str(kk) ' Hr_' num2str(kk) ' nr_sq_x_' num2str(kk) ' nr_sq_y_' num2str(kk)];
+end;
+
+eval(string_save);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_cont.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_cont.m
new file mode 100755
index 0000000..9ff3f0b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_cont.m
@@ -0,0 +1,142 @@
+% Simplified version of go_calib.m
+
+if ~exist('x_1'),
+   click_calib;
+end;
+      
+
+% Initialisation of the parameters for global minimization:
+
+init_param = [fc;kc];
+
+for kk = 1:n_ima,
+   
+   if ~exist(['omc_' num2str(kk)]),
+      eval(['Hkk = H_' num2str(kk) ';']);
+        H2 = inv_KK*Hkk;
+      sc = mean([norm(H2(:,1));norm(H2(:,2))]);
+        H2 = H2/sc;
+      omckk = rodrigues([H2(:,1:2) cross(H2(:,1),H2(:,2))]);
+      Tckk = H2(:,3);
+      eval(['omc_' num2str(kk) ' = omckk;']);
+      eval(['Tc_' num2str(kk) ' = Tckk;']);
+   end;
+   
+   eval(['init_param = [init_param; omc_' num2str(kk) '; Tc_' num2str(kk) '];']);
+end;
+
+init_param = [init_param;cc];
+
+
+
+%-------------------- Main Optimization:
+
+fprintf(1,'\nRe-Optimization...\n')
+
+
+param = init_param;
+change = 1;
+
+iter = 0;
+
+fprintf(1,'Iteration ');
+
+while (change > 1e-6)&(iter < 10),
+   
+   fprintf(1,'%d...',iter+1);
+
+        JJ = [];
+        ex = [];
+        
+        c = param(6*n_ima + 4 + 3:6*n_ima + 5 + 3);
+        f = param(1:2);
+        k = param(3:6);
+        
+        for kk = 1:n_ima,
+         
+        omckk = param(4+6*(kk-1) + 3:6*kk + 3);
+        
+                Tckk = param(6*kk+1 + 3:6*kk+3 + 3);
+        
+        eval(['X_kk = X_' num2str(kk) ';']);
+        eval(['x_kk = x_' num2str(kk) ';']);
+        
+        Np = size(X_kk,2);
+        
+        JJkk = zeros(2*Np,n_ima * 6 + 8);
+        
+        [x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X_kk,omckk,Tckk,f,c,k);
+      
+        exkk = x_kk - x;
+   
+        ex = [ex;exkk(:)];
+   
+        JJkk(:,1:2) = dxdf;
+        JJkk(:,3:6) = dxdk;
+        JJkk(:,4+6*(kk-1) + 3:6*kk + 3) = dxdom;
+        JJkk(:,6*kk+1 + 3:6*kk+3 + 3) = dxdT;
+        JJkk(:,6*n_ima + 4 + 3:6*n_ima + 5 + 3) = dxdc;
+        
+        JJ = [JJ;JJkk];
+        
+        end;
+        
+        param_innov = inv(JJ'*JJ)*(JJ')*ex;
+        param_up = param + param_innov;
+        change = norm(param_innov)/norm(param_up);
+        param = param_up;
+        iter = iter + 1;
+   
+end;
+
+fprintf(1,'\n');
+
+
+sol_with_center = param;
+
+solution = sol_with_center;
+
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters;
+comp_error_calib;
+
+fprintf(1,'\n\nCalibration results with 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); 
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+graphout_calib;
+
+
+
+fprintf(1,'Note: If the solution is not satisfactory, select solution without center estimation.\n\n');
+
+
+%%%%%%%%%%%%%% Save all the Calibration results:
+
+disp('Save calibration results under Calib_Results.mat');
+
+string_save = 'save Calib_Results fc kc cc ex x y solution sol_with_center solution_init history 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) ' Hini_' 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);
+
+return;
+
+if exist('calib_data.mat'),
+   ccc = computer;
+   if ccc(1)=='P',
+      eval('!del calib_data.mat');
+   else
+      eval('!rm calib_data.mat');
+   end;
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_iter.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_iter.m
new file mode 100755
index 0000000..a076214
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_iter.m
@@ -0,0 +1,332 @@
+%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
+
+
+check_active_images;
+
+
+
+quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)
+
+
+
+% 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 = 0.4; % set alpha = 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];
+end;
+
+
+if ~exist('kc'),
+   fprintf(1,'Initialization of the image distortion to zero.\n');
+   kc = zeros(4,1);
+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);
+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)!
+end;
+
+
+
+%% Initialization of the extrinsic parameters for global minimization:
+
+init_param = [fc;kc];
+
+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);
+         [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,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;']);
+   
+   eval(['init_param = [init_param; omc_' num2str(kk) '; Tc_' num2str(kk) '];']);
+   
+end;
+
+
+check_active_images;
+
+init_param = [init_param;cc];
+
+%-------------------- Main Optimization:
+
+fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));
+
+
+% The following vector helps to select the variables to update:
+ind_Jac = find([ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1);ones(2,1)])';
+
+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 = [];
+        
+        c = param(6*n_ima + 4 + 3:6*n_ima + 5 + 3);
+        f = param(1:2);
+        k = param(3:6);
+        
+   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);
+         
+         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 + 8);
+        
+                [x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X_kk,omckk,Tckk,f,c,k);
+      
+                exkk = x_kk - x;
+   
+                ex = [ex;exkk(:)];
+   
+                JJkk(:,1:2) = dxdf;
+                JJkk(:,3:6) = dxdk;
+                JJkk(:,4+6*(kk-1) + 3:6*kk + 3) = dxdom;
+                JJkk(:,6*kk+1 + 3:6*kk+3 + 3) = dxdT;
+                JJkk(:,6*n_ima + 4 + 3:6*n_ima + 5 + 3) = dxdc;
+                        
+         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(4+6*(kk-1) + 3:6*kk+3 + 3) = NaN*ones(6,1);
+         end;
+            
+         
+      end;
+        
+   end;
+   
+   
+   % List of active images (necessary if changed):
+   check_active_images;
+   ind_Jac = find([ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1);ones(2,1)])';
+   
+   
+   JJ = JJ(:,ind_Jac);
+   
+   JJ2 = JJ'*JJ;
+   
+   
+   % Smoothing coefficient:
+   
+   alpha2 = 1-(1-alpha)^(iter+1); %set to 1 to undo any smoothing!
+   
+   
+   param_innov = alpha2*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(6*n_ima + 4 + 3:6*n_ima + 5 + 3);
+   kc_current = param(3:6);
+   
+   
+   % 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(4+6*(kk-1) + 3:6*kk + 3);
+         Tc_current = param(6*kk+1 + 3:6*kk+3 + 3);
+        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);
+         [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_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(4+6*(kk-1) + 3:6*kk + 3) = omckk;
+         param(6*kk+1 + 3:6*kk+3 + 3) = Tckk;
+      end;
+   end;
+   
+   %fprintf(1,'\n\nCalibration results after optimization:\n\n');
+   %fprintf(1,'focal      = [%3.5f   %3.5f]\n',fc_current);
+   %fprintf(1,'center     = [%3.5f   %3.5f]\n',cc_current);
+   %fprintf(1,'distortion = [%3.5f   %3.5f  %3.5f   %3.5f]\n\n',kc_current);   
+   
+   
+   param_list = [param_list param];
+   
+   iter = iter + 1;
+   
+end;
+
+fprintf(1,'\n');
+
+
+sol_with_center = param;
+
+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,'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/calib_bouguetj/graphout_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib.m
new file mode 100755
index 0000000..a3f7040
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib.m
@@ -0,0 +1,12 @@
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%%
+
+ext_calib;
+
+%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%%
+
+reproject_calib;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib3D.m
new file mode 100755
index 0000000..b7edf43
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib3D.m
@@ -0,0 +1,153 @@
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% Color code for each image:
+
+colors = 'brgkcm';
+
+
+%%% Show the extrinsic parameters
+
+IP = 8*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));
+
+
+figure(4);
+[a,b] = view;
+
+figure(4);
+plot3(5*[0 dX 0 0 0 0 ],5*[0 0 0 0 0 dX],-5*[0 0 0 dX 0 0 ],'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,
+   
+   eval(['XX_kk = X_' num2str(kk) ';']);
+   eval(['omc_kk = omc_' num2str(kk) ';']);
+   eval(['Tc_kk = Tc_' num2str(kk) ';']);
+   
+   eval(['nl_sq_x = nl_sq_x_' num2str(kk) ';']);
+   eval(['nl_sq_y = nl_sq_y_' num2str(kk) ';']);
+   
+   eval(['nr_sq_x = nr_sq_x_' num2str(kk) ';']);
+   eval(['nr_sq_y = nr_sq_y_' num2str(kk) ';']);
+
+   R_kk = rodrigues(omc_kk);
+   
+   YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk));
+   
+   YYl_kk = YY_kk(:,1:(nl_sq_x+1)*(nl_sq_y+1));
+   YYr_kk = YY_kk(:,(nl_sq_x+1)*(nl_sq_y+1)+1:end);
+
+   
+   eval(['YYl_' num2str(kk) ' = YYl_kk;']);
+   eval(['YYr_' num2str(kk) ' = YYr_kk;']);
+   
+   uu = [-dX;-dY;0]/2;
+   uu = R_kk * uu + Tc_kk; 
+   
+   YYlx = zeros(nl_sq_x+1,nl_sq_y+1);
+   YYly = zeros(nl_sq_x+1,nl_sq_y+1);
+   YYlz = zeros(nl_sq_x+1,nl_sq_y+1);
+   
+   YYrx = zeros(nr_sq_x+1,nr_sq_y+1);
+   YYry = zeros(nr_sq_x+1,nr_sq_y+1);
+   YYrz = zeros(nr_sq_x+1,nr_sq_y+1);
+   
+   YYlx(:) = YYl_kk(1,:);
+   YYly(:) = YYl_kk(2,:);
+   YYlz(:) = YYl_kk(3,:);
+   
+   YYrx(:) = YYr_kk(1,:);
+   YYry(:) = YYr_kk(2,:);
+   YYrz(:) = YYr_kk(3,:);
+   
+   
+   %keyboard;
+   
+   figure(4);
+   hhh= mesh(YYlx,YYlz,-YYly);
+   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)]);
+   hhh= mesh(YYrx,YYrz,-YYry);
+   set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1);
+   text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1));
+   
+end;
+
+figure(4);rotate3d on;
+axis('equal');
+title('Extrinsic parameters');
+%view(60,30);
+view(a,b);
+hold off;
+
+
+
+% Reproject the patterns on the images, and compute the pixel errors:
+
+% Reload the images if necessary
+
+if ~exist('I_1'),
+   ima_read_calib;
+   if no_image_file,
+      return;
+   end;
+end;
+
+
+ex = []; % Global error vector
+x = []; % Detected corners on the image plane
+y = []; % Reprojected points
+
+for kk = 1:n_ima,
+   
+   eval(['omckk = omc_' num2str(kk) ';']);
+   eval(['Tckk = Tc_' num2str(kk) ';']);
+   
+   Rkk = rodrigues(omckk);
+   
+   eval(['y_' num2str(kk) '  = project2_oulu(X_' num2str(kk) ',Rkk,Tckk,fc,cc,kc);']);
+
+   eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' -y_' num2str(kk) ';']);
+   
+   eval(['x_kk = x_' num2str(kk) ';']);
+   
+   figure(4+kk);
+   eval(['I = I_' num2str(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;
+   hold off;
+   
+   
+   eval(['ex = [ex ex_' num2str(kk) '];']);
+   eval(['x = [x x_' num2str(kk) '];']);
+   eval(['y = [y y_' num2str(kk) '];']);
+   
+end;
+
+
+figure(5+n_ima);
+for kk = 1:n_ima,
+   eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']);
+   hold on;
+end;
+hold off;
+axis('equal');
+title('Reprojection error (in pixel)');
+xlabel('x');
+ylabel('y');
+
+err_std = std(ex')';
+
+fprintf(1,'Pixel error:      err = [ %3.5f   %3.5f]\n\n',err_std); 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ima_read_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ima_read_calib.m
new file mode 100755
index 0000000..09cef59
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ima_read_calib.m
@@ -0,0 +1,107 @@
+
+if ~exist('calib_name'),
+   data_calib;
+end;
+
+check_active_images;
+
+images_read = active_images;
+
+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',
+                Ii = double(pgmread(ima_name));
+        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...',i);
+
+                        images_read(i) = 0;
+
+                        no_image_file = 1;
+         
+                end;
+      
+   end;
+   
+   i = i+1;   
+   
+end;
+
+
+if no_image_file,
+   
+   fprintf(1,'\nWARNING! Cannot load calibration images\n');
+   
+else
+   
+   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;
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_calib_param.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_calib_param.m
new file mode 100755
index 0000000..92e14be
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_calib_param.m
@@ -0,0 +1,210 @@
+%init_calib_param
+%
+%Initialization of the intrinsic and extrinsic parameters.
+%Runs as a script.
+%
+%This function is obsolete with init_intrinsic_param called from go_calib_optim
+%
+%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: Compute 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])
+%        Once the intrinsic parameters are estimated, the extrinsic parameters
+%        are computed for each image.
+%
+%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 functions called within that program:
+%
+%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
+%
+%compute_extrinsic.m: Computes the location of a grid assuming known intrinsic parameters.
+%                      This function is called at the initialization step.
+
+
+
+
+check_active_images;
+
+if ~exist(['x_' num2str(ind_active(1)) ]),
+   click_calib;
+end;
+
+
+fprintf(1,'\nInitialization of the calibration 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
+
+%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) 0 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;
+
+
+% Computing of the extrinsic parameters (from the collineations)
+
+for kk = 1:n_ima,
+   
+   if active_images(kk),
+      
+      
+        eval(['x_kk = x_' num2str(kk) ';']);
+      eval(['X_kk = X_' num2str(kk) ';']);
+      
+      [omckk,Tckk] = compute_extrinsic(x_kk,X_kk,fc,cc,kc);
+      
+      Rckk = rodrigues(omc_kk);
+         
+   else
+      
+      omckk = NaN*ones(3,1);
+      Tckk = NaN*ones(3,1);
+      Rckk = NaN*ones(3,3);
+      
+        end;
+
+   eval(['omc_' num2str(kk) ' = omckk;']);
+   eval(['Rc_' num2str(kk) ' = Rckk;']);
+   eval(['Tc_' num2str(kk) ' = Tckk;']);
+      
+end;
+
+
+% Initialization of the parameters for global minimization:
+
+init_param = [f_init;k_init];
+
+for kk = 1:n_ima,   
+   eval(['init_param = [init_param; omc_' num2str(kk) '; Tc_' num2str(kk) '];']);
+end;
+
+solution_init = [init_param;c_init];
+
+solution = solution_init;
+
+comp_error_calib;
+
+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,'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); 
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+%graphout_calib;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_intrinsic_param.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_intrinsic_param.m
new file mode 100755
index 0000000..eac21ba
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_intrinsic_param.m
@@ -0,0 +1,153 @@
+%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
+%        KK: The camera matrix (containing fc and cc)
+%
+%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 onyl 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
+
+%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) 0 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;
+
+
+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,'Distortion:       kc = [ %3.5f   %3.5f   %3.5f   %3.5f]\n\n',kc);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/is3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/is3D.m
new file mode 100755
index 0000000..ab00b3d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/loading_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loading_calib.m
new file mode 100755
index 0000000..a0f50d2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/loadinr.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadinr.m
new file mode 100755
index 0000000..91b6f89
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/loadpgm.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadpgm.m
new file mode 100755
index 0000000..9386111
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/loadppm.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadppm.m
new file mode 100755
index 0000000..6dd7ca4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadppm.m
@@ -0,0 +1,101 @@
+%LOADPPM        Load a PPM image
+%
+%       [R,G,B] = 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 [R,G,B] = 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);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/mean_std_robust.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/mean_std_robust.m
new file mode 100755
index 0000000..0d18a62
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/multi_error_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/multi_error_oulu.m
new file mode 100755
index 0000000..8657158
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/multi_error_oulu.m
@@ -0,0 +1,49 @@
+function ex = multi_error_oulu(param,n_ima,cc);
+
+global X_1 x_1 X_2 x_2 X_3 x_3 X_4 x_4 X_5 x_5 X_6 x_6 X_7 x_7 X_8 x_8 X_9 x_9 X_10 x_10 X_11 x_11 X_12 x_12 X_13 x_13 X_14 x_14 X_15 x_15 X_16 x_16 X_17 x_17 X_18 x_18 X_19 x_19 X_20 x_20 X_21 x_21 X_22 x_22 X_23 x_23 X_24 x_24 X_25 x_25 X_26 x_26 X_27 x_27 X_28 x_28 X_29 x_29 X_30 x_30
+
+fc = param(1:2);
+kc = param(3:6);
+%ppc = param(5:6);
+
+if length(param) > 6*n_ima + 3 + 3,
+   
+   cc = param(6*n_ima + 4 + 3:6*n_ima + 5 + 3);
+   
+   if length(param) > 6*n_ima + 5 + 3,
+      
+      c_d = param(6*n_ima + 6 + 3 :6*n_ima + 7 + 3);
+      
+   else
+      
+      c_d = [0;0];
+      
+   end;
+   
+else
+   
+   c_d = [0;0];
+
+end;
+
+
+
+ex = [];
+
+%keyboard;
+
+for kk = 1:n_ima,
+   
+   omckk = param(4+6*(kk-1) + 3:6*kk + 3);
+   
+   Tckk = param(6*kk+1 + 3:6*kk+3 + 3);
+
+   Rkk = rodrigues(omckk);
+
+   eval(['ykk = project2_oulu(X_' num2str(kk) ',Rkk,Tckk,fc,cc,kc);']);
+   
+   eval(['exkk = x_' num2str(kk) ' -ykk;']);
+
+   ex = [ex;exkk(:)];
+   
+end;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/normalize.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/normalize.m
new file mode 100755
index 0000000..0a37378
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/normalize.m
@@ -0,0 +1,32 @@
+function [xn] = normalize(x_kk,fc,cc,kc),
+
+%normalize
+%
+%[xn] = normalize(x_kk,fc,cc,kc)
+%
+%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
+%
+%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix)
+%
+%Important functions called within that program:
+%
+%comp_distortion_oulu: undistort pixel coordinates.
+
+
+
+% First subtract principal point, and divide by the focal length:
+
+x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)];
+
+
+%Compensate for lens distortion:
+
+xn = comp_distortion_oulu(x_distort,kc);
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/pgmread.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/pgmread.m
new file mode 100755
index 0000000..c96ccb7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/project2_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/project2_oulu.m
new file mode 100755
index 0000000..c5c4a34
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/project_points.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/project_points.m
new file mode 100755
index 0000000..1823490
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/projectedGrid.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/projectedGrid.m
new file mode 100755
index 0000000..561a7d0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/readras.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/readras.m
new file mode 100755
index 0000000..fc1820b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/recomp_corner_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/recomp_corner_calib.m
new file mode 100755
index 0000000..e0af501
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/recomp_corner_calib.m
@@ -0,0 +1,96 @@
+% Re-select te corners after calibration
+
+check_active_images;
+
+if ~exist(['y_' num2str(ind_active(1))]),
+   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))]),
+   ima_read_calib;
+   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/calib_bouguetj/rect.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rect.m
new file mode 100755
index 0000000..d8b6366
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rect.m
@@ -0,0 +1,93 @@
+function [Irec] = rect(I,R,f,c,k,KK_new);
+
+
+% 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.
+
+[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,:)+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/calib_bouguetj/reproject_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/reproject_calib.m
new file mode 100755
index 0000000..86b13f5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/reproject_calib.m
@@ -0,0 +1,92 @@
+%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%%
+
+if ~exist('no_image'),
+   no_image = 0;
+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)) ]'),
+        ima_read_calib;
+        if no_image_file,
+        fprintf(1,'WARNING: Do not show the original images\n'); %return;
+        end;
+   end;
+else
+   no_image_file = 1;
+end;
+
+
+
+ima_numbers = input('Number(s) of image(s) to show ([] = all images) = ');
+
+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 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;
+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 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;
+        hold off;
+        drawnow;
+
+      set(5+kk,'Name',num2str(kk),'NumberTitle','off');
+      
+   end;
+   
+end;
+
+
+err_std = std(ex')';
+
+fprintf(1,'Pixel error:      err = [ %3.5f   %3.5f]\n\n',err_std); 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rigid_motion.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rigid_motion.m
new file mode 100755
index 0000000..473405c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/rodrigues.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rodrigues.m
new file mode 100755
index 0000000..9d55337
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/rotation.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rotation.m
new file mode 100755
index 0000000..87ee2fe
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/run_error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/run_error_analysis.m
new file mode 100755
index 0000000..095e17e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/saveinr.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saveinr.m
new file mode 100755
index 0000000..a176e39
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/savepgm.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/savepgm.m
new file mode 100755
index 0000000..397f028
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/saveppm.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saveppm.m
new file mode 100755
index 0000000..0062ee0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saveppm.m
@@ -0,0 +1,25 @@
+%SAVEPPM        Write a PPM format file
+%
+%       SAVEPPM(filename, r, g, b)
+%
+%       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, R, G, B)
+
+        fid = fopen(fname, 'w');
+        [r,c] = size(R');
+        fprintf(fid, 'P6\n');
+        fprintf(fid, '%d %d\n', r, c);
+        fprintf(fid, '255\n');
+        im = [R(:) G(:) B(:)];
+        im = reshape(c,r);
+        fwrite(fid, im, 'uchar');
+        fclose(fid)
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saving_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saving_calib.m
new file mode 100755
index 0000000..e0575e0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saving_calib.m
@@ -0,0 +1,27 @@
+fprintf(1,'\nSaving calibration results under Calib_Results.mat\n');
+
+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;
+
+
+string_save = 'save Calib_Results param_list active_images ind_active fc kc cc ex x y solution sol_with_center 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;
+
+%fprintf(1,'To load later click on Load\n');
+
+fprintf(1,'done\n');
+
+eval(string_save);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/script_fit_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/script_fit_distortion.m
new file mode 100755
index 0000000..c5e5430
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/select_sol_no_center.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center.m
new file mode 100755
index 0000000..15508e5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center.m
@@ -0,0 +1,19 @@
+%%% Selection of the calibration solution with center estimation
+
+if ~exist('sol_no_center'),
+   fprintf(1,'Need to calibrate before selecting solution without center. Maybe need to load Calib_Results.mat file.\n');
+   return;
+end;
+
+solution = sol_no_center;
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters;
+comp_error_calib;
+
+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); 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center3D.m
new file mode 100755
index 0000000..070d81c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center3D.m
@@ -0,0 +1,20 @@
+%%% Selection of the calibration solution with center estimation
+
+solution = sol_no_center;
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters3D;
+
+
+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);   
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+graphout_calib3D;
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center.m
new file mode 100755
index 0000000..2df9ba8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center.m
@@ -0,0 +1,19 @@
+%%% Selection of the calibration solution with center estimation
+
+if ~exist('sol_with_center'),
+   fprintf(1,'Need to calibrate before selecting solution with center. Maybe need to load Calib_Results.mat file.\n');
+   return;
+end;
+
+solution = sol_with_center;
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters;
+comp_error_calib;
+
+fprintf(1,'\n\nCalibration results with 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); 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center3D.m
new file mode 100755
index 0000000..eb6f4bf
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center3D.m
@@ -0,0 +1,20 @@
+%%% Selection of the calibration solution with center estimation
+
+solution = sol_with_center;
+
+%%% Extraction of the final intrinsic and extrinsic paramaters:
+
+extract_parameters3D;
+
+
+fprintf(1,'\n\nCalibration results with 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);   
+
+
+%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%
+
+graphout_calib3D;
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/startup.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/startup.m
new file mode 100755
index 0000000..aad0fa4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/test_3d.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/test_3d.m
new file mode 100755
index 0000000..9f442f4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/test_3d.m
@@ -0,0 +1,80 @@
+Rc_1 = rodrigues(omc_1);
+Rc_2 = rodrigues(omc_2);
+Rc_3 = rodrigues(omc_3);
+Rc_4 = rodrigues(omc_4);
+Rc_5 = rodrigues(omc_5);
+Rc_6 = rodrigues(omc_6);
+Rc_7 = rodrigues(omc_7);
+Rc_8 = rodrigues(omc_8);
+Rc_9 = rodrigues(omc_9);
+
+Rc_10 = rodrigues(omc_10);
+Rc_11 = rodrigues(omc_11);
+Rc_12 = rodrigues(omc_12);
+Rc_13 = rodrigues(omc_13);
+Rc_14 = rodrigues(omc_14);
+Rc_15 = rodrigues(omc_15);
+Rc_16 = rodrigues(omc_16);
+Rc_17 = rodrigues(omc_17);
+Rc_18 = rodrigues(omc_18);
+
+
+
+RR1 = Rc_1'*Rc_10; % should be rodrigues([0;pi/2;0])
+TT1 = Rc_1'*(Tc_10-Tc_1); % should be [dX*n_sq_x_1;0;0]
+
+Xr_1 = RR1 * X_10  + TT1*ones(1,length(X_10));
+
+figure(1);
+plot3(X_1(1,:),X_1(2,:),X_1(3,:),'r+'); hold on;
+plot3(Xr_1(1,:),Xr_1(2,:),Xr_1(3,:),'g+');
+hold off;
+axis('equal');
+rotate3d on;
+view(0,0);
+xlabel('x');
+ylabel('y');
+zlabel('z');
+
+aaa = [];
+
+RR1 = Rc_1'*Rc_10; % should be rodrigues([0;pi/2;0])
+TT1 = Rc_1'*(Tc_10-Tc_1); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR1) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR2 = Rc_2'*Rc_11; % should be rodrigues([0;pi/2;0])
+TT2 = Rc_2'*(Tc_11-Tc_2); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR2) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR3 = Rc_3'*Rc_12; % should be rodrigues([0;pi/2;0])
+TT3 = Rc_3'*(Tc_12-Tc_3); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR3) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR4 = Rc_4'*Rc_13; % should be rodrigues([0;pi/2;0])
+TT4 = Rc_4'*(Tc_13-Tc_4); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR4) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR5 = Rc_5'*Rc_14; % should be rodrigues([0;pi/2;0])
+TT5 = Rc_5'*(Tc_14-Tc_5); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR5) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR6 = Rc_6'*Rc_15; % should be rodrigues([0;pi/2;0])
+TT6 = Rc_6'*(Tc_15-Tc_6); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR6) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR7 = Rc_7'*Rc_16; % should be rodrigues([0;pi/2;0])
+TT7 = Rc_7'*(Tc_16-Tc_7); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR7) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
+RR8 = Rc_8'*Rc_17; % should be rodrigues([0;pi/2;0])
+TT8 = Rc_8'*(Tc_17-Tc_8); % should be [dX*n_sq_x_1;0;0]
+err = rodrigues(RR8) - [0;pi/2;0]
+aaa = [aaa 2*sin(err(2)/2)*.33*1000];
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/undistort_image.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/undistort_image.m
new file mode 100755
index 0000000..6393d78
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/undistort_image.m
@@ -0,0 +1,88 @@
+%%% INPUT THE IMAGE FILE NAME:
+
+dir;
+
+fprintf(1,'\n');
+disp('Program that undistort an image');
+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'') ','s');
+
+        if isempty(format_image2),
+        format_image2 = 'ras';
+        end;
+
+        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;
+
+ima_name = [image_name '.' format_image];
+
+
+
+%%% READ IN IMAGE:
+
+if format_image(1) == 'p',
+   I = double(pgmread(ima_name));
+else
+   if format_image(1) == 'r',
+      I = readras(ima_name);
+   else
+      I = double(imread(ima_name));
+   end;
+end;
+     
+if size(I,3)>1,
+   I = I(:,:,2);
+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,'Compututing the undistorted image...\n')
+
+[I2] = rect(I,eye(3),fc,cc,kc,KK);
+
+
+figure(3);
+image(I2);
+colormap(gray(256));
+title('Undistorted image - Stored in array I2');
+drawnow;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/writeras.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/writeras.m
new file mode 100755
index 0000000..c7eb7bc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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
diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj2/TOOLBOX_calib.tar b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj2/TOOLBOX_calib.tar
new file mode 100755
index 0000000..36d5de9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj2/TOOLBOX_calib.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC.m b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC.m
new file mode 100755
index 0000000..d612780
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC.m
@@ -0,0 +1,26 @@
+function [v,d] = Ncut_IC(I,nb_radius_IC);

+%

+% [v,d] = Ncut_IC(I,nb_radius_IC);

+%

+

+if nargin<2,

+    nb_radius_IC = 5;

+end

+

+I = I/max(I(:));

+

+eg_par = [16,2, 21,3]; eg_th = 0;

+   

+nv = 11; reg_fac = 0.01;

+

+[ex,ey,egx,egy,eg_par,eg_th,emag,ephase] = quadedgep(I,eg_par,eg_th); 

+    

+

+nb_radius_IC= 10;

+sample_rate = 0.2;

+disp('setupW\n');

+[w_i,w_j] = cimgnbmap(size(I),nb_radius_IC,sample_rate);

+w = affinityic(emag,ephase,w_i,w_j);

+disp('computeNcut');

+[v,d] = ncut(w,nv,[],reg_fac);

+

diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m.m b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m.m
new file mode 100755
index 0000000..146acf9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m.m
@@ -0,0 +1,42 @@
+function [v,d] = Ncut_IC_m(I,mask,nb_radius_IC,sig_IC);

+%

+% [v,d] = Ncut_IC_m(I,mask,nb_radius_IC,sig_IC);

+%

+

+if nargin<2,

+    mask = ones(size(I));

+end

+

+if nargin<3,

+    nb_radius_IC = 10;

+end

+

+if nargin<4,

+    sig_IC = 0.03;

+end

+

+%%% normalize the image

+I = I/max(I(:));

+

+%%% edge detecting parameter, [num_ori, sig, win_size, enlongation factor]

+eg_par = [6,2, 21,3]; eg_th = 0.01;

+   

+%% number of eigenvectors+ regulization factors

+nv = 10; reg_fac = 0.0;

+

+%% compute the edge response

+[ex,ey,egx,egy,eg_par,eg_th,emag,ephase] = quadedgep(I,eg_par,eg_th); 

+    

+%%% setup Wij connection pattern

+sample_rate = 0.1;

+[w_i,w_j] = cimgnbmap(size(I),nb_radius_IC,sample_rate);

+

+%%% compute Wij with IC

+emag = mask.*emag;

+w = affinityic(emag,ephase,w_i,w_j,sig_IC);

+

+%show_dist_w(I,w);

+%%% running Ncut

+[v,d] = ncut(w,nv);

+

+v = reshape(v,size(I,1),size(I,2),size(v,2));

diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m2.m b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m2.m
new file mode 100755
index 0000000..9668b19
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/Ncut_IC_m2.m
@@ -0,0 +1,51 @@
+function [v,d] = Ncut_IC_m(I,mask,nb_radius_IC,sig_IC);

+%

+% [v,d] = Ncut_IC_m(I,mask,nb_radius_IC,sig_IC);

+%

+

+if nargin<2,

+    mask = ones(size(I));

+end

+

+if nargin<3,

+    nb_radius_IC = 10;

+end

+

+if nargin<4,

+    sig_IC = 0.03;

+end

+

+%%% normalize the image

+I = I/max(I(:));

+

+%%% edge detecting parameter, [num_ori, sig, win_size, enlongation factor]

+eg_par = [6,2, 21,3]; eg_th = 0.01;

+   

+%% number of eigenvectors+ regulization factors

+nv = 10; reg_fac = 0.0;

+

+%% compute the edge response

+[nr,nc,nb] = size(I);

+emag = zeros(nr,nc);

+ephase = zeros(nr,nc);

+for j=1:nb,

+    [ex,ey,egx,egy,eg_par,eg_th,emag1,ephase1] = quadedgep(I(:,:,j),eg_par,eg_th); 

+    mask = emag1>emag;

+    ephase = ephase+ mask.*ephase1;

+    emag = emag + mask.*emag1;

+end

+

+

+%%% setup Wij connection pattern

+sample_rate = 0.1;

+[w_i,w_j] = cimgnbmap(size(I),nb_radius_IC,sample_rate);

+

+%%% compute Wij with IC

+emag = mask.*emag;

+w = affinityic(emag,ephase,w_i,w_j,sig_IC);

+

+%show_dist_w(I,w);

+%%% running Ncut

+[v,d] = ncut(w,nv);

+

+v = reshape(v,size(I,1),size(I,2),size(v,2));

diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.c b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.c
new file mode 100755
index 0000000..e48762a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.c
@@ -0,0 +1,186 @@
+/*================================================================
+* function w = affinityic(emag,ephase,pi,pj,sigma)
+* Input:
+*   emag = edge strength at each pixel
+*   ephase = edge phase at each pixel
+*   [pi,pj] = index pair representation for MALTAB sparse matrices
+*   sigma = sigma for IC energy
+* Output:
+*   w = affinity with IC at [pi,pj]
+*
+
+% test sequence
+f = synimg(10);
+[i,j] = cimgnbmap(size(f),2);
+[ex,ey,egx,egy] = quadedgep(f);
+a = affinityic(ex,ey,egx,egy,i,j)
+show_dist_w(f,a);
+
+* Jianbo Shi, Stella X. Yu, Nov 19, 2001.
+*=================================================================*/
+
+# include "mex.h"
+# include "math.h"
+
+void mexFunction(
+    int nargout,
+    mxArray *out[],
+    int nargin,
+    const mxArray *in[]
+)
+{
+    /* declare variables */
+    int nr, nc, np, total;
+    int i, j, k, ix, iy, jx, jy, ii, jj, iip1, jjp1, iip2, jjp2, step;
+    double sigma, di, dj, a, z, maxori, phase1, phase2, slope;
+        int *ir, *jc;
+        unsigned long *pi, *pj;
+        double *emag, *ephase, *w;
+    
+    /* check argument */
+    if (nargin<4) {
+        mexErrMsgTxt("Four input arguments required");
+    }
+    if (nargout>1) {
+        mexErrMsgTxt("Too many output arguments");
+    }
+
+    /* get edgel information */
+        nr = mxGetM(in[0]);
+        nc = mxGetN(in[0]);
+        if ( nr*nc ==0 || nr != mxGetM(in[1]) || nc != mxGetN(in[1]) ) {
+            mexErrMsgTxt("Edge magnitude and phase shall be of the same image size");
+        }
+    emag = mxGetPr(in[0]);
+    ephase = mxGetPr(in[1]);
+    np = nr * nc;
+    
+    /* get new index pair */
+    if (!mxIsUint32(in[2]) | !mxIsUint32(in[3])) {
+        mexErrMsgTxt("Index pair shall be of type UINT32");
+    }
+    if (mxGetM(in[3]) * mxGetN(in[3]) != np + 1) {
+        mexErrMsgTxt("Wrong index representation");
+    }
+    pi = mxGetData(in[2]);
+    pj = mxGetData(in[3]);    
+
+    /* create output */
+    out[0] = mxCreateSparse(np,np,pj[np],mxREAL);
+    if (out[0]==NULL) {
+            mexErrMsgTxt("Not enough memory for the output matrix");
+        }
+        w = mxGetPr(out[0]);
+        ir = mxGetIr(out[0]);
+        jc = mxGetJc(out[0]);
+        
+    /* find my sigma */
+        if (nargin<5) {
+            sigma = 0;
+        for (k=0; k<np; k++) { 
+            if (emag[k]>sigma) { sigma = emag[k]; }
+        }
+        sigma = sigma / 10;
+        printf("sigma = %6.5f",sigma);
+        } else {
+            sigma = mxGetScalar(in[4]);
+        }
+        a = 1.0/ (sigma);
+        
+    /* computation */ 
+    total = 0;
+    for (j=0; j<np; j++) {            
+
+        jc[j] = total;
+        jx = j / nr; /* col */
+        jy = j % nr; /* row */
+        
+        for (k=pj[j]; k<pj[j+1]; k++) {
+        
+            i = pi[k];
+          
+            if (i==j) {
+                maxori = 1;
+            
+            } else {
+        
+                ix = i / nr; 
+                iy = i % nr;
+
+                /* scan */            
+                di = (double) (iy - jy);
+                dj = (double) (ix - jx);
+            
+                maxori = 0.;
+                    phase1 = ephase[j];
+
+                       
+                /* sample in i direction */
+                if (abs(di) >= abs(dj)) {  
+                    slope = dj / di;
+                    step = (iy>=jy) ? 1 : -1;
+                
+                    iip1 = jy;
+                    jjp1 = jx;
+        
+                       
+                        for (ii=0;ii<abs(di);ii++){
+                            iip2 = iip1 + step;
+                            jjp2 = (int)(0.5 + slope*(iip2-jy) + jx);
+                  
+                            phase2 = ephase[iip2+jjp2*nr];
+               
+                            if (phase1 != phase2) {
+                                z = (emag[iip1+jjp1*nr] + emag[iip2+jjp2*nr]);
+                                if (z > maxori){
+                                    maxori = z;
+                                }
+                            } 
+                     
+                            iip1 = iip2;
+                            jjp1 = jjp2;
+                            phase1 = phase2;
+                        }
+                    
+                    /* sample in j direction */    
+                } else { 
+                        slope = di / dj;
+                        step =  (ix>=jx) ? 1: -1;
+
+                    jjp1 = jx;
+                        iip1 = jy;                 
+            
+         
+                        for (jj=0;jj<abs(dj);jj++){
+                            jjp2 = jjp1 + step;
+                            iip2 = (int)(0.5+ slope*(jjp2-jx) + jy);
+                  
+                            phase2 = ephase[iip2+jjp2*nr];
+             
+                            if (phase1 != phase2){
+                                z = (emag[iip1+jjp1*nr] + emag[iip2+jjp2*nr]);
+                                if (z > maxori){ 
+                                    maxori = z; 
+                                }
+                                
+                            }
+          
+                            iip1 = iip2;
+                            jjp1 = jjp2;
+                            phase1 = phase2;
+                        }
+                }            
+            
+                maxori = 0.5 * maxori*a;
+                maxori = exp(-maxori*maxori);
+            }       
+                    ir[total] = i;
+
+                    w[total] = maxori + 0.005;
+                    total = total + 1;
+                        
+                } /* i */
+    } /* j */
+        
+    jc[np] = total;
+}  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexa64 b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexa64
new file mode 100755
index 0000000..e60b4d1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexa64
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexglx b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexglx
new file mode 100755
index 0000000..5edb5d8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/affinityic.mexglx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/anisodiff.m b/SD-VBS/common/toolbox/toolbox_basic/common/anisodiff.m
new file mode 100755
index 0000000..b576d8f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/anisodiff.m
@@ -0,0 +1,20 @@
+function [outimage] = anisodiff(inimage,iterations,K)
+% [outimage] = anisodiff(inimage,iterations,K)
+% Pietro's anisotropic diffusion routine
+
+lambda = 0.25;
+outimage = inimage;    [m,n] = size(inimage);
+
+rowC = [1:m];     rowN = [1 1:m-1];     rowS = [2:m m];
+colC = [1:n];     colE = [1 1:n-1];     colW = [2:n n];
+
+for i=1:iterations,
+   deltaN = outimage(rowN,colC) - outimage(rowC,colC);
+   deltaE = outimage(rowC,colE) - outimage(rowC,colC);
+
+   fluxN = deltaN .* exp( - ((1/K) * abs(deltaN)).^2 );
+   fluxE = deltaE .* exp( - ((1/K) * abs(deltaE)).^2 );
+   
+   outimage = outimage + lambda * (fluxN - fluxN(rowS,colC) + fluxE - fluxE(rowC,colW)); 
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/bin.m b/SD-VBS/common/toolbox/toolbox_basic/common/bin.m
new file mode 100755
index 0000000..e2c3c90
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/bin.m
@@ -0,0 +1,39 @@
+function [i, nnbins] = bin(x, dx, x0, x1);
+%
+% [i, nbins] = bin(x, dx, x0, x1);
+%
+% Returns the vector of indices, starting from 1, 
+% corresponding to the chosen bin size, dx, 
+% start x0 and end x1. If x1 is omitted, x1 = max(x) - dx/2. 
+% If x0 is omitted, x0 = min(x) + dx/2. If dx is omitted, the data 
+% are divided into 10 classes. Note that outliers are not removed.
+%
+% Tested under MatLab 4.2, 5.0, and 5.1.
+%
+
+% 17.1.97, Oyvind.Breivik@gfi.uib.no.
+%
+% Oyvind Breivik
+% Department of Geophysics
+% University of Bergen
+% NORWAY
+
+N = 10; % Default is 10 classes
+
+if nargin < 2
+ dx = (max(x) - min(x))/N;
+end
+if nargin < 3
+ x0 = min(x) + dx/2;
+end
+if nargin < 4
+ x1 = max(x) - dx/2;
+end
+nbins = round((x1 - x0)/dx) + 1;
+i = round((x - x0)/dx) + 1;
+%in = (i >= 1) & (i <= nbins); % Indices are within range [1, nbins]. 
+%i = i(in);
+
+if nargout > 1
+ nnbins = nbins;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.c b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.c
new file mode 100755
index 0000000..1595b68
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.c
@@ -0,0 +1,189 @@
+/*================================================================
+* function [i,j] = cimgnbmap([nr,nc], nb_r, sample_rate)
+*   computes the neighbourhood index matrix of an image,
+*   with each neighbourhood sampled.
+* Input:
+*   [nr,nc] = image size
+*   nb_r = neighbourhood radius, could be [r_i,r_j] for i,j
+*   sample_rate = sampling rate, default = 1
+* Output:
+*   [i,j] = each is a column vector, give indices of neighbour pairs
+*     UINT32 type
+*       i is of total length of valid elements, 0 for first row
+*       j is of length nr * nc + 1
+*
+* See also: imgnbmap.c, id2cind.m
+*
+* Examples: 
+*   [i,j] = imgnbmap(10, 20); % [10,10] are assumed
+*
+* Stella X. Yu, Nov 12, 2001.
+
+% test sequence:
+nr = 15;
+nc = 15;
+nbr = 1;
+[i,j] = cimgnbmap([nr,nc], nbr);
+mask = csparse(i,j,ones(length(i),1),nr*nc);
+show_dist_w(rand(nr,nc),mask)
+
+*=================================================================*/
+
+# include "mex.h"
+# include "math.h"
+
+void mexFunction(
+    int nargout,
+    mxArray *out[],
+    int nargin,
+    const mxArray *in[]
+)
+{
+    /* declare variables */
+    int nr, nc, np, nb, total;
+        double *dim, sample_rate;
+    int r_i, r_j, a1, a2, b1, b2, self, neighbor;
+    int i, j, k, s, t, nsamp, th_rand, no_sample;
+    unsigned long *p, *qi, *qj;
+    
+    /* check argument */
+    if (nargin < 2) {
+        mexErrMsgTxt("Two input arguments required");
+    }
+    if (nargout> 2) {
+        mexErrMsgTxt("Too many output arguments.");
+    }
+
+    /* get image size */
+    i = mxGetM(in[0]);
+    j = mxGetN(in[0]);
+    dim = mxGetData(in[0]);
+    nr = (int)dim[0];
+    if (j>1 || i>1) {
+        nc = (int)dim[1];
+    } else {
+        nc = nr;
+    }
+    np = nr * nc;
+    
+    /* get neighbourhood size */
+    i = mxGetM(in[1]);
+    j = mxGetN(in[1]);
+    dim = mxGetData(in[1]);
+    r_i = (int)dim[0];
+    if (j>1 || i>1) {
+        r_j = (int)dim[1];              
+    } else {
+        r_j = r_i;
+    }
+    if (r_i<0) { r_i = 0; }
+        if (r_j<0) { r_j = 0; }
+
+        /* get sample rate */
+        if (nargin==3) {                
+                sample_rate = (mxGetM(in[2])==0) ? 1: mxGetScalar(in[2]);
+    } else {
+                sample_rate = 1;
+    }
+        /* prepare for random number generator */
+        if (sample_rate<1) {
+        srand( (unsigned)time( NULL ) );
+        th_rand = (int)ceil((double)RAND_MAX * sample_rate);
+        no_sample = 0;
+    } else {
+                sample_rate = 1;
+        th_rand = RAND_MAX;
+        no_sample = 1;
+    }
+    
+        /* figure out neighbourhood size */
+
+    nb = (r_i + r_i + 1) * (r_j + r_j + 1); 
+    if (nb>np) {
+        nb = np;
+    }
+    nb = (int)ceil((double)nb * sample_rate);    
+
+        /* intermediate data structure */
+        p = mxCalloc(np * (nb+1), sizeof(unsigned long));
+        if (p==NULL) {
+        mexErrMsgTxt("Not enough space for my computation.");
+        }
+        
+    /* computation */    
+        total = 0;
+    for (j=0; j<nc; j++) {
+    for (i=0; i<nr; i++) {
+
+                self = i + j * nr;
+
+                /* put self in, otherwise the index is not ordered */
+                p[self] = p[self] + 1;
+                p[self+p[self]*np] = self;
+
+        /* j range */
+                b1 = j;
+        b2 = j + r_j;
+        if (b2>=nc) { b2 = nc-1; }                
+    
+                /* i range */
+        a1 = i - r_i;
+                if (a1<0) { a1 = 0; }
+        a2 = i + r_i;
+        if (a2>=nr) { a2 = nr-1; }
+       
+                /* number of more samples needed */
+                nsamp = nb - p[self];
+
+                k = 0;          
+                t = b1;
+                s = i + 1;
+                if (s>a2) {
+                        s = a1;
+                        t = t + 1;
+                }
+                while (k<nsamp && t<=b2) {
+                        if (no_sample || (rand()<th_rand)) {
+                                k = k + 1;
+                                neighbor = s + t * nr;
+                                
+                                p[self] = p[self] + 1;                                  
+                                p[self+p[self]*np] = neighbor;
+                        
+                                p[neighbor] = p[neighbor] + 1;
+                                p[neighbor+p[neighbor]*np] = self;
+                        }
+                        s = s + 1;
+                        if (s>a2) {
+                s = a1;
+                                t = t + 1;
+                        }
+                } /* k */
+
+                total = total + p[self];
+        } /* i */
+    } /* j */
+    
+    /* i, j */
+    out[0] = mxCreateNumericMatrix(total, 1, mxUINT32_CLASS, mxREAL);
+        out[1] = mxCreateNumericMatrix(np+1,  1, mxUINT32_CLASS, mxREAL);
+        qi = mxGetData(out[0]);
+        qj = mxGetData(out[1]);
+        if (out[0]==NULL || out[1]==NULL) {
+            mexErrMsgTxt("Not enough space for the output matrix.");
+        }
+
+        total = 0;
+    for (j=0; j<np; j++) {
+                qj[j] = total;
+                s = j + np;
+                for (t=0; t<p[j]; t++) {
+                    qi[total] = p[s];
+                        total = total + 1;
+                        s = s + np;
+                }
+    }
+        qj[np] = total;
+
+        mxFree(p);
+}  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexa64 b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexa64
new file mode 100755
index 0000000..19eabe1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexa64
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexglx b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexglx
new file mode 100755
index 0000000..6f3bb32
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/cimgnbmap.mexglx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/density.m b/SD-VBS/common/toolbox/toolbox_basic/common/density.m
new file mode 100755
index 0000000..23d88cc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/density.m
@@ -0,0 +1,133 @@
+function [rrho, xxvec, yyvec] = density(x, y, dx, dy, xy0);
+% [rho, xvec, yvec] = density(x, y, dx, dy, [x0 x1 y0 y1]);  
+% 
+%  2-D probability density plot.
+%
+%  Input:  Vectors x and y of equal length. If dx and/or dy are omitted, the
+%          default is 75 bins in each direction.
+%      
+%          If dx or dy is negative, then the variable is taken as the number of
+%          bins rather than a grid resolution.
+%
+%          The vector containing the limits can be padded with NaNs if only
+%          certain limits are desired, e g if x0 and y1 are wanted:
+%
+%            density(x, y, [.5 nan nan 45])
+%
+%  Output: The density matrix RHO together with vectors XVEC and YVEC. 
+%          If no output arguments are specified, DENSITY will plot the density
+%          function with the prescribed axes using PCOLOR. 
+%
+%  Requires bin.m. Tested under MatLab 4.2, 5.0, and 5.1.
+%
+%  See also bin.m for further details about dx, x0, etc, and ffgrid.m.
+%
+
+% 1.9.97 Oyvind.Breivik@gfi.uib.no.
+%
+% Oyvind Breivik
+% Department of Geophysics
+% University of Bergen
+% NORWAY
+
+DX = -75;  % Default grid size
+
+x = x(:);
+
+if nargin < 2
+ y = x;
+end
+
+y = y(:);
+
+xy = NaN*ones(1,4);
+
+if (nargin < 4)
+ xy0 = min(x);
+end
+
+if (nargin == 3 & length(dx) > 1)
+ xy0 = dx;
+ dx = DX;
+end
+
+if nargin < 3
+ dx = DX;
+end
+
+if (nargin == 4 & length(dy) > 1)
+ xy0 = dy;
+ dy = dx;
+end
+if nargin < 4
+ dy = dx;
+end
+
+nxy = length(xy0);
+xy(1:nxy) = xy0;
+
+if (isnan(xy(4)))
+ xy(4) = max(y);
+end
+if (isnan(xy(3)))
+ xy(3) = min(y);
+end
+if (isnan(xy(2)))
+ xy(2) = max(x);
+end
+if (isnan(xy(1)))
+ xy(1) = min(x);
+end
+x0 = xy(1); x1 = xy(2); y0 = xy(3); y1 = xy(4);
+
+if (dx < 0)
+ dx = (x1 - x0)/abs(dx);
+end
+if (dy < 0)
+ dy = (y1 - y0)/abs(dy);
+end
+
+ix = bin(x, dx, x0, x1);
+iy = bin(y, dy, y0, y1); % bin data in (x,y)-space
+
+xvec = x0:dx:x1;
+yvec = y0:dy:y1;
+
+nx = length(xvec);
+ny = length(yvec);
+
+inx = (ix >= 1) & (ix <= nx);
+iny = (iy >= 1) & (iy <= ny);
+in = (inx & iny);
+ix = ix(in); iy = iy(in);
+N = length(ix); % how many datapoints are left now?
+
+rho = zeros(length(xvec), length(yvec)) + eps;
+
+for i = 1:N
+ rho(ix(i), iy(i)) = rho(ix(i), iy(i)) + 1;
+end
+
+rho = rho/(N*dx*dy); % Density is n per dx per dy
+
+rho = rho'; % Get in shape
+
+if nargout == 0
+ pcolor(xvec, yvec, sqrt(rho)); shading interp; axis image;
+ colorbar
+ colormap jet
+ xlabel(inputname(1))
+ ylabel(inputname(2))
+ dum = size(rho');
+ str = sprintf('%d data points, grid: %dx%d', N, dum(1)-1, dum(2)-1);
+ title(str);
+end
+
+if nargout > 0
+ rrho = rho;
+end
+
+if nargout > 1
+ xxvec = xvec;
+ yyvec = yvec;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/find_edge.m b/SD-VBS/common/toolbox/toolbox_basic/common/find_edge.m
new file mode 100755
index 0000000..4299c29
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/find_edge.m
@@ -0,0 +1,24 @@
+function [edgemap,mag,th] = find_edge(I,sig,mag_thld)
+%
+% [edgemap,mag,th] = find_edge(I,sig,mag_thld)
+%
+
+if nargin<2,
+  sig = 1;
+end
+
+if nargin<3,
+  mag_thld = 1/30;
+end
+
+I = I/max(I(:));
+
+ismax = 1;r = 1;
+
+[gx,gy] = grad(I,sig);
+[th,mag] = cart2pol(gy,gx);
+
+g = cat(3,gy,gx);
+edgemap = nonmaxsup(g,ismax,r);
+edgemap = edgemap.*(mag>mag_thld);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/grad.m b/SD-VBS/common/toolbox/toolbox_basic/common/grad.m
new file mode 100755
index 0000000..05fce39
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/grad.m
@@ -0,0 +1,28 @@
+% gradient of an image
+% coordinates (r, c) follow matrix convention;
+% the gaussian is truncated at x = +- tail, and there are samples samples
+% inbetween, where samples = hsamples * 2 + 1
+
+function[gr,gc] = gradient(image, hsamples,cm)
+
+if nargin <3,
+ cm = 'same';
+end
+
+tail=4;
+samples = hsamples * 2 + 1;
+
+x = linspace(-tail, tail, samples);
+gauss = exp(-x.^2);
+n = gauss * ones(samples,1);
+gauss = gauss/n;
+
+gaussderiv = -x.*gauss;
+n = -gaussderiv*linspace(1,samples,samples)';
+gaussderiv = gaussderiv/n;
+
+%gr = conv2(conv2(image, gaussderiv','valid'), gauss,'valid');
+%gc = conv2(conv2(image, gaussderiv,'valid'), gauss','valid');
+
+gr = conv2(conv2(image, gaussderiv',cm), gauss,cm);
+gc = conv2(conv2(image, gaussderiv,cm), gauss',cm);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_even2.m b/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_even2.m
new file mode 100755
index 0000000..937f9bd
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_even2.m
@@ -0,0 +1,45 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz,enlong)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+if nargin<4,
+    enlong = 3;
+end
+
+enlong = 2*enlong
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog2(filter_scales(m),enlong,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_odd2.m b/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_odd2.m
new file mode 100755
index 0000000..0059dca
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/make_filterbank_odd2.m
@@ -0,0 +1,46 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz,enlong)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+if nargin<4,
+    enlong = 3;
+end
+
+enlong = enlong*2;
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog1(filter_scales(m),enlong,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/max_supress2.m b/SD-VBS/common/toolbox/toolbox_basic/common/max_supress2.m
new file mode 100755
index 0000000..05b5f11
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/max_supress2.m
@@ -0,0 +1,59 @@
+function NMS = max_supress2(data,ismax);
+%
+%   NMS = max_supress(data,ismax);
+%
+%      data: [nr,nc,nfilter,nscale]
+%               of filter mag. map
+%      ismax: 1 local max, 0 local min
+%
+
+[nr,nc,nfilter,nscale] = size(data);
+
+% set up the orthognal neighbourhood for each oriented filter
+if nfilter == 6,
+  nbr_template=[1 1 1 0 -1 -1
+                0 1 1 1  1  1];
+else
+  nbr_template=[1 0 ; 
+                0 1];
+end
+
+%% for each scale, compute the dominate filter response
+canny_dir_I = zeros(nr,nc,nscale);
+
+for m = 1:nscale,
+   [max_Ori_resp_I,Ori_sca_I] = max(data(:,:,:,m),[],3);
+   canny_dir_I(:,:,m) = Ori_sca_I;
+end
+
+max_Ori_resp_small = max_Ori_resp_I(2:end-1,2:end-1);
+canny_dir = canny_dir_I(2:end-1,2:end-1);
+
+%% 
+
+NMS = zeros(nr,nc,nscale);
+
+
+for m = 1:nscale,
+ 
+  [x,y] = meshgrid(2:nc-1,2:nr-1);
+  xid = x(:)+nbr_template(2,canny_dir(:))';
+  yid = y(:)+nbr_template(1,canny_dir(:))';
+  id1 = (xid-1)*nr+yid;
+
+  xid = x(:)-nbr_template(2,canny_dir(:))';
+  yid = y(:)-nbr_template(1,canny_dir(:))';
+  id2 = (xid-1)*nr+yid;
+  if ismax,
+    a = (max_Ori_resp_small(:)>max_Ori_resp_I(id1(:))) .* (max_Ori_resp_small(:)>max_Ori_resp_I(id2(:)));
+    NMS(2:end-1,2:end-1,m) = reshape(a,nr-2,nc-2);
+    NMS(:,:,m) = NMS(:,:,m).*max_Ori_resp_I;
+  else
+    a = (max_Ori_resp_small(:)<max_Ori_resp_I(id1(:))) .* (max_Ori_resp_small(:)<max_Ori_resp_I(id2(:)));
+    NMS(2:end-1,2:end-1,m) = reshape(a,nr-2,nc-2);
+  end
+  
+end
+
+ 
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/mgrad.m b/SD-VBS/common/toolbox/toolbox_basic/common/mgrad.m
new file mode 100755
index 0000000..1d89c87
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/mgrad.m
@@ -0,0 +1,11 @@
+function [dx,dy] = mgrad(I,w)
+%
+%  [dx,dy] = mgrad(I,w)
+%
+
+[nr,nc] = size(I);
+
+dx = zeros(nr,nc);dy = zeros(nr,nc);
+
+dx(:,1:nc-w) = I(:,1:nc-w) - I(:,w+1:nc);
+dy(1:nr-w,:) = I(1:nr-w,:) - I(w+1:nr,:);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.m b/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.m
new file mode 100755
index 0000000..7f964ab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.m
@@ -0,0 +1,25 @@
+%MPGREAD Read an MPEG encoded movie file.       
+%       [M, map] = mpgread('filename', frames) reads the specifed
+%       MPEG file and translates it into the movie M, and colormap map.
+%       If a vector frames is specified, then only the frames specified
+%       in this vector will be placed in M.  Otherwise, all frames will
+%       be placed in M.
+%
+%       M = mpgread('filename', frames, 'indexed')
+%       Reads an MPEG file into the MATLAB 5.3+ format movie which
+%       is a structure array.  Each element has a cdata field 
+%       containing a uint8 image matrix and a colormap field
+%       containing the colormap.  The frames parameter can be [] to
+%       indicate that all frames should be read.
+%
+%       M = mpgread('filename', frames, 'truecolor')
+%       Reads an MPEG file into the MATLAB 5.3+ format movie.  Each
+%       frame in the movie has a truecolor MxNx3 cdata field and
+%       an empty colormap field.
+%
+%       [R, G, B] = mpgread('filename', frames) will perform the same 
+%       operation as above, except that the decoded MPEG frames will
+%       be placed into the matrices R, G, B, where R contains the red
+%       component for each frame, G, the green component, and B, the
+%       blue component.
+%
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.mexlx b/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.mexlx
new file mode 100755
index 0000000..1190192
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/mpgread.mexlx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.m b/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.m
new file mode 100755
index 0000000..b1cb773
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.m
@@ -0,0 +1,29 @@
+%MPGWRITE Write an MPEG file.
+%       MPGWRITE(M, map, 'filename', options) Encodes M in MPEG 
+%       format using the specified colormap and writes the result to the
+%       specified file.  The options argument is an optional vector of
+%       8 or fewer options where each value has the following meaning:
+%       1. REPEAT:
+%               An integer number of times to repeat the movie
+%               (default is 1).
+%       2. P-SEARCH ALGORITHM:
+%               0 = logarithmic (fastest, default value)
+%               1 = subsample
+%               2 = exhaustive  (better, but slow)
+%       3. B-SEARCH ALGORITHM:
+%               0 = simple      (fastest)
+%               1 = cross2      (slightly slower, default value)
+%               2 = exhaustive  (very slow)
+%       4. REFERENCE FRAME:
+%               0 = original    (faster, default)
+%               1 = decoded     (slower, but results in better quality)
+%       5. RANGE IN PIXELS:
+%               An integer search radius.  Default is 10.
+%       6. I-FRAME Q-SCALE:
+%               An integer between 1 and 31.  Default is 8.
+%       7. P-FRAME Q-SCALE:
+%               An integer between 1 and 31.  Default is 10.
+%       8. B-FRAME Q-SCALE:
+%               An integer between 1 and 31.  Default is 25.
+%
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.mexlx b/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.mexlx
new file mode 100755
index 0000000..770bad1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/mpgwrite.mexlx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut.m
new file mode 100755
index 0000000..81fb52f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut.m
@@ -0,0 +1,108 @@
+% function [v,s,na,d] = ncut(a,nv,sigma,offset) 

+% Input:

+%    a = affinity matrix, hermitian, could be 3D, or a cell

+%    nv = number of eigenvectors v

+%    sigma = refer to EIGS.M, 0 for smallest, default = 'LR'

+%    offset = vector, each for an affinity matrix, offset for nondirectional repulsion

+%       W = A - R = (A + offset) - (R + offset)

+%       Expected value = offset in affinity value * # of neighboors

+% Output:

+%    v = generalized eigenvectors of A and D

+%    s = eigenvalues

+%    na = normalized affinity matrix

+%    d = normalization matrix 1/sqrt(rowsum(a))

+% This version now accepts multiple weight matrices

+% the format of cells are good for sparse affinity matrices

+

+% Jianbo Shi

+

+function [v,s,na,d] = ncut(a,nv,sigma,offset)

+

+is_cell = iscell(a);

+if is_cell,

+   nw = length(a);

+   [nr,nc] = size(a{1});

+else

+   [nr,nc,nw] = size(a);

+end

+

+if nargin<2 | isempty(nv),

+   nv = min(nr,6);

+end

+

+if nargin<3 | isempty(sigma),

+   sigma = 'LR';

+end

+

+if nargin<4 | isempty(offset),

+   offset = 0;

+end;

+offset=offset(:);

+j = length(offset);

+offset(j+1:nw) = offset(j);

+

+d = 0;

+na = sparse(nr,nc);

+for j=1:nw, % simultaneous partitioning with multiple weight matrices.

+   if is_cell,

+      w = a{j};

+   elseif issparse(a), % only supports 2D indexing

+      w = a;

+   else

+      w = a(:,:,j);

+   end

+   if j==nw, % to save space

+       clear a;

+   end

+   

+   d = d + sum(abs(w),2) + 2*offset(j); % single equivalent D

+   

+   % modify matrix a to deal with nondirectional repulsion 

+   wr = real(w);

+   wr = (sum(abs(wr),2)-sum(wr,2))*0.5 + offset(j);

+   w = w + spdiags(wr,0,nr,nr);

+   

+   na = na + w; % single equivalent A

+   

+   % if you want the rectified individual weight matrix

+   %if is_cell,

+   %   a{j} = w;      

+   %else

+   %   a(:,:,j) = w;

+   %end

+end

+clear w wr

+

+% normalize

+d = 1./sqrt(d+eps);

+if 1,

+    na = spmtimesd(na,d,d);    

+else    

+    d = spdiags(d,0,nr,nr);

+    na = d * na * d;

+end

+

+options.disp = 0; 

+%options.tol = 1e-10;

+%options.maxit = 15;

+

+warning off

+[v,s] = eigs(na,nv,sigma,options);

+s = real(diag(s));

+warning on

+

+% to make sure positive eigs always come first

+% [x,y] = sort(-s); 

+% s = -x;

+% v = v(:,y);

+

+% project back to get the eigenvectors for the pair (a,d)

+% a x = lambda d x

+% na y = lambda y

+% x = d^(-1/2) y

+

+if 1,

+    v = spdiags(d,0,nr,nr) * v;

+else

+    v = d * v;

+end    

diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_b.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_b.m
new file mode 100755
index 0000000..d686981
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_b.m
@@ -0,0 +1,46 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+%    it computes the largest eigenvectors of
+%      A*v = \lambda D * v;   D = diag(sum(A));
+%
+%    this is same as solving the smallest eigenvectors of
+%     (D-A)*v = \lambda D *v;
+%   
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+
+for j=1:size(A,1),
+  A(j,:) = A(j,:).*ds;
+end
+
+for j=1:size(A,2);
+  A(:,j) = A(:,j).*ds';
+end
+
+
+%disp(sprintf('computing eig values'));
+OPTIONS.tol=1e-10;
+OPTIONS.maxit=15;
+OPTIONS.disp=0;
+%tic;toc;
+
+[v,d] = eigs(A,nv,'LM',OPTIONS);
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_bb.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_bb.m
new file mode 100755
index 0000000..c9b7394
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_bb.m
@@ -0,0 +1,39 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+%    it computes the largest eigenvectors of
+%      A*v = \lambda D * v;   D = diag(sum(A));
+%
+%    this is same as solving the smallest eigenvectors of
+%     (D-A)*v = \lambda D *v;
+%   
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+
+D_inv_half = spdiags(ds',0,size(A,1),size(A,2));
+A = D_inv_half*A*D_inv_half;
+
+disp(sprintf('computing eig values'));
+OPTIONS.tol=1e-10;
+OPTIONS.maxit=15;
+OPTIONS.disp=0;
+%tic;toc;
+
+[v,d] = eigs(A,nv,OPTIONS);
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_e.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_e.m
new file mode 100755
index 0000000..e06c8a6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_e.m
@@ -0,0 +1,36 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(A);
+D = diag(ds);
+
+ds = ones(size(ds))./sqrt(ds);
+
+B = D-A;
+
+for j=1:size(A,1),
+  B(j,:) = B(j,:).*ds;
+end
+
+for j=1:size(A,2);
+  B(:,j) = B(:,j).*ds';
+end
+
+disp(sprintf('computing eig values'));
+tic;[v,d] = eigs(B,nv,'sm');toc;
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_neg.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_neg.m
new file mode 100755
index 0000000..89a1caa
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_neg.m
@@ -0,0 +1,45 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+%    it computes the largest eigenvectors of
+%      A*v = \lambda D * v;   D = diag(sum(A));
+%
+%    this is same as solving the smallest eigenvectors of
+%     (D-A)*v = \lambda D *v;
+%   
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(abs(A));
+ds = ones(size(ds))./sqrt(ds);
+
+for j=1:size(A,1),
+  A(j,:) = A(j,:).*ds;
+end
+
+for j=1:size(A,2);
+  A(:,j) = A(:,j).*ds';
+end
+
+
+%disp(sprintf('computing eig values'));
+OPTIONS.tol=1e-10;
+OPTIONS.maxit=15;
+OPTIONS.disp=0;
+%tic;toc;
+
+[v,d] = eigs(A,nv,OPTIONS);
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_sparse.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_sparse.m
new file mode 100755
index 0000000..dc6d076
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_sparse.m
@@ -0,0 +1,45 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%    Assume A is sparse,
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+%    it computes the largest eigenvectors of
+%      A*v = \lambda D * v;   D = diag(sum(A));
+%
+%    this is same as solving the smallest eigenvectors of
+%     (D-A)*v = \lambda D *v;
+%   
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(abs(A));
+ds = 1./sqrt(ds);
+
+[id_i,id_j,W] = find(A);
+A = sparse(id_i,id_j,ds(id_i)'.*ds(id_j)'.*(W));
+
+%disp(sprintf('computing eig values'));
+SIGMA = 'LM';
+%OPTIONS.issym = 0;
+OPTIONs.isreal = 1;
+OPTIONS.tol=1e-12;
+OPTIONS.maxit=25;
+OPTIONS.disp=0;
+%tic;toc;
+
+tic
+[v,d] = eigs(A,nv,SIGMA,OPTIONS);
+%,OPTIONS);
+toc
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncut_tmp.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_tmp.m
new file mode 100755
index 0000000..b623ee4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncut_tmp.m
@@ -0,0 +1,45 @@
+function [v,d] = ncut(A,nv)
+%
+%  [v,d] = ncut(A,nv)
+%
+%
+%    computes 'nv' of the normalized cut vectors 'v' from
+%    matrix 'A'
+%
+%    it computes the largest eigenvectors of
+%      A*v = \lambda D * v;   D = diag(sum(A));
+%
+%    this is same as solving the smallest eigenvectors of
+%     (D-A)*v = \lambda D *v;
+%   
+
+%
+%  Jianbo Shi   
+%
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+
+for j=1:size(A,1),
+  A(j,:) = A(j,:).*ds;
+end
+
+for j=1:size(A,2);
+  A(:,j) = A(:,j).*ds';
+end
+
+
+%disp(sprintf('computing eig values'));
+OPTIONS.tol=1e-10;
+OPTIONS.maxit=15;
+OPTIONS.disp=0;
+%tic;toc;
+
+[v,d] = eigs(A,nv,OPTIONS);
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/ncutd.m b/SD-VBS/common/toolbox/toolbox_basic/common/ncutd.m
new file mode 100755
index 0000000..cd5e8a4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/ncutd.m
@@ -0,0 +1,108 @@
+% function [v,s,na,d] = ncutd(a,nv,beta,sigma,offset) 
+% Input:
+%    a = (attraction + i*repulsion), both nonnegative but could be asymmetrical
+%    nv = number of eigenvectors v
+%    beta = weighting between undirected graph and directed graph
+%        beta = 1, undirected; beta = 0, directed.  Default = 0.5
+%    sigma = refer to EIGS.M, 0 for smallest, default = 'LR'
+%    offset = vector, each for an affinity matrix, 
+%       per_edge offset for nondirectional attraction and repulsion
+% Output:
+%    v = generalized eigenvectors of A and D
+%    s = eigenvalues
+%    na = normalized affinity matrix
+%    d = normalization matrix 1/sqrt(rowsum(a))
+% This version now accepts multiple weight matrices
+% the format of cells are good for sparse affinity matrices
+
+% Stella X. Yu, 2001.
+
+function [v,s,na,d] = ncutd(a,nv,beta,sigma,offset)
+
+is_cell = iscell(a);
+if is_cell,
+   nw = length(a);
+   [nr,nc] = size(a{1});
+else
+   [nr,nc,nw] = size(a);
+end
+
+if nargin<2 | isempty(nv),
+   nv = min(nr,6);
+end
+
+if nargin<3 | isempty(beta),
+   beta = 0.5;
+end
+beta = beta *2;
+
+if nargin<4 | isempty(sigma),
+   sigma = 'LR';
+end
+
+if nargin<5 | isempty(offset),
+   offset = 0;
+end;
+offset=offset(:);
+j = length(offset);
+offset(j+1:nw) = offset(j);
+
+% modify per-edge offset delta to 2 D_{\delta} = offset to D_R
+offset = offset * (2*nc);
+
+z = zeros(nr,nw);
+na = 0;
+for j=1:nw, % simultaneous partitioning with multiple weight matrices.
+   if is_cell,
+      w = a{j};
+   elseif issparse(a), % only supports 2D indexing
+      w = a;
+   else
+      w = a(:,:,j);
+   end
+   
+   wr = real(w); % attraction
+   wi = imag(w); % repulsion
+   
+   % if wr has negative numbers, treat as repulsion
+   % while negative numbers in wi is ignored
+   aa = wr.*(wr>0);
+   rr = wi.*(wi>0)-wr.*(wr<0);
+   
+   % decomposition
+   au = aa + aa';
+   ad = aa - aa';
+   ru = rr + rr';
+   rd = rr - rr';
+   
+   % construct equivalent matrices
+   x = sum(ru,2);
+   wr = au - ru + diag(x);
+   wi = ad + rd;
+   x = x + sum(au,2);
+   
+   % re-organize, add in offset and beta
+   z(:,j) = x + 2 * offset;
+   na = na + ( beta * (wr + offset) + sqrt(-1)* (2-beta) * wi  );
+   
+end
+z = sum(z,2); % diag(z) = single equivalent D
+
+% normalize
+d = repmat(1./sqrt(z+eps),1,nc);
+na = d.*na;
+na = na.*d';
+
+options.disp = 0; 
+%options.tol = 1e-10;
+%options.maxit = 15;
+
+[v,s] = eigs(na,nv,sigma,options);
+s = real(diag(s));
+
+% project back to get the eigenvectors for the pair (a,d)
+% a x = lambda d x
+% na y = lambda y
+% x = d^(-1/2) y
+
+v = v .* d(:,ones(nv,1));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/nonmaxsup.m b/SD-VBS/common/toolbox/toolbox_basic/common/nonmaxsup.m
new file mode 100755
index 0000000..d12301c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/nonmaxsup.m
@@ -0,0 +1,81 @@
+% function f = nonmaxsup(g,ismax,r) return extrema boolean map.

+% Input: g = image, gradient image pair [x,y], or [x,y,g] in 3D matrix
+%        ismax (=1)/0 is for non maximum/minimum suppression
+%        r (=1) is the neighbourhood radius.
+% Output:
+%        f = thinned extrema boolean map, where
+%            d (||gradient||) / d gradient = 0 
+
+% Stella X. Yu, 2000.
+
+function f = nonmaxsup(g,ismax,r)
+
+if nargin<2,
+    ismax = 1;
+end
+
+if nargin<3,
+    r = 1;
+end
+
+i = size(g,3);
+if i==3,
+    x = g(:,:,1);
+    y = g(:,:,2);
+    g = g(:,:,3);
+elseif i==2,
+    x = g(:,:,1);
+    y = g(:,:,2);
+    g = x.*x + y.*y;
+else
+    [x,y] = gradient(g);
+end
+
+% label angles into 4 directions
+a = angle(x - sqrt(-1).*y); % [-pi,pi)
+s = ceil((abs(a)+pi/8)./(pi/4)); 
+s(find(s==5)) = 1;
+s(find(isnan(s))) = 1;
+
+% augment the image
+[m,n] = size(g);
+newm = m + r + r;
+i = [g(:,1);g(:,end);g(1,:)';g(end,:)']; % image boundary
+if ismax,
+    v = min(i) - 1;
+else
+    v = max(i) + 1;
+end
+i = zeros(newm,r) + v;
+j = zeros(r,n) + v;
+newg = [i, [j; g; j;], i];
+
+% k = index as the interior of the new image
+i = [r+1:newm-r]'+ r*newm;
+j = [0:n-1].*newm;
+k = i(:,ones(1,n)) + j(ones(1,m),:);
+k = k(:);
+
+% unit displacement vectors along gradient directions
+d = [newm,newm-1,-1,-1-newm]'; % for 4 directions
+d = d(s(:));
+
+% non maximum suppression
+f = ones(m*n,1);
+g = g(:);
+newd = 0;
+
+if ismax,
+    for i=1:r,
+        newd = newd + d;
+        f = f & (g>newg(k+newd)) & (g>newg(k-newd));
+    end
+else
+    for i=1:r,
+        newd = newd + d;
+        f = f & (g<newg(k+newd)) & (g<newg(k-newd));
+    end
+end
+
+f = reshape(f,[m,n]);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/pair_dist.m b/SD-VBS/common/toolbox/toolbox_basic/common/pair_dist.m
new file mode 100755
index 0000000..3ea970a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/pair_dist.m
@@ -0,0 +1,14 @@
+function W  = pari_hist_dist_fast(data)
+%
+%  W  = pari_hist_dist(data)
+%
+%        data: num_im by num_feature 
+%        W  : num_im by num_im hist diff
+%
+
+[num_im,num_feature] = size(data);
+mag = sum(data.*data,2);
+
+W = mag(:,ones(1,num_im))  - 2*data*data';
+mag = mag';
+W = W+mag(ones(num_im,1),:);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/quadedgep.m b/SD-VBS/common/toolbox/toolbox_basic/common/quadedgep.m
new file mode 100755
index 0000000..eda33f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/quadedgep.m
@@ -0,0 +1,106 @@
+% function [x,y,gx,gy,par,threshold,mag,mage,g,FIe,FIo,mago] = quadedgep(I,par,threshold);

+% Input:

+%    I = image

+%    par = vector for 4 parameters

+%      [number of filter orientations, number of scales, filter size, elongation]

+%      To use default values, put 0.

+%    threshold = threshold on edge strength

+% Output:

+%    [x,y,gx,gy] = locations and gradients of an ordered list of edgels

+%       x,y could be horizontal or vertical or 45 between pixel sites

+%       but it is guaranteed that there [floor(y) + (floor(x)-1)*nr] 

+%       is ordered and unique.  In other words, each edgel has a unique pixel id.

+%    par = actual par used

+%    threshold = actual threshold used

+%    mag = edge magnitude

+%    mage = phase map

+%    g = gradient map at each pixel

+%    [FIe,FIo] = odd and even filter outputs

+%    mago = odd filter output of optimum orientation

+

+% Stella X. Yu, 2001

+

+function [x,y,gx,gy,par,threshold,mag,mage,g,FIe,FIo,mago] = quadedgep(I,par,threshold);

+

+if nargin<3 | isempty(threshold),

+    threshold = 0.2;

+end

+

+[r,c] = size(I);

+def_par = [8,1,20,3];

+

+% take care of parameters, any missing value is substituted by a default value

+if nargin<2 | isempty(par),

+   par = def_par;

+end

+par(end+1:4)=0;

+par = par(:);

+j = (par>0);

+have_value = [ j, 1-j ];

+j = 1; n_filter = have_value(j,:) * [par(j); def_par(j)];

+j = 2; n_scale  = have_value(j,:) * [par(j); def_par(j)];

+j = 3; winsz    = have_value(j,:) * [par(j); def_par(j)];

+j = 4; enlong   = have_value(j,:) * [par(j); def_par(j)];

+

+% always make filter size an odd number so that the results will not be skewed

+j = winsz/2;

+if not(j > fix(j) + 0.1),

+    winsz = winsz + 1;

+end

+

+% filter the image with quadrature filters

+FBo = make_filterbank_odd2(n_filter,n_scale,winsz,enlong);

+FBe = make_filterbank_even2(n_filter,n_scale,winsz,enlong);

+n = ceil(winsz/2);

+f = [fliplr(I(:,2:n+1)), I, fliplr(I(:,c-n:c-1))];

+f = [flipud(f(2:n+1,:)); f; flipud(f(r-n:r-1,:))];

+FIo = fft_filt_2(f,FBo,1); 

+FIo = FIo(n+[1:r],n+[1:c],:);

+FIe = fft_filt_2(f,FBe,1);

+FIe = FIe(n+[1:r],n+[1:c],:);

+

+% compute the orientation energy and recover a smooth edge map

+% pick up the maximum energy across scale and orientation

+% even filter's output: as it is the second derivative, zero cross localize the edge

+% odd filter's output: orientation

+mag = sqrt(sum(FIo.^2,3)+sum(FIe.^2,3));

+mag_a = sqrt(FIo.^2+FIe.^2);

+[tmp,max_id] = max(mag_a,[],3);

+base_size = r * c;

+id = [1:base_size]';

+mage = reshape(FIe(id+(max_id(:)-1)*base_size),[r,c]);

+mage = (mage>0) - (mage<0);

+

+ori_incr=pi/n_filter; % to convert jshi's coords to conventional image xy

+ori_offset=ori_incr/2;

+theta = ori_offset+([1:n_filter]-1)*ori_incr; % orientation detectors

+% [gx,gy] are image gradient in image xy coords, winner take all

+mago = reshape(FIo(id+(max_id(:)-1)*base_size),[r,c]);

+ori = theta(max_id);

+ori = ori .* (mago>0) + (ori + pi).*(mago<0);

+gy = mag .* cos(ori);

+gx = -mag .* sin(ori);

+g = cat(3,gx,gy);

+

+% phase map: edges are where the phase changes

+mag_th = max(mag(:)) * threshold;

+eg = (mag>mag_th);

+h = eg & [(mage(2:r,:) ~= mage(1:r-1,:)); zeros(1,c)];

+v = eg & [(mage(:,2:c) ~= mage(:,1:c-1)), zeros(r,1)];

+[y,x] = find(h | v);

+k = y + (x-1) * r;

+h = h(k);

+v = v(k);

+y = y + h * 0.5; % i

+x = x + v * 0.5; % j

+t = h + v * r;

+gx = g(k) + g(k+t);

+k = k + (r * c);

+gy = g(k) + g(k+t);

+

+% display

+if 1,

+%figure; showmask(I,mage<0);

+figure(2); clf;showim(I,1); hold on; quiver(x,y,gx,gy);

+%figure; showim(-I,1); hold on; quiver(i,j,ex,ey);

+end

diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/readpcm.m b/SD-VBS/common/toolbox/toolbox_basic/common/readpcm.m
new file mode 100755
index 0000000..ca736da
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/readpcm.m
@@ -0,0 +1,12 @@
+function I = readpcm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d\n',2);
+I = fscanf(fid,'%c',A(2)*A(1));
+I = I';
+I = str2num(I);
+I = reshape(I,A(2),A(1))';
+
+
+fclose(fid);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/readpdm.m b/SD-VBS/common/toolbox/toolbox_basic/common/readpdm.m
new file mode 100755
index 0000000..9a1068e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/readpdm.m
@@ -0,0 +1,8 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%d',[A(1),A(2)]);
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/readpfm.m b/SD-VBS/common/toolbox/toolbox_basic/common/readpfm.m
new file mode 100755
index 0000000..48ecd78
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/readpfm.m
@@ -0,0 +1,10 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%f',[A(1),A(2)]);
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/renormalize.m b/SD-VBS/common/toolbox/toolbox_basic/common/renormalize.m
new file mode 100755
index 0000000..5d84724
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/renormalize.m
@@ -0,0 +1,32 @@
+function W2 = renormalize(W,nstep)
+%
+%  keep renormalizing until W is almost double
+%  stocastic
+%
+
+if nargin<2,
+ nstep = 5;
+end
+
+n_node = size(W,1);
+
+for j=1:nstep,
+ fprintf(',');
+ % normalize row 
+ D = sum(W,2);
+ D = 1./(D+eps);
+ W = W.*D(:,ones(1,n_node));
+ 
+ % normlize column
+ D = sum(W,1);
+ D = 1./(D+eps);
+ W = W.*D(ones(n_node,1),:);
+end
+fprintf('\n');
+
+ D = sum(W,2);
+ D = 1./(D+eps);
+ W2 = W.*D(:,ones(1,n_node));
+
+
+ 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/show_edge.m b/SD-VBS/common/toolbox/toolbox_basic/common/show_edge.m
new file mode 100755
index 0000000..63b2f98
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/show_edge.m
@@ -0,0 +1,11 @@
+function [id_i,id_j,ids] = show_edge(I,MI,thI);
+%
+% show_edge(I,MI,thI);
+%
+ 
+[id_i,id_j,tmp] = find(MI);
+ids = sub2ind(size(I),id_i,id_j);
+clf;im(I);colormap(gray);hold on;
+quiver(id_j,id_i,-sin(thI(ids)),cos(thI(ids)),0.5);hold off;
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.c b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.c
new file mode 100755
index 0000000..a98dc0a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.c
@@ -0,0 +1,141 @@
+/*================================================================
+* spmtimesd.c
+* This routine computes a sparse matrix times a diagonal matrix
+* whose diagonal entries are stored in a full vector.
+*
+* Examples: 
+*     spmtimesd(m,d,[]) = diag(d) * m,
+*     spmtimesd(m,[],d) = m * diag(d)
+*     spmtimesd(m,d1,d2) = diag(d1) * m * diag(d2)
+*     m could be complex, but d is assumed real
+*
+* Stella X. Yu's first MEX function, Nov 9, 2001.
+
+% test sequence:
+
+m = 1000;
+n = 2000;
+a=sparse(rand(m,n));
+d1 = rand(m,1);
+d2 = rand(n,1);
+tic; b=spmtimesd(a,d1,d2); toc
+tic; bb = spdiags(d1,0,m,m) * a * spdiags(d2,0,n,n); toc
+e = (bb-b);
+max(abs(e(:)))
+
+*=================================================================*/
+
+# include "mex.h"
+
+void mexFunction(
+    int nargout,
+    mxArray *out[],
+    int nargin,
+    const mxArray *in[]
+)
+{
+    /* declare variables */
+    int i, j, k, m, n, nzmax, cmplx, xm, yn;
+    int *pir, *pjc, *qir, *qjc;
+    double *x, *y, *pr, *pi, *qr, *qi;
+    
+    /* check argument */
+    if (nargin != 3) {
+        mexErrMsgTxt("Three input arguments required");
+    }
+    if (nargout>1) {
+        mexErrMsgTxt("Too many output arguments.");
+    }
+    if (!(mxIsSparse(in[0]))) {
+        mexErrMsgTxt("Input argument #1 must be of type sparse");
+    }
+    if ( mxIsSparse(in[1]) || mxIsSparse(in[2]) ) {
+        mexErrMsgTxt("Input argument #2 & #3 must be of type full");
+    }
+  
+    /* computation starts */
+    m = mxGetM(in[0]);
+    n = mxGetN(in[0]);
+    pr = mxGetPr(in[0]);
+    pi = mxGetPi(in[0]);
+    pir = mxGetIr(in[0]);
+    pjc = mxGetJc(in[0]);
+ 
+    i = mxGetM(in[1]); 
+    j = mxGetN(in[1]);
+    xm = ((i>j)? i: j);
+
+    i = mxGetM(in[2]); 
+    j = mxGetN(in[2]);
+    yn = ((i>j)? i: j);
+   
+    if ( xm>0 && xm != m) {
+        mexErrMsgTxt("Row multiplication dimension mismatch.");
+    }
+    if ( yn>0 && yn != n) {
+        mexErrMsgTxt("Column multiplication dimension mismatch.");
+    }
+    
+ 
+    nzmax = mxGetNzmax(in[0]);
+    cmplx = (pi==NULL ? 0 : 1);    
+    out[0] = mxCreateSparse(m,n,nzmax,cmplx);
+    if (out[0]==NULL) {
+        mexErrMsgTxt("Not enough space for the output matrix.");
+    }    
+   
+    qr = mxGetPr(out[0]);
+    qi = mxGetPi(out[0]);
+    qir = mxGetIr(out[0]);
+    qjc = mxGetJc(out[0]);
+    
+    /* left multiplication */
+    x = mxGetPr(in[1]);
+    if (yn==0) {
+        for (j=0; j<n; j++) {
+            qjc[j] = pjc[j];
+            for (k=pjc[j]; k<pjc[j+1]; k++) {
+                i = pir[k];   
+                qir[k] = i;
+                 qr[k] = x[i] * pr[k];
+                 if (cmplx) {
+                     qi[k] = x[i] * pi[k];
+                 }
+            }
+        }
+        qjc[n] = k;
+        return;
+    }
+    
+    /* right multiplication */
+    y = mxGetPr(in[2]);
+    if (xm==0) {
+        for (j=0; j<n; j++) {
+            qjc[j] = pjc[j];
+            for (k=pjc[j]; k<pjc[j+1]; k++) {
+                qir[k] = pir[k];
+                qr[k]  = pr[k] * y[j];
+                if (cmplx) {
+                    qi[k] = qi[k] * y[j];
+                }
+            }
+       }
+       qjc[n] = k;
+       return;
+   }
+    
+   /* both sides */
+   for (j=0; j<n; j++) {
+       qjc[j] = pjc[j];
+       for (k=pjc[j]; k<pjc[j+1]; k++) {
+           i = pir[k];
+           qir[k]= i;
+           qr[k] = x[i] * pr[k] * y[j];
+           if (cmplx) {
+               qi[k] = x[i] * qi[k] * y[j];      
+           }
+       }
+       qjc[n] = k;
+   }
+   
+}   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexa64 b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexa64
new file mode 100755
index 0000000..15fa9d2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexa64
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexglx b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexglx
new file mode 100755
index 0000000..7478fb0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/spmtimesd.mexglx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/tmp.tex b/SD-VBS/common/toolbox/toolbox_basic/common/tmp.tex
new file mode 100755
index 0000000..0458ef3
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/tmp.tex
@@ -0,0 +1,16 @@
+Hi Prof. Geiger,
+
+        My name is Jianbo Shi.  I am a graduate student working
+with Jitendra Malik at UC Berkeley.    My current research interest is
+on the topic of grouping and perceptual organization.  I have read
+many of your works in this area.
+
+        I will be giving a talk at NECI and David Sarnoff Lab on Dec. 8
+and 9 on ``Normalized cuts and its application to motion segmentation''.
+I am wondering if I could visit you and your lab on Dec. 10.  I would
+very much like the opportunity to talk to you on varies topics related
+to the problem of grouping.  Let me know if that is possible.
+
+        -thanks!
+
+                        -Jianbo
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/vmquant.m b/SD-VBS/common/toolbox/toolbox_basic/common/vmquant.m
new file mode 100755
index 0000000..ab4eb28
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/vmquant.m
@@ -0,0 +1,112 @@
+function [im, map] = vmquant(arg1,arg2,arg3,arg4,arg5,arg6,arg7)
+%VMQUANT Variance Minimization Color Quantization.
+%       [X, MAP] = VMQUANT(R,G,B,K,[Qr Qg Qb],DITHER,Qe) or
+%       VMQUANT(RGB,K,[Qr Qg Qb],DITHER,Qe), where RGB is a 3-D array, 
+%       converts an arbitrary image comprised of RGB triples into an 
+%       indexed image X with color map MAP.  K specifies the number 
+%       of desired entries in the target color map, and [Qr Qg Qb] 
+%       specifies the number of quantization bits to assign each color 
+%       axis during color interpolation.  DITHER is a string ('dither' or
+%       'nodither') that indicates whether or not to perform error propagation 
+%       dither on the output image.  Qe specifies the number of bits of
+%       quantization used in the error calculations.
+%
+%       K is optional and defaults to 256.
+%       [Qr Qg Qb] is optional and defaults to [5 5 5].
+%       DITHER is optional and defaults to 'nodither'.
+%       Qe is optional and defaults to 8.
+%
+%       See also: RGB2IND, RGB2GRAY, DITHER, IND2RGB, CMUNIQUE, IMAPPROX.
+
+%       This is the wrapper function for the MEX file VMQUANTC.C
+
+%       Joseph M. Winograd 6-93
+%       Copyright (c) 1993 by The MathWorks, Inc.
+%       $Revision: 5.3 $  $Date: 1996/08/22 22:09:03 $
+
+%       Reference: Xiaolin Wu, "Efficient Statistical Computation for
+%       Optimal Color Quantization," Graphics Gems II, (ed. James
+%       Arvo).  Academic Press: Boston. 1991.
+
+if nargin < 1,
+  error('Not enough input arguments.');
+end
+
+threeD = (ndims(arg1)==3); % Determine if input includes a 3-D array
+
+if threeD,
+  error( nargchk( 1, 5, nargin ) );
+
+                        % NOTE: If you change defaults, change them also
+                        %       in VMQUANTC.C and recompile the MEX function.
+  if nargin < 5
+      arg5 = 8;         % DEFAULT_QE = 8
+  end
+
+  if nargin < 4
+      arg4 = 'n';               % DEFAULT_DITHER = 0
+  end
+
+  if nargin < 3
+      arg3 = [5 5 5];   % DEFAULT_Q = [5 5 5]
+  end
+
+  if nargin < 2
+      arg2 = 256;               % DEFAULT_K = 256
+  end
+
+  rout = arg1(:,:,1);
+  g = arg1(:,:,2);
+  b = arg1(:,:,3); 
+
+  if strcmp(lower(arg4(1)),'d')
+    dith = 1;
+  else
+    dith = 0;
+  end
+  
+  arg7 = arg5;
+  arg5 = arg3;
+  arg4 = arg2;
+
+else
+  error( nargchk( 3, 7, nargin ) );
+
+  if nargin < 7
+      arg7 = 8;         % DEFAULT_QE = 8
+  end
+
+  if nargin < 6
+      arg6 = 'n';               % DEFAULT_DITHER = 0
+  end
+
+  if nargin < 5
+      arg5 = [5 5 5];   % DEFAULT_Q = [5 5 5]
+  end
+
+  if nargin < 4
+      arg4 = 256;               % DEFAULT_K = 256
+  end
+
+  rout = arg1;
+  g = arg2;
+  b = arg3;
+
+  if strcmp(lower(arg6(1)),'d')
+    dith = 1;
+  else
+    dith = 0;
+  end
+
+end
+
+if (~isa(rout,'uint8'))
+  rout = uint8(round(255*rout));
+end
+if (~isa(g,'uint8'))
+  g = uint8(round(255*g));
+end
+if (~isa(b,'uint8'))
+  b = uint8(round(255*b));
+end
+[im,map] = vmquantc( rout, g, b, arg4, arg5, dith, arg7 );
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexhp7 b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexhp7
new file mode 100755
index 0000000..c0e35c4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexhp7
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexlx b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexlx
new file mode 100755
index 0000000..f79c1d3
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexlx
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexsol b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexsol
new file mode 100755
index 0000000..f711a98
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/vmquantc.mexsol
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/writepdm.m b/SD-VBS/common/toolbox/toolbox_basic/common/writepdm.m
new file mode 100755
index 0000000..a8cba11
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/writepdm.m
@@ -0,0 +1,11 @@
+function writepfm(name,I)
+%
+%  writepfm(name,I)
+%
+  [nr,nc] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, '%d %d\n', nr,nc);
+  fprintf(fid,'%d ',I');
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/common/writepfm.m b/SD-VBS/common/toolbox/toolbox_basic/common/writepfm.m
new file mode 100755
index 0000000..ee25fdc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/common/writepfm.m
@@ -0,0 +1,11 @@
+function writepfm(name,I)
+%
+%  writepfm(name,I)
+%
+  [nr,nc] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, '%d %d\n', nr,nc);
+  fprintf(fid,'%f ',I');
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/disp_image.m b/SD-VBS/common/toolbox/toolbox_basic/disp/disp_image.m
new file mode 100755
index 0000000..4745d8d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/disp_image.m
@@ -0,0 +1,19 @@
+function disp_image(img,sze,index,mask)
+
+%figure(2)
+subplot(sze(1),sze(2),index);
+
+if (size(mask) ~= size(mask)),
+ error(['size of image is ',int2str(size(mask)),' size of mask is ',...
+        int2str(size(mask))]);
+end
+
+img = img-min(min(img));
+I = 0*(img.*(~mask)) + img.*mask;
+I = img.*mask;
+colormap(gray)
+imagesc(I);
+%axis('off')
+axis('equal');
+axis('square');
+drawnow;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box.m b/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box.m
new file mode 100755
index 0000000..556afdc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box.m
@@ -0,0 +1,9 @@
+function draw_box(left,right,top,down)
+
+plot([left,right],[top,top]);
+hold on
+plot([left,right],[down,down]);
+hold on
+plot([left,left],[top,down]);
+hold on
+plot([right,right],[top,down]);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box2.m b/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box2.m
new file mode 100755
index 0000000..adec284
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/draw_box2.m
@@ -0,0 +1,17 @@
+function draw_box(center,w_h,j)
+
+
+center_x = center(1);
+center_y = center(2);
+
+%plot(center_x,center_y,'r*');
+%text(center_x,center_y,int2str(j));
+
+l_x = center_x-w_h(1);
+r_x = center_x+w_h(1);
+u_y = center_y-w_h(2);
+l_y = center_y+w_h(2);
+
+plot([l_x,r_x,r_x,l_x,l_x],[u_y,u_y,l_y,l_y,u_y],'c');
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/im.m b/SD-VBS/common/toolbox/toolbox_basic/disp/im.m
new file mode 100755
index 0000000..a707916
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/im.m
@@ -0,0 +1,8 @@
+function im(I)
+
+imagesc(I);drawnow;
+pixval on
+title(inputname(1))
+%colormap(gray)
+%colorbar
+axis('image')
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/ims.m b/SD-VBS/common/toolbox/toolbox_basic/disp/ims.m
new file mode 100755
index 0000000..2fb5f25
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/ims.m
@@ -0,0 +1,3 @@
+function ims(I,nr,nc)
+
+im(reshape(I,nr,nc));
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/montage2.m b/SD-VBS/common/toolbox/toolbox_basic/disp/montage2.m
new file mode 100755
index 0000000..fb04b0b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/montage2.m
@@ -0,0 +1,17 @@
+function montage2(FI)
+% montage2(FI)
+% display 3D stack of images in a montage
+
+[N1,N2,N3]=size(FI);
+Q=zeros(N1,N2,1,N3);
+for n=1:N3
+   Q(:,:,1,n)=FI(:,:,n);
+end
+
+a=min(Q(:));
+b=max(Q(:));
+
+Q=Q-a;
+Q=Q/(b-a);
+
+montage(Q);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/showmask.m b/SD-VBS/common/toolbox/toolbox_basic/disp/showmask.m
new file mode 100755
index 0000000..f301638
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/showmask.m
@@ -0,0 +1,20 @@
+function showmask(V,M,display_flag);
+% showmask(V,M);
+%
+% M is a nonneg. mask
+
+V=V-min(V(:));
+V=V/max(V(:));
+V=.25+0.75*V; %brighten things up a bit
+
+M=M-min(M(:));
+M=M/max(M(:));
+
+H=0.0+zeros(size(V));
+S=M;
+RGB=hsv2rgb(H,S,V);
+
+%if nargin>2
+   image(RGB)
+   axis('image')
+%end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/disp/showmaskb.m b/SD-VBS/common/toolbox/toolbox_basic/disp/showmaskb.m
new file mode 100755
index 0000000..1f67ba2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/disp/showmaskb.m
@@ -0,0 +1,20 @@
+function RGB = showmask(V,M,M2,display_flag);
+% showmask(V,M);
+%
+% M is a nonneg. mask
+
+V=V-min(V(:));
+V=V/max(V(:));
+V=.25+0.75*V; %brighten things up a bit
+
+M=M-min(M(:));
+M=M/max(M(:));
+
+H=0.6*M2+0*M;
+S=min(1,M2+M);
+RGB=hsv2rgb(H,S,V);
+
+%if nargin>2
+   image(RGB)
+   axis('image')
+%end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m
new file mode 100755
index 0000000..372b4b6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w.m
@@ -0,0 +1,25 @@
+function W = construct_w(centers,Ds,img_center,indexes,frames)
+%
+%  function W = construct_w(centers,Ds,img_center,indexes,frames)
+%   optional: frames
+%  
+
+
+points = length(indexes);
+if (nargin == 4),
+ frames = 0.5*size(centers,2);
+end
+
+W = zeros(2*frames,points);
+
+center_x = img_center(1);
+center_y = img_center(2);
+
+for j=1:frames,
+  % x is centers(:,2*j-1)
+  % y is centers(:,2*j)
+  % d is Ds(:,2*j-1)
+  W(j,:) = (centers(indexes,2*j-1) -center_x)'./Ds(indexes,2*j-1)';
+  W(j+frames,:) = (centers(indexes,2*j) -center_y)'./Ds(indexes,2*j-1)'; 
+ % W(j+2*frames,:) = ones(1,points)./Ds(indexes,2*j-1)';
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m
new file mode 100755
index 0000000..b2939b7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/construct_w2.m
@@ -0,0 +1,25 @@
+function W = construct_w2(centers,Ds,img_center,indexes,frames)
+%
+%  function W = construct_w2(centers,Ds,img_center,indexes,frames)
+%   optional: frames
+%  
+
+
+points = length(indexes);
+if (nargin == 4),
+ frames = 0.5*size(centers,2);
+end
+
+W = zeros(3*frames,points);
+
+center_x = img_center(1);
+center_y = img_center(2);
+
+for j=1:frames,
+  % x is centers(:,2*j-1)
+  % y is centers(:,2*j)
+  % d is Ds(:,2*j-1)
+  W(j,:) = (centers(indexes,2*j-1) -center_x)'./Ds(indexes,2*j-1)';
+  W(j+frames,:) = (centers(indexes,2*j) -center_y)'./Ds(indexes,2*j-1)'; 
+  W(j+2*frames,:) = ones(1,points)./Ds(indexes,2*j-1)';
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m
new file mode 100755
index 0000000..635c29a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor.m
@@ -0,0 +1,50 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+G = findG(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m
new file mode 100755
index 0000000..12ceb95
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test.m
@@ -0,0 +1,52 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S,C,b] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+
+figure(3);plot(diag(b))
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+[G,C] = findg1(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m
new file mode 100755
index 0000000..3520122
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factor_test2.m
@@ -0,0 +1,52 @@
+%
+% Usage:
+%
+%    [R,t,S] = factor(W)
+%
+% Function to factor a matrix of input data (W) into the camera
+% rotation matrix (R), translation (t), and the shape matrix (S).
+% Three-dimensional version. Failure of normalization results in
+% empty R and S.
+
+function [R,t,S,C,b] = factor(W)
+
+pts = size(W,2);
+t = W*ones(pts,1)/pts;
+W = W - t*ones(1,pts);
+
+% Use SVD to factor W. 
+   [a,b,c] = svd(W,0);
+
+figure(3);plot(diag(b))
+ 
+smallb = b(1:3,1:3);    % Since W is rank 3, b has only three meaningful values
+sqrtb = sqrt(smallb);
+Rhat = a(:,1:3) * sqrtb;
+Shat = sqrtb * c(:,1:3)';
+
+[G,C] = findg2(Rhat);
+
+if size(G,1) == 0,
+R = [];
+S = [];
+else
+  R = Rhat*G;
+  S = inv(G)*Shat;
+
+  % rotation matrix that aligns the reference frame with the first camera
+  F = size(R,1)/2;
+  R1 = R(1,:);
+  R1 = R1/norm(R1);
+  R2 = R(F+1,:);
+  R2 = R2/norm(R2);
+  R3 = cross(R1,R2);
+  R3 = R3/norm(R3);
+  P = [R1; R2; R3];
+  P = P';
+
+  R = R*P;
+  S = inv(P)*S;
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar b/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar
new file mode 100755
index 0000000..133cdff
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/factorizaion.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m
new file mode 100755
index 0000000..9a6bd73
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findG.m
@@ -0,0 +1,48 @@
+function G = find3G(Rhat)
+
+% number of frames
+F = size(Rhat,1)/2;
+
+% Build matrix Q such that Q * v = [1,...,1,0,...,0] where v is a six
+% element vector containg all six distinct elements of the Matrix C
+
+clear Q
+for f = 1:F,
+  g = f + F;
+  h = g + F;
+  Q(f,:) = zt(Rhat(f,:), Rhat(f,:));
+  Q(g,:) = zt(Rhat(g,:), Rhat(g,:));
+  Q(h,:) = zt(Rhat(f,:), Rhat(g,:));
+end
+
+% Solve for v
+rhs = [ones(2*F,1); zeros(F,1)];
+v = Q \ rhs;
+
+% C is a symmetric 3x3 matrix such that C = G * transpose(G)
+C(1,1) = v(1);                  
+C(1,2) = v(2);                  
+C(1,3) = v(3);                  
+C(2,2) = v(4);
+C(2,3) = v(5);
+C(3,3) = v(6);
+C(2,1) = C(1,2);
+C(3,1) = C(1,3);
+C(3,2) = C(2,3);
+
+e = eig(C);
+disp(e)
+
+if (any(e<= 0)),
+  G = [];
+else
+  G = sqrtm(C);
+end
+
+%neg = 0;
+%if e(1) <= 0, neg = 1; end
+%if e(2) <= 0, neg = 1; end
+%if e(3) <= 0, neg = 1; end
+%if neg == 1, G = [];
+%else G = sqrtm(C);
+%end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m
new file mode 100755
index 0000000..f14ecc6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findg1.m
@@ -0,0 +1,49 @@
+function [G,C] = find3G(Rhat)
+
+% number of frames
+F = size(Rhat,1)/2;
+
+% Build matrix Q such that Q * v = [1,...,1,0,...,0] where v is a six
+% element vector containg all six distinct elements of the Matrix C
+
+clear Q
+for f = 1:F,
+  g = f + F;
+  h = g + F;
+  Q(f,:) = zt(Rhat(f,:), Rhat(f,:));
+  Q(g,:) = zt(Rhat(g,:), Rhat(g,:));
+  Q(h,:) = zt(Rhat(f,:), Rhat(g,:));
+end
+
+% Solve for v
+rhs = [ones(2*F,1); zeros(F,1)];
+v = Q \ rhs;
+
+% C is a symmetric 3x3 matrix such that C = G * transpose(G)
+C(1,1) = v(1);                  
+C(1,2) = v(2);                  
+C(1,3) = v(3);                  
+C(2,2) = v(4);
+C(2,3) = v(5);
+C(3,3) = v(6);
+C(2,1) = C(1,2);
+C(3,1) = C(1,3);
+C(3,2) = C(2,3);
+
+e = eig(C);
+disp(e)
+
+if (any(e<= 0)),
+  C = C -2*min(e)*eye(3);
+  G = sqrtm(C);
+else
+  G = sqrtm(C);
+end
+
+%neg = 0;
+%if e(1) <= 0, neg = 1; end
+%if e(2) <= 0, neg = 1; end
+%if e(3) <= 0, neg = 1; end
+%if neg == 1, G = [];
+%else G = sqrtm(C);
+%end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m b/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m
new file mode 100755
index 0000000..5a84b86
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/findg2.m
@@ -0,0 +1,56 @@
+function [G,C] = find3G(Rhat)
+
+% number of frames
+F = size(Rhat,1)/3;
+
+% Build matrix Q such that Q * v = [1,...,1,0,...,0] where v is a six
+% element vector containg all six distinct elements of the Matrix C
+
+clear Q
+for f = 1:F,
+  g = f + F;
+  h = g + F;
+  j = h + F;
+  k = j + F;
+  l = k + F;
+  Q(f,:) = zt(Rhat(f,:), Rhat(f,:));
+  Q(g,:) = zt(Rhat(g,:), Rhat(g,:));
+  Q(h,:) = zt(Rhat(h,:), Rhat(h,:));
+  Q(j,:) = zt(Rhat(f,:), Rhat(g,:));
+  Q(k,:) = zt(Rhat(f,:), Rhat(h,:));
+  Q(l,:) = zt(Rhat(g,:), Rhat(h,:));
+end
+
+% Solve for v
+rhs = [ones(3*F,1); zeros(3*F,1)];
+v = Q \ rhs;
+
+% C is a symmetric 3x3 matrix such that C = G * transpose(G)
+C(1,1) = v(1);                  
+C(1,2) = v(2);                  
+C(1,3) = v(3);                  
+C(2,2) = v(4);
+C(2,3) = v(5);
+C(3,3) = v(6);
+C(2,1) = C(1,2);
+C(3,1) = C(1,3);
+C(3,2) = C(2,3);
+
+e = eig(C);
+disp(e)
+
+
+if (any(e<= 0)),
+  C = C -2*min(e)*eye(3);
+  G = sqrtm(C);
+else
+  G = sqrtm(C);
+end
+
+%neg = 0;
+%if e(1) <= 0, neg = 1; end
+%if e(2) <= 0, neg = 1; end
+%if e(3) <= 0, neg = 1; end
+%if neg == 1, G = [];
+%else G = sqrtm(C);
+%end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat b/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat
new file mode 100755
index 0000000..61ea6c8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/hotel.mat
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m
new file mode 100755
index 0000000..b6edfd5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_3dpoints.m
@@ -0,0 +1,22 @@
+function show_3dpoints(S)
+
+
+for j=1:size(S,2),
+  x = S(1,j);
+  y = S(2,j);
+  z = S(3,j);
+  plot3(x,y,z,'*');
+  hold on;
+  plot3([x,0],[y,0],[z,0],'r');
+%  plot3([x,x],[y,y],[z,0],'r');
+%  plot3([x,0],[y,y],[z,z],'r'); plot3([x,x],[y,0],[z,z],'r');
+  text(x,y,z,int2str(j))
+%  plot3(x,y,0,'co');
+end
+
+grid on
+xlabel('x');
+ylabel('y');
+zlabel('z');
+
+hold off
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m
new file mode 100755
index 0000000..5828696
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_S.m
@@ -0,0 +1,17 @@
+function show_S(S,fig)
+
+if (nargin == 1),
+  figure(1);
+else
+  figure(fig);
+end
+
+num_points = size(S,2);
+
+subplot(1,2,1); plot(S(1,:),S(3,:),'cx'); axis('equal');axis('square');hold on
+subplot(1,2,2); plot(S(2,:),S(3,:),'cx'); axis('equal');axis('square');hold on
+
+for j=1:num_points,
+ subplot(1,2,1);text(S(1,j),S(3,j),int2str(j));hold off
+ subplot(1,2,2);text(S(2,j),S(3,j),int2str(j));hold off
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m
new file mode 100755
index 0000000..b475c76
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t.m
@@ -0,0 +1,10 @@
+function show_t(t)
+
+frames = 0.5*length(t);
+
+ts = reshape(t,frames,2);
+
+plot(ts(:,1),ts(:,2));
+hold on
+plot(ts(:,1),ts(:,2),'rx');
+hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m
new file mode 100755
index 0000000..2766061
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/show_t3.m
@@ -0,0 +1,10 @@
+function show_t(t)
+
+frames = length(t)/3;
+
+ts = reshape(t,frames,3);
+
+plot3(ts(:,1),ts(:,2),ts(:,3));
+hold on
+plot3(ts(:,1),ts(:,2),ts(:,3),'rx');
+hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m b/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m
new file mode 100755
index 0000000..3f88d21
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/fact/zt.m
@@ -0,0 +1,6 @@
+% the z' operator described in the paper (returns a row vector)
+
+function v = zt(a, b)
+
+v = [   a(1)*b(1), a(1)*b(2)+a(2)*b(1), a(1)*b(3)+a(3)*b(1), ...
+        a(2)*b(2), a(2)*b(3)+a(3)*b(2), a(3)*b(3) ];
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/91048.jpg b/SD-VBS/common/toolbox/toolbox_basic/filter/91048.jpg
new file mode 100755
index 0000000..6b2313b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/91048.jpg
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/bar2d.m b/SD-VBS/common/toolbox/toolbox_basic/filter/bar2d.m
new file mode 100755
index 0000000..76fa819
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/bar2d.m
@@ -0,0 +1,16 @@
+function kernel = bar2d(sigx,sigy,siz,angle)
+
+X = -siz:.1:siz;
+G = exp(-0.5*X.^2/sigx^2);
+
+DGG = (1/sigy^2) * ((X/sigy).^2-1) .* exp(- (X/sigy).^2/2);
+%DGG = (X.^2/(sqrt(2*pi)*sigy^5) - 1/(sqrt(2*pi)*sigy^2)) .* ...
+%      exp(-0.5*X.^2/sigy^2);
+
+K = G'*DGG;
+K = rotate_J(angle,K);
+
+K = imresize(K,0.1);
+K = K-mean(mean(K));
+
+kernel = K;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/barrot.m b/SD-VBS/common/toolbox/toolbox_basic/filter/barrot.m
new file mode 100755
index 0000000..bd7676e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/barrot.m
@@ -0,0 +1,22 @@
+img1 = gifread('color010.gif');
+img1 = img1(220:350,1:200);    
+img2 = gifread('color-avg.gif');
+img2 = img2(220:350,1:200); 
+
+sigx = 3;
+sigy = 3;
+siz = 8;
+angles = 0:19:179;
+
+[Imag1,Iangle1] = brute_force_angle(img1,sigx,sigy,siz,angles);
+[Imag2,Iangle2] = brute_force_angle(img2,sigx,sigy,siz,angles);
+
+tresh = max(max(Imag1))*0.1
+D = angle_diff(Imag1,Iangle1,Imag2,Iangle2,tresh);
+subplot(2,2,1); imagesc(Iangle1*180/pi); colorbar;          
+subplot(2,2,2); imagesc(Iangle2*180/pi); colorbar;          
+subplot(2,2,3); imagesc(D*180/pi); colorbar;          
+subplot(2,2,4); imagesc(Imag1); colorbar;
+colormap(jet)
+          
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/bars.m b/SD-VBS/common/toolbox/toolbox_basic/filter/bars.m
new file mode 100755
index 0000000..77eccea
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/bars.m
@@ -0,0 +1,39 @@
+function [filt] = bars(X,Y,ks);
+%FIL1   the first filter to use has the following specifications:
+%
+%       real part: 2nd derivative of gaussian along Y
+%                  normal gaussian along X
+%       This filter is elongated along the X direction
+%       imag part: hilbert transform of the real part
+%
+%       [filt] = fil1(X,Y,ks);
+%       X,Y  : index matrix obtained by meshgrid
+%       ks   : kernel size
+%       filt : the output kernel
+%
+
+%%
+%%      (c) Thomas Leung
+%%      California Institute of Technology
+%%      Feb 27, 1994.
+%%
+
+if(nargin == 2)
+        ks = 17;
+end
+
+sigmay = 2.4 * ks / 17;
+sigmax = 3 * sigmay;
+
+fxr = exp(-(X/sigmax).^2/2);
+fyr = (1/sigmay^2) * ((Y/sigmay).^2-1) .* exp(- (Y/sigmay).^2/2);
+nrm = 1/(sigmax*sigmay*2*pi);
+
+% real part of filter
+fr = nrm * fxr .* fyr;
+
+% imag part of filter
+filt = hilbert(fr);
+
+
+return;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/clip_image.m b/SD-VBS/common/toolbox/toolbox_basic/filter/clip_image.m
new file mode 100755
index 0000000..bf7b50c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/clip_image.m
@@ -0,0 +1,6 @@
+function J = clip_image(I,w)
+
+[size_y,size_x] = size(I);
+
+J = I(w+1:size_y-w,w+1:size_x-w);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_J_simple.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_J_simple.m
new file mode 100755
index 0000000..e790601
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_J_simple.m
@@ -0,0 +1,50 @@
+function J = compute_J(I,A,D,base)
+%%  function J = compute_J(I,A,D)
+%
+
+if nargin == 3,
+  base = -1;
+end
+
+[size_y,size_x] = size(I);
+[center_x,center_y] = find_center(size_x,size_y);
+
+add_x = round(size_x*0.45);
+add_y = round(size_y*0.45);
+big_I = base*ones(size_y+2*add_y,size_x+2*add_x);
+
+big_I(add_y+1:add_y+size_y,add_x+1:add_x+size_x) = I;
+
+center_x = add_x+ center_x;
+center_y = add_y+ center_y;
+[size_y,size_x] = size(big_I);
+
+%a = angle * pi/180;
+%A = [cos(a),-sin(a);sin(a),cos(a)];
+
+[XX,YY] = meshgrid(1:size_x,1:size_y);
+
+x = reshape(XX,size_x*size_y,1);
+y = reshape(YY,size_x*size_y,1);
+index(:,1) = x-center_x;
+
+%index(:,2) = (size_y+1) - y;
+index(:,2) = y-center_y;
+
+position_new = A*index';
+position_new(1,:) = position_new(1,:)+D(1)+center_x;
+position_new(2,:) = position_new(2,:)+D(2)+center_y;
+%position_new(2,:) = (size_y+1) - position_new(2,:);
+
+position_new_x = reshape(position_new(1,:),size_y,size_x);
+position_new_y = reshape(position_new(2,:),size_y,size_x);
+
+J = m_interp4(big_I,position_new_x,position_new_y);
+
+
+[size_y,size_x] = size(I);
+J = J(add_y+1:add_y+size_y,add_x+1:add_x+size_x);
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_angle.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_angle.m
new file mode 100755
index 0000000..7a995af
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_angle.m
@@ -0,0 +1,18 @@
+function [angle,mag,c2,c3] = compute_angle(I)
+
+[g,ga,gb,gc] = compute_g2(I,0); 
+[h,ha,hb,hc,hd] = compute_h2(I,0);
+
+c2 = 0.5*(ga.^2 - gc.^2) + 0.46875*(ha.^2 - hd.^2) +...
+     0.28125*(hb.^2 - hc.^2) + 0.1875*(ha.*hc - hb.*hd);
+
+c3 = -ga.*gb - gb.*gc - 0.9375*(hc.*hd + ha.*hb) -...
+  1.6875*hb.*hc - 0.1875*ha.*hd;
+
+[angle,mag] = cart2pol(-c2,-c3);
+
+%angle = angle/2+pi/2;
+%angle = (angle>pi).*(angle-2*pi) + (angle<=pi).*angle;
+
+angle = angle/2;
+mag = sqrt(mag);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_filter_fft.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_filter_fft.m
new file mode 100755
index 0000000..359c6ba
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_filter_fft.m
@@ -0,0 +1,84 @@
+function [filter_output,filters] = compute_filter_fft(I,sig,r,sz,num_ori);
+%
+%
+%
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+as = ori_offset:ori_incr:180+ori_offset-ori_incr;
+
+filter_output = [];
+filters = [];
+
+wsz = 2*round(sz(end)) + 1;
+M1 = wsz;M2 = wsz;
+
+%%%%% prepare FFT of image  %%%%%%%%%%%%%
+
+[N1,N2]=size(I);
+tmp=zeros(size(I)+[M1-1 M2-1]);
+tmp(1:N1,1:N2)=I;
+IF=fft2(tmp);
+
+
+%%%%%%%%%% filtering stage %%%%%%%%%%%
+if size(sig,2)== 1,
+
+  for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = mdoog2(sig,r,angle,round(sz));
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    filters(:,:,j) = g;
+
+    gF  = fft2(g,N1+M1-1,N2+M2-1);
+    IgF = IF.*gF;
+    Ig  = real(ifft2(IgF));
+    Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+    %c = conv2(I,g,'valid');
+
+    filter_output(:,:,j) = Ig;
+  end
+else
+
+  % there are multiple scales
+  sigs = sig;
+  szs = sz;
+  for k = 1:size(sigs,2),
+     sig = sigs(k);
+     sz = szs(end);
+     fprintf('%d',k);
+     for j = 1:length(as),
+        fprintf('.');
+        angle = as(j);
+
+        g = mdoog2(sig,r,angle,round(sz));
+        g = g - mean(reshape(g,prod(size(g)),1));
+        g = g/sum(sum(abs(g)));
+
+        gF  = fft2(g,N1+M1-1,N2+M2-1);
+        IgF = IF.*gF;
+        Ig  = real(ifft2(IgF));
+        Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+        %c = conv2(I,g,'valid');
+        %c = conv2(I,g,'same');
+
+        filter_output(:,:,j,k) = Ig;
+        filters(:,:,j,k) = g; 
+     end
+
+
+  end
+
+end
+
+fprintf('\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_g2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_g2.m
new file mode 100755
index 0000000..ac27d00
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_g2.m
@@ -0,0 +1,23 @@
+function [g,ga,gb,gc] = compute_g2(I,angle)
+
+if (nargin == 1),
+  angle = 0;
+end
+
+f1 = [0.0094 0.1148 0.3964 -0.0601 -0.9213 -0.0601 0.3964 0.1148 0.0094];
+f2 = [0.0008 0.0176 0.166 0.6383 1.0 0.6383 0.166 0.0176 0.0008];
+f3 = [-0.0028 -0.048 -0.302 -0.5806 0 0.5806 0.302 0.048 0.0028];
+
+%ga = conv2(conv2(I,f2,'same'),f1','same');
+%gb = conv2(conv2(I,f3,'same'),f3','same');
+%gc = conv2(conv2(I,f1,'same'),f2','same');
+
+ga = conv2(conv2(I,f1,'same'),f2','same');
+gb = conv2(conv2(I,f3,'same'),f3','same');
+gc = conv2(conv2(I,f2,'same'),f1','same');
+
+ka = cos(angle)^2;
+kb = -2*cos(angle)*sin(angle);
+kc = sin(angle)^2;
+
+g = ka*ga + kb*gb + kc*gc;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_h2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_h2.m
new file mode 100755
index 0000000..e4cdcfb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_h2.m
@@ -0,0 +1,27 @@
+function [h,ha,hb,hc,hd] = compute_h2(I,angle)
+
+if (nargin == 1),
+  angle = 0;
+end
+
+f1 = [0.0098 0.0618 -0.0998 -0.7551 0 0.7551 0.0998 -0.0618 -0.0098];
+f2 = [ 0.0008 0.0176 0.166 0.6383 1 0.6383 0.166 0.0176 0.0008];
+f3 = -[-0.002 -0.0354 -0.2225 -0.4277 0 0.4277 0.2225 0.0354 0.002];
+f4 = [0.0048 0.0566 0.1695 -0.1889 -0.7349 -0.1889 0.1695 0.0566 0.0048];
+
+%ha = conv2(conv2(I,f2,'same'),f1','same');
+%hb = conv2(conv2(I,f3,'same'),f4','same');
+%hc = conv2(conv2(I,f4,'same'),f3','same');
+%hd = conv2(conv2(I,f1,'same'),f2','same');
+
+ha = conv2(conv2(I,f1,'same'),f2','same');
+hb = conv2(conv2(I,f4,'same'),f3','same');
+hc = conv2(conv2(I,f3,'same'),f4','same');
+hd = conv2(conv2(I,f2,'same'),f1','same');
+
+ka = cos(angle)^3;
+kb = -3*cos(angle)^2*sin(angle);
+kc = 3*cos(angle)*sin(angle)^2;
+kd = -sin(angle)^3;
+
+h = ka*ha + kb*hb + kc*hc + kd*hd;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/compute_ofilter_fft.m b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_ofilter_fft.m
new file mode 100755
index 0000000..41989ed
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/compute_ofilter_fft.m
@@ -0,0 +1,88 @@
+function [filter_output,filters] = compute_odd_filter_fft(I,sig,r,sz,num_ori);
+%
+% computes the filter response of I to the set of odd symmetric filters
+%
+%  sig = scale(sigma) for the filters
+%  r = enlongation factor
+%  sz = the radius of the filters
+%  num_ori = number of orientations
+%
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+as = ori_offset:ori_incr:180+ori_offset-ori_incr;
+
+filter_output = zeros(size(I,1),size(I,2),num_ori,length(sig));
+filters = [];
+
+wsz = 2*round(sz(end)) + 1;
+M1 = wsz;M2 = wsz;
+
+%%%%% prepare FFT of image  %%%%%%%%%%%%%
+
+[N1,N2]=size(I);
+tmp=zeros(size(I)+[M1-1 M2-1]);
+tmp(1:N1,1:N2)=I;
+IF=fft2(tmp);
+
+
+%%%%%%%%%% filtering stage %%%%%%%%%%%
+if size(sig,2)== 1,
+
+  for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = mk_odd_filter(sig,r,angle,round(sz));
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    filters(:,:,j,1) = g;
+
+    gF  = fft2(g,N1+M1-1,N2+M2-1);
+    IgF = IF.*gF;
+    Ig  = real(ifft2(IgF));
+    Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+    %c = conv2(I,g,'valid');
+
+    filter_output(:,:,j,1) = Ig;
+  end
+else
+
+  % there are multiple scales
+  sigs = sig;
+  szs = sz;
+  for k = 1:size(sigs,2),
+     sig = sigs(k);
+     sz = szs(end);
+     fprintf('%d',k);
+     
+     for j = 1:length(as),
+        fprintf('.');
+        angle = as(j);
+
+        g = mk_odd_filter(sig,r,angle,round(sz));
+        g = g - mean(reshape(g,prod(size(g)),1));
+        g = g/sum(sum(abs(g)));
+
+        gF  = fft2(g,N1+M1-1,N2+M2-1);
+        IgF = IF.*gF;
+        Ig  = real(ifft2(IgF));
+        Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+        %c = conv2(I,g,'valid');
+        %c = conv2(I,g,'same');
+
+        filter_output(:,:,j,k) = Ig;
+        filters(:,:,j,k) = g; 
+     end
+  end
+
+end
+
+fprintf('\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/dgauss.m b/SD-VBS/common/toolbox/toolbox_basic/filter/dgauss.m
new file mode 100755
index 0000000..207e781
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/dgauss.m
@@ -0,0 +1,16 @@
+function dg = dgauss(sig)
+% first derivative of N(sig)
+% cutoff after 1% of max
+
+  i = 1;
+  max = 0;
+  dgi = max;
+  dg = [dgi];
+  while dgi >= 0.01*max
+    dgi = i / (sqrt(2*pi) * sig^3) * exp(-0.5*i^2/sig^2);
+    dg = [dgi dg -dgi];
+    i = i + 1;
+    if dgi > max
+      max = dgi;
+     end
+  end;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/dog1.m b/SD-VBS/common/toolbox/toolbox_basic/filter/dog1.m
new file mode 100755
index 0000000..e9f64e6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/dog1.m
@@ -0,0 +1,28 @@
+function G=dog1(sig,N);
+% G=dog1(sig,N);
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+sigi=0.71*sig;
+sigo=1.14*sig;
+Ci=diag([sigi,sigi]);
+Co=diag([sigo,sigo]);
+
+X=[x(:) y(:)];
+
+Ga=gaussian(X,[0 0]',Ci);
+Ga=reshape(Ga,N,N);
+Gb=gaussian(X,[0 0]',Co);
+Gb=reshape(Gb,N,N);
+
+a=1;
+b=-1;
+
+G = a*Ga + b*Gb;
+
+G=G-mean(G(:));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/dog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/dog2.m
new file mode 100755
index 0000000..8446198
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/dog2.m
@@ -0,0 +1,31 @@
+function G=dog2(sig,N);
+% G=dog2(sig,N);
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+sigi=0.62*sig;
+sigo=1.6*sig;
+C=diag([sig,sig]);
+Ci=diag([sigi,sigi]);
+Co=diag([sigo,sigo]);
+
+X=[x(:) y(:)];
+
+Ga=gaussian(X,[0 0]',Ci);
+Ga=reshape(Ga,N,N);
+Gb=gaussian(X,[0 0]',C);
+Gb=reshape(Gb,N,N);
+Gc=gaussian(X,[0 0]',Co);
+Gc=reshape(Gc,N,N);
+
+a=-1;
+b=2;
+c=-1;
+
+G = a*Ga + b*Gb + c*Gc;
+
+G=G-mean(G(:));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/doog1.m b/SD-VBS/common/toolbox/toolbox_basic/filter/doog1.m
new file mode 100755
index 0000000..dd8e87b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/doog1.m
@@ -0,0 +1,32 @@
+function H=doog1(sig,r,th,N);
+% H=doog1(sig,r,th,N);
+
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+phi=pi*th/180;
+sigy=sig;
+sigx=r*sig;
+R=[cos(phi) -sin(phi); sin(phi) cos(phi)];
+C=R*diag([sigx,sigy])*R';
+
+X=[x(:) y(:)];
+
+Gb=gaussian(X,[0 0]',C);
+Gb=reshape(Gb,N,N);
+
+m=R*[0 sig]';
+
+a=1;
+b=-1;
+
+% make odd-symmetric filter
+Ga=gaussian(X,m/2,C);
+Ga=reshape(Ga,N,N);
+Gb=rot90(Ga,2);
+H=a*Ga+b*Gb; 
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/doog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/doog2.m
new file mode 100755
index 0000000..a0511cb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/doog2.m
@@ -0,0 +1,38 @@
+function G=doog2(sig,r,th,N);
+% G=doog2(sig,r,th,N);
+% Make difference of offset gaussians kernel
+% theta is in degrees
+% (see Malik & Perona, J. Opt. Soc. Amer., 1990)
+%
+% Example:
+% >> imagesc(doog2(1,12,0,64,1))
+% >> colormap(gray)
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+phi=pi*th/180;
+sigy=sig;
+sigx=r*sig;
+R=[cos(phi) -sin(phi); sin(phi) cos(phi)];
+C=R*diag([sigx,sigy])*R';
+
+X=[x(:) y(:)];
+
+Gb=gaussian(X,[0 0]',C);
+Gb=reshape(Gb,N,N);
+
+m=R*[0 sig]';
+Ga=gaussian(X,m,C);
+Ga=reshape(Ga,N,N);
+Gc=rot90(Ga,2);
+
+a=-1;
+b=2;
+c=-1;
+
+G = a*Ga + b*Gb + c*Gc;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt.m b/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt.m
new file mode 100755
index 0000000..25fa5f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt.m
@@ -0,0 +1,82 @@
+% script for fft-based filtering
+
+% set up filterbank
+make_filterbank
+   
+% prepare FFT of image for filtering
+[N1,N2]=size(V);
+I=zeros(size(V)+[M1-1 M2-1]);
+I(1:N1,1:N2)=V;
+IF=fft2(I);
+FI=zeros(N1,N2,total_num_filt);
+
+% apply filters
+for n=1:total_num_filt
+   disp(n)
+   f=rot90(FB(:,:,n),2);
+   fF=fft2(f,N1+M1-1,N2+M2-1);
+   IfF=IF.*fF;
+   If=real(ifft2(IfF));
+   If=If(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   FI(:,:,n)=If;
+%   im(If)
+%   drawnow
+end
+
+%%%% end of filtering part; the remainder is for reconstruction & analysis
+break
+
+
+% use pseudoinverse to reconstruct image from filter projections
+fbv=reshape(FB,M1*M2,total_num_filt)';
+fbi=pinv(fbv);
+
+% find principal components
+T=reshape(FI,N1*N2,total_num_filt)';
+C=T*T';
+[U,S,junk]=svd(C);
+s=diag(S);
+
+% synthesize using some eigenvectors
+synth=fbi*U;
+k=ceil(sqrt(total_num_filt));
+for n=1:total_num_filt
+   subplot(k,k,n)
+   im(reshape(synth(:,n),M1,M2));
+   title(num2str(s(n)))
+   drawnow
+end
+
+% synthesize at a point by clicking on coordinates
+figure(1)
+im(V)
+[x,y]=ginput(1);
+x=round(x);
+y=round(y);
+u=squeeze(FI(y,x,:));
+synth=fbi*u;
+synth=reshape(synth,M1,M2);
+figure(2)
+subplot(1,2,1)
+im(V)
+axis([x-M2/2 x+M2/2 y-M1/2 y+M1/2])
+subplot(1,2,2)
+im(synth)
+title(num2str(max(synth(:))));
+
+figure(3)
+plot(u,'o-')
+
+% show pseudoinverse filterbank
+if 0
+k=ceil(sqrt(total_num_filt));
+for n=1:total_num_filt
+   subplot(k,k,n)
+   im(reshape(fbi(:,n),M1,M2));
+   axis('off')
+   drawnow
+end
+end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt_2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt_2.m
new file mode 100755
index 0000000..9c84e96
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/fft_filt_2.m
@@ -0,0 +1,29 @@
+function FI=fft_filt_2(V,FB,sf);
+% FI=fft_filt_2(V,FB,sf);
+% fft-based filtering
+% requires image to be called "V"
+% and filters to be in FB
+% sf is the subsampling factor
+%
+% FI is the result
+
+[M1,M2,N3]=size(FB);
+% prepare FFT of image for filtering
+[N1,N2]=size(V);
+I=zeros(size(V)+[M1-1 M2-1]);
+I(1:N1,1:N2)=V;
+N1s=length(1:sf:N1);
+N2s=length(1:sf:N2);
+IF=fft2(I);
+FI=zeros(N1s,N2s,N3);
+
+% apply filters
+for n=1:N3;
+   f=rot90(FB(:,:,n),2);
+   fF=fft2(f,N1+M1-1,N2+M2-1);
+   IfF=IF.*fF;
+   If=real(ifft2(IfF));
+   If=If(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   FI(:,:,n)=If(1:sf:N1,1:sf:N2);
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/filter_bank_jshi.tar b/SD-VBS/common/toolbox/toolbox_basic/filter/filter_bank_jshi.tar
new file mode 100755
index 0000000..b43b49c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/filter_bank_jshi.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/gauss.m b/SD-VBS/common/toolbox/toolbox_basic/filter/gauss.m
new file mode 100755
index 0000000..f0403ed
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/gauss.m
@@ -0,0 +1,16 @@
+function dg = gauss(sig)
+% first derivative of N(sig)
+% cutoff after 1% of max
+
+  i = 1;
+  max = 0;
+  dgi = max;
+  dg = [1/ (sqrt(2*pi) * sig) ];
+  while dgi >= 0.01*max
+    dgi = 1/ (sqrt(2*pi) * sig) * exp(-0.5*i^2/sig^2);
+    dg = [dgi dg dgi];
+    i = i + 1;
+    if dgi > max
+      max = dgi;
+     end
+  end;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/gaussian.m b/SD-VBS/common/toolbox/toolbox_basic/filter/gaussian.m
new file mode 100755
index 0000000..509b129
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/gaussian.m
@@ -0,0 +1,31 @@
+function p=gaussian(x,m,C);
+% p=gaussian(x,m,C);
+%
+% Evaluate the multi-variate density with mean vector m and covariance
+% matrix C for the input vector x.
+% 
+% p=gaussian(X,m,C);
+% 
+% Vectorized version: Here X is a matrix of column vectors, and p is 
+% a vector of probabilities for each vector.
+
+d=length(m);
+
+if size(x,1)~=d
+   x=x';
+end
+N=size(x,2);
+
+detC = det(C);
+if rcond(C)<eps
+%   fprintf(1,'Covariance matrix close to singular. (gaussian.m)\n');
+   p = zeros(N,1);
+else
+   m=m(:);
+   M=m*ones(1,N);
+   denom=(2*pi)^(d/2)*sqrt(abs(detC));
+   mahal=sum(((x-M)'*inv(C)).*(x-M)',2);   % Chris Bregler's trick
+   numer=exp(-0.5*mahal);
+   p=numer/denom;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff2.m
new file mode 100755
index 0000000..31f3ac5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff2.m
@@ -0,0 +1,43 @@
+function [diffI,corr,mag,theta] = get_diff2(I,J,w)
+% car: I
+% background: J
+
+% half window size
+if (nargin == 2)
+ w = 1;
+end
+
+[gIx,gIy] = grad2(I,w);
+[gJx,gJy] = grad2(J,w);
+gx = gIx;
+gy = gIy;
+
+% normalize
+sI= sqrt(gIx.*gIx+gIy.*gIy);
+sJ= sqrt(gJx.*gJx+gJy.*gJy);
+gIx = gIx./sI;
+gIy = gIy./sI;
+gJx = gJx./sJ;
+gJy = gJy./sJ;
+
+theta = cart2pol(gIy,gIx);
+corr = gIx.*gJx + gIy.*gJy;
+
+%[gx,gy]= grad((I-J),w);mag = sqrt(gx.*gx+gy.*gy);
+%mag = sI;
+
+
+mag = sqrt((cos(theta).^2).*gy.^2 + (sin(theta).^2).*gx.^2 +...
+      2*cos(theta).*sin(theta).*gx.*gy);
+
+% want to look at the grad. mag greater than 10, and corr less than 0.9
+threshold = max(3.5,0.1*max(max(mag(5:size(mag,1)-5,5:size(mag,2)-5))))
+diffI = (abs(corr)<0.85).*(mag>threshold);
+
+
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff_free.m b/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff_free.m
new file mode 100755
index 0000000..f020fab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/get_diff_free.m
@@ -0,0 +1,8 @@
+function [diff,corr,mag,angle] = get_diff_free(I,J)
+
+[angle,mag,c2,c3] = compute_angle(I);
+[angleJ,magJ] = compute_angle(J);
+
+corr = cos(angle).*cos(angleJ) + sin(angle).*sin(angleJ);
+threshold = 0.075*max(max(mag(5:size(mag,1)-5,5:size(mag,2)-5)));
+diff = (abs(corr)<0.9).*(mag>threshold);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/grad1.m b/SD-VBS/common/toolbox/toolbox_basic/filter/grad1.m
new file mode 100755
index 0000000..eca34db
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/grad1.m
@@ -0,0 +1,11 @@
+function [gx,gy] = grad2(I,ratio)
+%
+%
+
+kern = dgauss(ratio);kern = kern/sum(abs(kern));
+gkern = gauss(ratio);gkern = gkern/sum(abs(kern));
+
+gx = conv2(I,kern,'same');
+gx = conv2(gx,gkern','same');
+
+gy = conv2(conv2(I,kern','same'),gkern,'same');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/grad2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/grad2.m
new file mode 100755
index 0000000..ea4ec0e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/grad2.m
@@ -0,0 +1,11 @@
+function [gx,gy] = grad2(I,ratio)
+%
+%
+
+ddgauss = gradient(dgauss(ratio));ddgauss = ddgauss/sum(abs(ddgauss));
+gkern = gauss(ratio); gkern = gkern/sum(abs(gkern));
+
+gx = conv2(I,ddgauss,'same');
+gx = conv2(gx,gkern','same');
+
+gy = conv2(conv2(I,ddgauss','same'),gkern,'same');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/m_interp4.m b/SD-VBS/common/toolbox/toolbox_basic/filter/m_interp4.m
new file mode 100755
index 0000000..314f140
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/m_interp4.m
@@ -0,0 +1,49 @@
+function [F,mask] = m_interp4(z,s,t)
+%INTERP4 2-D bilinear data interpolation.
+%       ZI = INTERP4(Z,XI,YI) assumes X = 1:N and Y = 1:M, where
+%       [M,N] = SIZE(Z).
+%
+%       Copyright (c) 1984-93 by The MathWorks, Inc.
+%       Clay M. Thompson 4-26-91, revised 7-3-91, 3-22-93 by CMT.
+%
+%  modified to 
+
+ 
+[nrows,ncols] = size(z);
+
+
+if any(size(z)<[3 3]), error('Z must be at least 3-by-3.'); end
+if size(s)~=size(t), error('XI and YI must be the same size.'); end
+
+% Check for out of range values of s and set to 1
+sout = find((s<1)|(s>ncols));
+if length(sout)>0, s(sout) = ones(size(sout)); end
+
+% Check for out of range values of t and set to 1
+tout = find((t<1)|(t>nrows));
+if length(tout)>0, t(tout) = ones(size(tout)); end
+
+% Matrix element indexing
+ndx = floor(t)+floor(s-1)*nrows;
+
+% Compute intepolation parameters, check for boundary value.
+d = find(s==ncols); 
+s(:) = (s - floor(s));
+if length(d)>0, s(d) = s(d)+1; ndx(d) = ndx(d)-nrows; end
+
+% Compute intepolation parameters, check for boundary value.
+d = find(t==nrows);
+t(:) = (t - floor(t));
+if length(d)>0, t(d) = t(d)+1; ndx(d) = ndx(d)-1; end
+d = [];
+
+% Now interpolate, reuse u and v to save memory.
+F =  ( z(ndx).*(1-t) + z(ndx+1).*t ).*(1-s) + ...
+     ( z(ndx+nrows).*(1-t) + z(ndx+(nrows+1)).*t ).*s;
+
+mask = ones(size(z));
+
+% Now set out of range values to zeros.
+if length(sout)>0, F(sout) = zeros(size(sout));mask(sout)=zeros(size(sout));end
+if length(tout)>0, F(tout) = zeros(size(tout));mask(tout)=zeros(size(tout));end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank.m b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank.m
new file mode 100755
index 0000000..2ff15d2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank.m
@@ -0,0 +1,63 @@
+function FB = make_filterbank(num_ori,num_scale,wsz)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FBdoog1=zeros(M1,M2,num_scale,num_ori);
+FBdoog2=zeros(M1,M2,num_scale,num_ori);
+FBdog1=zeros(M1,M2,num_scale);
+FBdog2=zeros(M1,M2,num_scale);
+
+% elongated filter set
+counter = 1;
+filter_scale = 1.0;
+filter_scale_step = sqrt(2);
+ 
+for m=1:num_scale
+   f=dog1(filter_scale,M1);
+   FBdog1(:,:,m)=f;
+   f=dog2(filter_scale,M1);
+   FBdog2(:,:,m)=f;
+   counter=counter+1;
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog2(filter_scale,6,ori_offset+(n-1)*ori_incr,M1);
+      FBdoog2(:,:,m,n)=f;
+      f=doog1(filter_scale,6,ori_offset+(n-1)*ori_incr,M1);
+      FBdoog1(:,:,m,n)=f;
+   end
+   filter_scale = filter_scale * filter_scale_step;
+end
+
+FB=cat(3,3*FBdog1,4.15*FBdog2,2*reshape(FBdoog1,M1,M2,num_scale*num_ori),2*reshape(FBdoog2,M1,M2,num_scale*num_ori));
+total_num_filt=size(FB,3);
+
+nb = size(FB,3);
+for j=1:nb,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
+
+if 0
+k=ceil(sqrt(total_num_filt));
+for n=1:total_num_filt
+   subplot(k,k,n)
+   im(FB(:,:,n));
+   axis('off')
+   drawnow
+end
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_23.m b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_23.m
new file mode 100755
index 0000000..f9fcaa9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_23.m
@@ -0,0 +1,40 @@
+function [FB,M1,M2,N3]=make_filterbank_23; 
+% multi-scale even and odd filters 
+ 
+M1=31;  % size in pixels 
+M2=M1; 
+num_ori=6; 
+num_scales=3; 
+num_phases=2; 
+N3=num_ori*num_scales*num_phases; 
+FB=zeros(M1,M2,N3); 
+ 
+counter=1; 
+ 
+for m=1:num_scales 
+   for n=1:num_ori 
+      [F1,F2]=quadpair(sqrt(2)^m,3,180*(n-1)/num_ori,M1); 
+      FB(:,:,counter)=F1; 
+      counter=counter+1; 
+      FB(:,:,counter)=F2; 
+      counter=counter+1; 
+   end 
+end 
+ 
+FB=cat(3,FB,dog2(1,M1),dog2(sqrt(2),M1),dog2(2,M1),dog2(2*sqrt(2),M1)); 
+ 
+N3=size(FB,3); 
+ 
+% stuff for visualizing spectra of filters: 
+if 0 
+FBF=zeros(size(FB)); 
+ 
+for n=1:36 
+   FBF(:,:,n)=abs(fftshift(fft2(FB(:,:,n)))); 
+end 
+ 
+montage2(FBF) 
+ 
+im(sum(FBF,3)) 
+ 
+end 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_even.m b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_even.m
new file mode 100755
index 0000000..8c8d802
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_even.m
@@ -0,0 +1,40 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog2(filter_scales(m),6,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_odd.m b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_odd.m
new file mode 100755
index 0000000..8103598
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/make_filterbank_odd.m
@@ -0,0 +1,41 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog1(filter_scales(m),6,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mdoog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mdoog2.m
new file mode 100755
index 0000000..25bc2f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mdoog2.m
@@ -0,0 +1,36 @@
+function G=doog2(sig,r,th,N);
+% [G,H]=doog2(sig,r,th,N);
+% Make difference of offset gaussians kernel
+% theta is in degrees
+% (see Malik & Perona, J. Opt. Soc. Amer., 1990)
+%
+
+
+[x,y]=meshgrid(-N:N,-N:N);
+
+a=-1;
+b=2;
+c=-1;
+
+ya=sig;
+yc=-ya;
+yb=0;
+sigy=sig;
+sigx=r*sig;
+
+Ga=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-ya)/sigy).^2));
+Gb=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yb)/sigy).^2));
+Gc=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yc)/sigy).^2));
+
+Go = a*Ga + b*Gb + c*Gc;
+%Ho = imag(hilbert(Go));
+G = Go;
+
+G = mimrotate(Go,th,'bilinear','crop');
+
+G = G-mean(reshape(G,prod(size(G)),1));
+
+G = G/sum(sum(abs(G)));
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mimrotate.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mimrotate.m
new file mode 100755
index 0000000..7dd31a2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mimrotate.m
@@ -0,0 +1,119 @@
+function bout = imrotate(arg1,arg2,arg3,arg4)
+%IMROTATE Rotate image.
+%       B = IMROTATE(A,ANGLE,'method') rotates the image A by ANGLE
+%       degrees.  The image returned B will, in general, be larger 
+%       than A.  Invalid values on the periphery are set to one
+%       for indexed images or zero for all other image types. Possible 
+%       interpolation methods are 'nearest','bilinear' or 'bicubic'.
+%       'bilinear' is the default for intensity images, otherwise 
+%       'nearest' is used if no method is given.
+%
+%       B = IMROTATE(A,ANGLE,'crop') or IMROTATE(A,ANGLE,'method','crop')
+%       crops B to be the same size as A.
+%
+%       Without output arguments, IMROTATE(...) displays the rotated
+%       image in the current axis.
+%
+%       See also IMRESIZE, IMCROP, ROT90.
+
+%       Clay M. Thompson 8-4-92
+%       Copyright (c) 1992 by The MathWorks, Inc.
+%       $Revision: 1.14 $  $Date: 1993/09/01 21:27:38 $
+
+if nargin<2, error('Requires at least two input parameters.'); end
+if nargin<3,
+  if isgray(arg1), caseid = 'bil'; else caseid = 'nea'; end
+  docrop = 0;
+elseif nargin==3,
+  if isstr(arg3),
+    method = [lower(arg3),'   ']; % Protect against short method
+    caseid = method(1:3);
+    if caseid(1)=='c', % Crop string
+      if isgray(arg1), caseid = 'bil'; else caseid = 'nea'; end
+      docrop = 1;
+    else
+      docrop = 0;
+    end
+  else
+    error('''METHOD'' must be a string of at least three characters.');
+  end
+else
+  if isstr(arg3),
+    method = [lower(arg3),'   ']; % Protect against short method
+    caseid = method(1:3);
+  else
+    error('''METHOD'' must be a string of at least three characters.');
+  end
+  docrop = 1;
+end
+
+% Catch and speed up 90 degree rotations
+if rem(arg2,90)==0 & nargin<4,
+  phi = rem(arg2,360);
+  if phi==90,
+    b = rot90(arg1);
+  elseif phi==180,
+    b = rot90(arg1,2);
+  elseif phi==270,
+    b = rot90(arg1,-1);
+  else
+    b = arg1;
+  end
+  if nargout==0, imshow(b), else bout = b; end
+  return
+end
+      
+phi = arg2*pi/180; % Convert to radians
+
+% Rotation matrix
+T = [cos(phi) -sin(phi); sin(phi) cos(phi)];
+
+% Coordinates from center of A
+[m,n] = size(arg1);
+if ~docrop, % Determine limits for rotated image
+  siz = ceil(max(abs([(n-1)/2 -(m-1)/2;(n-1)/2 (m-1)/2]*T))/2)*2;
+  uu = -siz(1):siz(1); vv = -siz(2):siz(2);
+else % Cropped image
+  uu = (1:n)-(n+1)/2; vv = (1:m)-(m+1)/2;
+end
+nu = length(uu); nv = length(vv);
+
+blk = bestblk([nv nu]);
+nblks = floor([nv nu]./blk); nrem = [nv nu] - nblks.*blk;
+mblocks = nblks(1); nblocks = nblks(2);
+mb = blk(1); nb = blk(2);
+
+rows = 1:blk(1); b = zeros(nv,nu);
+for i=0:mblocks,
+  if i==mblocks, rows = (1:nrem(1)); end
+  for j=0:nblocks,
+    if j==0, cols = 1:blk(2); elseif j==nblocks, cols=(1:nrem(2)); end
+    if ~isempty(rows) & ~isempty(cols)
+      [u,v] = meshgrid(uu(j*nb+cols),vv(i*mb+rows));
+      % Rotate points
+      uv = [u(:) v(:)]*T'; % Rotate points
+      u(:) = uv(:,1)+(n+1)/2; v(:) = uv(:,2)+(m+1)/2;
+      if caseid(1)=='n', % Nearest neighbor interpolation
+        b(i*mb+rows,j*nb+cols) = interp6(arg1,u,v);
+      elseif all(caseid=='bil'), % Bilinear interpolation
+        b(i*mb+rows,j*nb+cols) = interp2(arg1,u,v,'linear');
+      elseif all(caseid=='bic'), % Bicubic interpolation
+        b(i*mb+rows,j*nb+cols) = interp5(arg1,u,v);
+      else
+        error(['Unknown interpolation method: ',method]);
+      end
+    end
+  end
+end
+
+d = find(isnan(b));
+if length(d)>0, 
+  if isind(arg1), b(d) = ones(size(d)); else b(d) = zeros(size(d)); end
+end
+
+if nargout==0,
+  imshow(b), return
+end
+bout = b;
+
+  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mk_odd_filter.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mk_odd_filter.m
new file mode 100755
index 0000000..43ec7d7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mk_odd_filter.m
@@ -0,0 +1,36 @@
+function G=doog2(sig,r,th,N);
+% [G,H]=doog2(sig,r,th,N);
+% Make difference of offset gaussians kernel
+% theta is in degrees
+% (see Malik & Perona, J. Opt. Soc. Amer., 1990)
+%
+
+
+[x,y]=meshgrid(-N:N,-N:N);
+
+a=-1;
+b=2;
+c=-1;
+
+ya=sig;
+yc=-ya;
+yb=0;
+sigy=sig;
+sigx=r*sig;
+
+Ga=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-ya)/sigy).^2));
+Gb=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yb)/sigy).^2));
+Gc=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yc)/sigy).^2));
+
+Go = a*Ga + b*Gb + c*Gc;
+Ho = imag(hilbert(Go));
+%G = Ho;
+
+G = mimrotate(Ho,th,'bilinear','crop');
+
+G = G-mean(reshape(G,prod(size(G)),1));
+
+G = G/sum(sum(abs(G)));
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog1.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog1.m
new file mode 100755
index 0000000..f1225cc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog1.m
@@ -0,0 +1,20 @@
+function dog1 = mkdog1(sigma_base,size_w)
+%
+% function dog1 = mkdog1(sigma_base,size_w)
+%
+%
+
+%scale_base = 1; 
+scale_base = 3;
+
+a = scale_base;
+c = -1*scale_base;
+
+sigma_a = 0.71*sigma_base;
+sigma_c = 1.14*sigma_base;
+
+dog1 = a*mkg(0,0,sigma_a,sigma_a,size_w) +...
+       c*mkg(0,0,sigma_c,sigma_c,size_w);
+
+dog1 = dog1-mean(reshape(dog1,prod(size(dog1)),1));
+dog1 = dog1/sum(sum(abs(dog1)));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog2.m
new file mode 100755
index 0000000..a78824a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdog2.m
@@ -0,0 +1,22 @@
+function dog2 = mkdog2(sigma_base,size_w)
+%
+% function dog2 = mkdog2(sigma_base,size_w)
+%
+%
+
+%scale_base = 1.224; 
+scale_base = 4.15;
+
+a = scale_base;
+b = -2*scale_base;
+c = scale_base;
+
+sigma_a = 0.62*sigma_base;
+sigma_b = sigma_base;
+sigma_c = 1.6*sigma_base;
+
+dog2 = a*mkg(0,0,sigma_a,sigma_a,size_w) +...
+       b*mkg(0,0,sigma_b,sigma_b,size_w) +...
+       c*mkg(0,0,sigma_c,sigma_c,size_w);
+
+%dog2 = 255*5.1745*dog2;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoog2.m
new file mode 100755
index 0000000..5db2877
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoog2.m
@@ -0,0 +1,30 @@
+function doog2 = mkdoog2(sigma_w,r,theta,size_w)
+%
+% function doog2 = mkdoog2(sigma_w,r,theta,size_w)
+%
+%
+
+%scale_base = 2.8814; 
+scale_base = 2;
+
+a = -1*scale_base;
+b = 2*scale_base;
+c = -1*scale_base;
+
+sigma_x = r*sigma_w;
+sigma_y = sigma_w;
+
+ya = sigma_w;
+yc = -sigma_w;
+yb = 0;
+
+doog2 = a*mkg(0,ya,sigma_x,sigma_y,size_w) +...
+        b*mkg(0,yb,sigma_x,sigma_y,size_w) +...
+        c*mkg(0,yc,sigma_x,sigma_y,size_w);
+
+%doog2 = 255*5.1745*doog2;
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoogs.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoogs.m
new file mode 100755
index 0000000..e5796bc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mkdoogs.m
@@ -0,0 +1,15 @@
+function [doogs,index] = mkdoogs(sigma_w,r,theta,theta_to,size_w)
+%  function doogs = mkdoogs(sigma_w,r,theta,theta_to,size_w)
+% 
+
+doogs = [];
+
+angle_start = theta*pi/180;
+angle_end = theta_to*pi/180;
+step = pi/180;
+
+index = 1;
+for k=angle_start:step:angle_end,
+  doogs = [doogs,mkdoog2(sigma_w,r,k,size_w)];
+  index = index +1;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/mkg.m b/SD-VBS/common/toolbox/toolbox_basic/filter/mkg.m
new file mode 100755
index 0000000..1fb1f7e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/mkg.m
@@ -0,0 +1,9 @@
+function g= mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%  
+%   function G = mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%
+ 
+size_wh = round(0.5*size_w);
+[x,y] = meshgrid([-size_wh:1:size_wh],[-size_wh:1:size_wh]);
+g = 1/(2*pi*sigma_x*sigma_y)*(exp(-( ((x-xo)/sigma_x).^2 + ((y-yo)/sigma_y).^2)));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/quadpair.m b/SD-VBS/common/toolbox/toolbox_basic/filter/quadpair.m
new file mode 100755
index 0000000..96c2a22
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/quadpair.m
@@ -0,0 +1,20 @@
+function [F1,F2]=quadpair(sig,lam,th,N);
+% [F1,F2]=quadpair(sig,lam,th,N);
+%
+% For Thomas' ECCV98 filters, use sig=sqrt(2), lam=4.
+
+%N=31;
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+
+F1=(4*(y.^2)/(sig^4)-2/(sig^2)).*exp(-(y.^2)/(sig^2)-(x.^2)/(lam^2*sig^2));
+F2=imag(hilbert(F1));
+
+F1=imrotate(F1,th,'bil','crop');
+F2=imrotate(F2,th,'bil','crop');
+
+F1=F1-mean(F1(:));
+F2=F2-mean(F2(:));
+
+F1=F1/norm(F1(:),1);
+F2=F2/norm(F2(:),1);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/smooth.m b/SD-VBS/common/toolbox/toolbox_basic/filter/smooth.m
new file mode 100755
index 0000000..5ef7579
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/smooth.m
@@ -0,0 +1,24 @@
+% smooth an image
+% coordinates (r, c) follow matrix convention;
+% the gaussian is truncated at x = +- tail, and there are samples samples
+% inbetween, where samples = hsamples * 2 + 1
+
+function g = smooth(image, hsamples)
+
+tail=4;
+samples = hsamples * 2 + 1;
+
+x = linspace(-tail, tail, samples);
+gauss = exp(-x.^2);
+%s = sum(gauss)/length(x);gauss = gauss-s;
+gauss = gauss/sum(abs(gauss));
+
+n = gauss * ones(samples,1);
+gauss = gauss/n;
+
+
+g = conv2(conv2(image, gauss,'same'), gauss','same');
+%g = conv2(conv2(image, gauss,'valid'), gauss','valid');
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/softkmean.m b/SD-VBS/common/toolbox/toolbox_basic/filter/softkmean.m
new file mode 100755
index 0000000..b9b4feb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/softkmean.m
@@ -0,0 +1,56 @@
+function [cluster,var,mix,membership,lG] = softkmeans(data,k,cluster0)
+
+[n,D] = size(data);
+var = 1.0;
+var0 = ones(k,1)*var; minvar = 0.0001;
+mix0 = ones(k,1)/k; minmix = 0.0001;
+
+k = size(var0,1);
+[n,D] = size(data);
+
+lGG = [];
+ma = -1e20;
+in = 0;
+
+if (nargin == 2),
+  max_data = max(data);
+  min_data = min(data);
+  %step = (max_data-min_data)/(k+1);
+  %cluster0 = [1:k]'*step+min_data;
+  mag = ones(k,1)*(max_data-min_data);
+  base = ones(k,1)*min_data;
+  cluster0 = rand(k,D).*mag + base;
+end
+
+%cluster0
+for t = 1:3,
+  %rndindx = round(rand(1,k)*(n-3))+2;
+  %cluster0 = (data(rndindx,:)+data(rndindx+1,:)+data(rndindx-1,:))/2;
+  [cluster,var,mix,membership,lG] = softmeans(cluster0,var0,minvar,mix0,minmix,data);
+  eval(sprintf('mix_var_cluster_%d = [mix,var,cluster];',t));
+  eval(sprintf('lG_%d = lG;',t));
+  if ma<lG(size(lG,2)),
+    ma = lG(size(lG,2));
+    in = t;
+  end
+
+end
+
+eval(sprintf('mix_var_cluster = mix_var_cluster_%d;',in));
+eval(sprintf('lG = lG_%d;',in));
+mix = mix_var_cluster(:,1);
+var = mix_var_cluster(:,2);
+cluster = mix_var_cluster(:,3:size(mix_var_cluster,2))
+
+
+[tmp,vecs2cluster] = max(membership');
+vecs2cluster = vecs2cluster';
+cluster_iCV = inv(var);
+
+
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans.m b/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans.m
new file mode 100755
index 0000000..b7e5068
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans.m
@@ -0,0 +1,46 @@
+function [cluster,var,mix,membership,lG] = softmeans(cluster0,var0,minvar,mix0,minmix,data)
+
+[k,D] = size(cluster0);
+[n,D] = size(data);
+cluster_p = cluster0; var_p = var0; mix_p = mix0;
+old_lg = -inf; lG = [];
+
+for iter = 1:30, % max.iterations
+  % E-Step + comp. incomplete likelihood
+
+  H = zeros(n,k);
+  for j = 1:k,
+    Hj = (data-(ones(n,1)*cluster_p(j,:))).^2;
+    if D > 1, Hj = sum(Hj')'; end
+    H(:,j) = exp(Hj /(-2*var_p(j)))/(sqrt(var_p(j))^D);
+  end
+  H = H.*(ones(n,1)*mix_p');
+  new_lg = sum(log(sum(H')/(sqrt(2*pi)^D)));
+  lG = [lG, new_lg];
+  if new_lg == old_lg, break; end; old_lg = new_lg;
+  H = H./(sum(H')'*ones(1,k)); % normalize
+
+  % M-Step:
+
+  if minmix > 0,    
+    mix_p = sum(H); mix_p = mix_p/sum(mix_p); mix_p = mix_p';
+    for j = 1:k, if mix_p(j)<minmix, mix_p(j) = minmix; end; end;
+  end
+  cluster_p = (H./(ones(n,1)*sum(H)))'*data;
+  if minvar > 0,
+   for j = 1:k,
+    varj = (data-(ones(n,1)*cluster_p(j,:))).^2;
+    if D > 1, varj = sum(varj')'; end
+    var_p(j) = sum(H(:,j).*varj)/(D*sum(H(:,j)));
+    if var_p(j)<minvar, var_p(j) = minvar; end;
+   end;
+  end
+
+%  cluster_p = cluster_p./(sqrt(sum(cluster_p'.^2)')*ones(1,D));
+end
+
+cluster = cluster_p;
+var = var_p;
+mix = mix_p;
+membership = H;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans2.m b/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans2.m
new file mode 100755
index 0000000..8fd194e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter/softmeans2.m
@@ -0,0 +1,39 @@
+function [cluster,var,mix,membership,lG] = softmeans2(var0,minvar,mix0,minmix,data,cluster0,iter)
+
+if (~exist('iter')),
+  iter = 3;
+end
+
+k = size(var0,1);
+[n,D] = size(data);
+
+lGG = [];
+ma = -1e20;
+in = 0;
+
+if (nargin == 5),
+  max_data = max(data);
+  min_data = min(data);
+  step = (max_data-min_data)/(k+1);
+  cluster0 = [1:k]'*step+min_data;
+end
+cluster0
+for t = 1:iter,
+  %rndindx = round(rand(1,k)*(n-3))+2;
+  %cluster0 = (data(rndindx,:)+data(rndindx+1,:)+data(rndindx-1,:))/2;
+  [cluster,var,mix,membership,lG] = softmeans(cluster0,var0,minvar,mix0,minmix,data);
+  eval(sprintf('mix_var_cluster_%d = [mix,var,cluster];',t));
+  eval(sprintf('lG_%d = lG;',t));
+  if ma<lG(size(lG,2)),
+    ma = lG(size(lG,2));
+    in = t;
+  end
+
+end
+
+eval(sprintf('mix_var_cluster = mix_var_cluster_%d;',in));
+eval(sprintf('lG = lG_%d;',in));
+mix = mix_var_cluster(:,1);
+var = mix_var_cluster(:,2);
+cluster = mix_var_cluster(:,3:size(mix_var_cluster,2))
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filterQuad.zip b/SD-VBS/common/toolbox/toolbox_basic/filterQuad.zip
new file mode 100755
index 0000000..00e6141
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filterQuad.zip
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/1d_cut.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/1d_cut.m
new file mode 100755
index 0000000..46f865b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/1d_cut.m
@@ -0,0 +1,16 @@
+function [x,map] = idcut(data,cmap,nbin)
+%
+%
+%
+
+lc = size(cmap,1);
+
+data = data - min(data);
+data = 1+ ((lc-1)*data/max(data));
+
+r = cmap(data,1);
+g = cmap(data,2);
+b = cmap(data,3);
+
+[x,map] = vmquant(r,g,b,nbin);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Bfilter.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Bfilter.m
new file mode 100755
index 0000000..ee086f0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Bfilter.m
@@ -0,0 +1,11 @@
+function output = Bfilter(img,H)
+%
+%  function output = Bfilter(img,H)
+%
+
+sze = size(img);
+
+Y = fft(reshape(img,1,sze(1)*sze(2)));
+C = Y.*conj(H);
+c = real(ifft(C));
+output = reshape(c,sze(1),sze(2));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/BfilterS.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/BfilterS.m
new file mode 100755
index 0000000..4eb4fd6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/BfilterS.m
@@ -0,0 +1,17 @@
+function output = Bfilters(img,H,w)
+%
+%  function output = Bfilter(img,H,w)
+%
+
+sze = size(img);
+w_h = round(0.5*(w-1));
+
+Y = fft(reshape(img,1,sze(1)*sze(2)));
+C = Y.*conj(H);
+c = real(ifft(C));
+o = reshape(c,sze(1),sze(2));
+
+output(1:w_h(1),1:w_h(2)) = o(sze(1)-w_h(1)+1:sze(1),sze(2)-w_h(2)+1:sze(2));
+output(1:w_h(1),w_h(2)+1:sze(2)) = o(sze(1)-w_h(1)+1:sze(1),1:sze(2)-w_h(2));
+output(w_h(1)+1:sze(1),w_h(2)+1:sze(2)) = o(1:sze(1)-w_h(1),1:sze(2)-w_h(2));
+output(w_h(1)+1:sze(1),1:w_h(2)) = o(1:sze(1)-w_h(1),sze(2)-w_h(2)+1:sze(2));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Ncut.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Ncut.m
new file mode 100755
index 0000000..30c9b33
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/Ncut.m
@@ -0,0 +1,14 @@
+function [v,d] = ncut(A,nv)
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+
+D1 = ds'*ones(1,length(ds));
+A = D1'.*A.*D1;
+
+disp(sprintf('computing eig values'));
+tic;[v,d] = eigs(A,nv);toc;
+
+d = abs(diag(d));
+
+v = D1(:,1:size(v,2)).*v;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/apply_image.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/apply_image.m
new file mode 100755
index 0000000..0791aa4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/apply_image.m
@@ -0,0 +1,38 @@
+function [aout1,aout2,aout3,aout4] = apply_image(gx,gy,wc)  
+%
+%  aout = apply_image(gx,gy,wc)
+%
+%
+
+[nr,nc] =size(gx);
+
+w = 2*wc+1;
+
+aout1 = ones(nr,nc);
+aout2 = zeros(nr,nc);
+aout3 = aout2;
+aout4 = aout2;
+
+%mask = smooth(ones(w,w),w);
+%sig = w;
+%[x,y] = meshgrid(-wc:wc,-wc:wc);
+%mask = exp(-(x.*x)/sig).*exp(-(y.*y)/sig);
+%mask = mask/sum(sum(mask));
+
+
+tmp = ones(w,w);
+for j=wc+1:w:nr-wc-1,
+        for k=wc+1:w:nc-wc-1,
+                tgx = get_win(gx,[k,j],[wc,wc]);
+                tgy = get_win(gy,[k,j],[wc,wc]);
+                %mag = sum(sum(sqrt((mask.*tgx).^2+(mask.*tgy).^2)));
+                mag = sum(sum(sqrt(tgx.^2 + tgy.^2)))/prod(size(tgy));
+                
+                M = is_step(tgx,tgy);
+
+                aout1(j-wc:j+wc,k-wc:k+wc) = M(1)*tmp;
+                aout2(j-wc:j+wc,k-wc:k+wc) = M(2)*tmp;
+                aout3(j-wc:j+wc,k-wc:k+wc) = M(3)*tmp;
+                aout4(j-wc:j+wc,k-wc:k+wc) = mag*tmp;
+        end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/back_proj.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/back_proj.m
new file mode 100755
index 0000000..47ac865
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/back_proj.m
@@ -0,0 +1,10 @@
+function BI = back_proj(PFt,vec)
+
+BI = [];
+
+sz1 = sqrt(size(PFt,1));
+
+for j=1:size(vec,2)
+  tmp = PFt*vec(:,j);
+  BI(:,:,j) = reshape(tmp,sz1,sz1);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer.m
new file mode 100755
index 0000000..b4560c5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer.m
@@ -0,0 +1,9 @@
+function v = backproj_outer(fvs,u,hb)
+%
+%  given the eigenvecs of the hist.bin. features
+%  computes the back projection on the eigenvects
+%
+
+[nv,np] = size(fvs);
+
+for j=1:
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank.m
new file mode 100755
index 0000000..5e3eac9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank.m
@@ -0,0 +1,33 @@
+function v = backproj_outer_chank(fvs,u,d,chank_size)
+%
+%  given the eigenvecs of the hist.bin. features
+%  computes the back projection on the eigenvects
+%
+
+[nv,np] = size(fvs);
+[nbins,nv] = size(u);
+
+n_chanks = ceil(np/chank_size);
+
+v = ones(np,nv);
+
+for j=1:n_chanks,
+  fprintf('<');
+  
+  cm = sprintf('load st_%d',j);
+  eval(cm);
+  fprintf(sprintf('%d',n_chanks-j));    
+
+  v((j-1)*chank_size+1:min(np,j*chank_size),:) = fh'*u;
+  fprintf('>');
+end
+
+fprintf('\n');
+
+s = 1./sqrt(d);
+
+for j=1:nv,
+  v(:,j) = v(:,j)*s(j);
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank2.m
new file mode 100755
index 0000000..084c150
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/backproj_outer_chank2.m
@@ -0,0 +1,36 @@
+function v = backproj_outer_chank(fvs,u,d,chank_size)
+%
+%  given the eigenvecs of the hist.bin. features
+%  computes the back projection on the eigenvects
+%
+
+[nv,np] = size(fvs);
+[nbins,nv] = size(u);
+
+n_chanks = ceil(np/chank_size);
+
+v = ones(np,nv);
+
+for j=1:n_chanks,
+  fprintf('<');
+  
+  cm = sprintf('load st_%d',j);
+  eval(cm);
+  fprintf(sprintf('%d',n_chanks-j));    
+
+  ms = mean(fh');
+  fh = fh - ms'*ones(1,size(fh,2));
+
+  v((j-1)*chank_size+1:min(np,j*chank_size),:) = fh'*u;
+  fprintf('>');
+end
+
+fprintf('\n');
+
+s = 1./sqrt(d);
+
+for j=1:nv,
+  v(:,j) = v(:,j)*s(j);
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize.m
new file mode 100755
index 0000000..d166cd5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize.m
@@ -0,0 +1,15 @@
+function [binv,bins] = binize(data,sig,bin_min,bin_max,num_bin)
+%
+%  given an input data, and sigma which describes the uncertainty
+%  of the data, along with information on the bins,
+%  return the soft-hist on data
+%
+
+ndata = length(data);
+
+bins = linspace(bin_min,bin_max,num_bin+1);
+binv = zeros(num_bin,ndata);
+
+for j=1:num_bin,
+    binv(j,:) = erf((bins(j+1)-data)/sig) - erf((bins(j)-data)/sig);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize_old.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize_old.m
new file mode 100755
index 0000000..d56d263
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binize_old.m
@@ -0,0 +1,34 @@
+function [binv,bins] = binize(data,sig,bin_min,bin_max,num_bin)
+%
+%  given an input data, and sigma which describes the uncertainty
+%  of the data, along with information on the bins,
+%  return the soft-hist on data
+%
+
+ndata = length(data);
+
+if 0,
+bins = linspace(bin_min,bin_max,num_bin);
+binv = zeros(num_bin,ndata);
+
+Largev = 1000;
+
+bins = [-Largev,bins];
+
+for j=1:num_bin,
+    binv(j,:) = erf((bins(j+1)-data)/sig) - erf((bins(j)-data)/sig);
+end
+
+binv(num_bin,:) = binv(num_bin,:) + erf((Largev-data)/sig) - erf((bins(end)-data)/sig);
+bins = bins(2:end);
+else
+
+bins = linspace(bin_min,bin_max,num_bin+1);
+binv = zeros(num_bin,ndata);
+
+
+for j=1:num_bin,
+    binv(j,:) = erf((bins(j+1)-data)/sig) - erf((bins(j)-data)/sig);
+end
+
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binomialfield.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binomialfield.m
new file mode 100755
index 0000000..d83d96d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/binomialfield.m
@@ -0,0 +1,75 @@
+function [x,y,success] = BinomialField(n,sx,sy,ir,numtri);
+%BF_HardCore    Generates a hard core binomial field
+%               [x,y,success] = BinomialField(n,sx,sy,ir);
+%               n      : # points (default 100)
+%               sx     : size in x (default 100)
+%               sy     : size in y (default 100)
+%               ir     : inhibition radius (default 0)
+%               numtri : number of trials (default 200)
+%               x      : x coordinates
+%               y      : y coordinates
+%               success: whether success or not, useful when producing hard core model
+
+%%
+%%              (C) Thomas K. Leung
+%%              University of California at Berkeley
+%%              April 26, 1995.
+%%              leungt@cajal.cs.berkeley.edu
+%%
+
+%%
+%% Generate n points first and then reject those closer to the
+%% previous points than ir
+%%
+
+if nargin < 1
+        n  = 100;
+        sx = 100;
+        sy = 100;
+        ir = 0;
+        numtri = 200;
+elseif (nargin == 1 | nargin == 2)
+        sx = 100;
+        sy = 100;
+        ir = 0;
+        numtri = 200;
+elseif (nargin == 3)
+        ir = 0;
+        numtri = 200;
+elseif (nargin == 4)
+        numtri = 200;
+end
+
+x = zeros(1,n);
+y = zeros(1,n);
+
+rand('seed',sum(100*clock));
+x(1) = rand(1) * sx;
+y(1) = rand(1) * sy;
+
+success = 1;
+
+I = 2;
+trial = 0;
+while (I <= n & trial < numtri)
+        found = 0;
+        trial = 0;
+        while (~found & trial < numtri);
+                tx = rand(1) * sx;
+                ty = rand(1) * sy;
+                D  = (x(1:(I-1)) - tx).^2 + (y(1:(I-1)) - ty).^2;
+                if sum(D > (ir^2)) == (I-1)
+                        found = 1;
+                        x(I) = tx;
+                        y(I) = ty;
+                end
+                trial = trial + 1;
+        end
+        I = I + 1;
+end
+
+if trial >= numtri
+        fprintf(1,'Failed to generate a point in %d trials\n',numtri);
+        success = 0;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize.m
new file mode 100755
index 0000000..b03616f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize.m
@@ -0,0 +1,9 @@
+function t1a = colize(t1,I1);
+
+t1a = t1;
+
+t1a = reshape(t1a,size(t1,1)*size(t1,2),1,size(t1,3));
+t1a = squeeze(t1a);
+t1a = t1a';
+
+%I1a = 2*I1(:)';I1a = I1a-mean(I1a(:));t1a = [I1a;t1a];
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_hist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_hist.m
new file mode 100755
index 0000000..9c7b68e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_hist.m
@@ -0,0 +1,29 @@
+function fh = colize_hist(fv,hb)
+%      (hb = sigs,bin_mins,bin_maxs,nbins)
+%
+%   fv = [nfeature x npoints];
+%   fh = [nfeatures*nbins x npoints];
+%
+%  take a feature matrix, and turn it into histogram bin feature matrix
+%
+%
+
+[nf,np] = size(fv);
+
+nbins = [0,hb.nbins];
+disp(sprintf('need matrix of %d x %d ',sum(nbins),np));
+
+fh = zeros(sum(nbins),np);
+
+for k=1:nf,
+  bin_min = hb.bmins(k);
+  bin_max = hb.bmaxs(k);
+  nbin    = nbins(k+1);
+  sig     = hb.sigs(k);
+  fprintf('.');
+     b = binize(fv(k,:),sig,bin_min,bin_max,nbin);
+     fh(sum(nbins(1:k))+1:sum(nbins(1:k+1)),:) = b;
+  
+end
+
+fprintf('\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_s.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_s.m
new file mode 100755
index 0000000..61c81ca
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_s.m
@@ -0,0 +1,47 @@
+function fhs = colize_histnb_s(fh,Is,nw,hw)
+%
+%  fhs = colize_histneigh(fh,fvs,nw)
+%         
+%
+
+[tnbins,np] = size(fh);
+
+[nr,nc] = size(Is);
+
+st_sz = 2*hw + 1;
+
+nr_chank = floor(nr/st_sz);
+nc_chank = floor(nc/st_sz);
+
+fhs = zeros(size(fh,1),nr_chank*nc_chank);
+
+idx = 0;
+for k=1+hw:st_sz:nc-hw,
+        
+        fprintf('.');
+        sk = max(1,k-nw);
+        ek = min(nc,k+nw);
+
+        
+        % for each column,
+        for j=1+hw:st_sz:nr-hw,
+           sj = max(1,j-nw);
+           ej = min(nr,j+nw);
+        
+           id = j+(k-1)*nr;
+           idx = idx+1;
+           for li=sj:ej,
+              for lj=sk:ek,
+                idn = li+(lj-1)*nr;
+
+                fhs(:,idx) = fhs(:,idx) + fh(:,idn);
+                
+              end
+           end
+        end
+end
+
+fprintf('\n');
+
+           
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_sf.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_sf.m
new file mode 100755
index 0000000..d0d60f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histnb_sf.m
@@ -0,0 +1,52 @@
+function fhs = colize_histnb_s(fvs,Is,hb,nw,hw)
+%
+%  fhs = colize_histneigh(fvs,Is,hb,nw,hw)
+%         
+%
+
+[nf,np] = size(fvs);
+
+[nr,nc] = size(Is);
+
+st_sz = 2*hw + 1;
+
+nr_chank = floor(nr/st_sz);
+nc_chank = floor(nc/st_sz);
+
+tnbins = prod(hb.nbins(1:nf));
+disp(sprintf('allocat memory for %d x %d',tnbins,nr_chank*nc_chank));
+
+fhs = zeros(tnbins,nr_chank*nc_chank);
+
+idx = 0;
+for k=1+hw:st_sz:nc-hw,
+        
+        fprintf(',');
+        sk = max(1,k-nw);
+        ek = min(nc,k+nw);
+
+        
+        % for each column,
+        for j=1+hw:st_sz:nr-hw,
+           sj = max(1,j-nw);
+           ej = min(nr,j+nw);
+        
+           id = j+(k-1)*nr;
+           idx = idx+1;
+
+           %% find idx for the neighboring points
+           lis = [sj:ej]'*ones(1,ek-sk+1);
+           ljs = ones(ej-sj+1,1)*[sk:ek];
+           idns = lis+(ljs-1)*nr;
+           
+           fh = colize_joint_hist(fvs(:,idns(:)),hb);
+
+           fhs(:,idx) = sum(fh')';
+
+        end
+end
+
+fprintf('\n');
+
+           
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histneighb.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histneighb.m
new file mode 100755
index 0000000..6189cab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_histneighb.m
@@ -0,0 +1,37 @@
+function fhs = colize_histneigh(fh,Is,nw)
+%
+%  fhs = colize_histneigh(fh,fvs,nw)
+%         
+%
+
+[tnbins,np] = size(fh);
+
+[nr,nc] = size(Is);
+
+fhs = zeros(size(fh));
+
+for j=1:nr,
+        fprintf('.');
+        sj = max(1,j-nw);
+        ej = min(nr,j+nw);
+        
+        % for each column,
+        for k=1:nc,
+           sk = max(1,k-nw);
+           ek = min(nc,k+nw);
+
+           id = j+(k-1)*nr;
+
+           for li=sj:ej,
+              for lj=sk:ek,
+                idn = li+(lj-1)*nr;
+
+                fhs(:,id) = fhs(:,id) + fh(:,idn);
+              end
+           end
+        end
+end
+fprintf('\n');
+
+           
+   
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_joint_hist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_joint_hist.m
new file mode 100755
index 0000000..e7844d8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_joint_hist.m
@@ -0,0 +1,41 @@
+function fh = colize_joint_hist(fv,hb)
+%      (hb = sigs,bin_mins,bin_maxs,nbins)
+%
+%  take which histogram value and turn it into histogram bin
+%
+
+
+        [nf,np] = size(fv);
+
+        nbins = [0,hb.nbins];
+        %disp(sprintf('need matrix of %d x %d ',prod(hb.nbins),np));
+
+        fh = zeros(hb.nbins(1),hb.nbins(2),np);
+
+        k=1;
+                bin_min = hb.bmins(k);
+                bin_max = hb.bmaxs(k);
+                nbin    = nbins(k+1);
+                sig     = hb.sigs(k);
+                %fprintf('.');
+
+                b1 = binize(fv(k,:),sig,bin_min,bin_max,nbin);
+        k=2;
+                bin_min = hb.bmins(k);
+                bin_max = hb.bmaxs(k);
+                nbin    = nbins(k+1);
+                sig     = hb.sigs(k);
+                %fprintf('.');
+
+                b2 = binize(fv(k,:),sig,bin_min,bin_max,nbin);
+
+        
+        for k=1:hb.nbins(1),
+           for j=1:hb.nbins(2),
+                fh(k,j,:) = b1(k,:).*b2(j,:);
+           end
+        end
+
+%fprintf('\n');
+
+fh = reshape(fh,hb.nbins(1)*hb.nbins(2),np);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_test.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_test.m
new file mode 100755
index 0000000..a9135cc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/colize_test.m
@@ -0,0 +1,19 @@
+function t1a = colize(t1,I1);
+
+if 1,
+t1a = t1;
+%t1a = 1.2*half_sigmoid(t1,0.3,0.1);;
+t1a = reshape(t1a,size(t1,1)*size(t1,2),1,size(t1,3));
+t1a = squeeze(t1a);
+t1a = t1a';
+
+%I1a = I1(:)';I1a = I1a-mean(I1a(:));t1a = [I1a;t1a];
+
+else
+  mask = t1>=0;
+  t1a = abs(t1);
+  t1a = 0.5-t1a;
+  t1a = reshape(t1a,size(t1,1)*size(t1,2),1,size(t1,3));
+  t1a = squeeze(t1a);
+  t1a = t1a';
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compact.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compact.m
new file mode 100755
index 0000000..9863e0f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compact.m
@@ -0,0 +1,36 @@
+function I = compact(img,ws)
+
+
+%ws = 2*hws+1;
+
+[sy,sx] = size(img);
+
+rem_x = rem(sx,ws);
+rem_y = rem(sy,ws);
+
+fix_x = ceil(sx/ws);
+fix_y = ceil(sy/ws);
+
+fprintf('nr = %d, nc = %d\n',fix_y,fix_x);
+
+%startx= 1 + floor(rem_x*0.5)+hws;
+%starty= 1 + floor(rem_y*0.5)+hws;
+
+I = zeros(fix_y,fix_x);
+
+yid = 0;
+for j=1:ws:sy,
+   xid = 0;
+   yid = yid +1;
+   fprintf('.');
+   for k=1:ws:sx,
+       xid = xid+1;
+       %I(yid,xid) = median(median(img(j-hws:j+hws,k-hws:k+hws)));
+       %I(yid,xid) = sum(sum(img(j-hws:j+hws,k-hws:k+hws)));
+       v = img(j:min(sy,j+ws-1),k:min(sx,k+ws-1));
+       %I(yid,xid) = median(reshape(v,prod(size(v)),1));
+       I(yid,xid) = median(median(v));
+   end
+end
+fprintf('\n');
+ 
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_J.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_J.m
new file mode 100755
index 0000000..99b8b69
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_J.m
@@ -0,0 +1,31 @@
+function J = compute_J(A,I,size_x,size_y,D)
+%%  function J = compute_J(A,I,size_x,size_y,D)
+%
+
+[center_x,center_y] = find_center(size_x,size_y);
+
+tmp = ones(size_y,1)*[1:size_x];
+index(:,1) = reshape(tmp,size_x*size_y,1)-center_x*ones(size_x*size_y,1);
+index(:,2) = reshape(tmp',size_x*size_y,1)-center_y*ones(size_x*size_y,1);
+
+position_new = A*index'+ [D(1),0;0,D(2)]*ones(2,size_x*size_y);
+position_new = round(position_new +...
+ [center_x,0;0,center_y]*ones(2,size_x*size_y));
+% we have to deal with out of boundary ones
+%
+bad_ones(1,:) = position_new(1,:)<1 | position_new(1,:)>size_x;
+bad_ones(2,:) = position_new(2,:)<1 | position_new(2,:)>size_y;
+bad = max([bad_ones(1,:);bad_ones(2,:)]);
+good = ~bad;
+% if new index is out of boundary, then set it to (0,0)
+position_new(1,:) = position_new(1,:).*good;
+position_new(2,:) = position_new(2,:).*good;
+
+new_index = size_y*(position_new(1,:)-ones(1,size_x*size_y))+...
+    position_new(2,:);
+new_index = max([new_index;ones(1,size_x*size_y)]);
+J = I(new_index);
+% set the "out of boundary" to zero.
+J = J.*good;
+J = reshape(J',size_y,size_x);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_Lf.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_Lf.m
new file mode 100755
index 0000000..7cda523
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_Lf.m
@@ -0,0 +1,35 @@
+function dists = compute_Lf(F,cts,wz,nr,nc)
+
+gap = 2*wz(1)+1;
+hw = wz(1);
+
+nr = nr+1;
+nc = nc+1;
+
+dists = zeros(size(cts,1),nr*nc);
+for ctj = 1:size(cts,1),
+    t1 = cutout(F,cts(ctj,:),wz);
+
+    rid = 1;
+    fprintf('>');
+    
+    for ri = hw+1:gap:size(F,1)-hw,
+        %fprintf('[%d]',ri);
+        cid = 1;
+        for ci = hw+1:gap:size(F,2)-hw,
+          %fprintf('(%d)',ci);
+          t2 = cutout(F,[ci,ri],wz);
+
+          dist = abs(mean(t1(:))-mean(t2(:)));
+
+          dists(ctj,rid+cid*nr) = max(dist,dists(ctj,rid+cid*nr));
+
+          cid = cid+1;
+        end
+        rid = rid+1;
+     end
+
+   %fprintf('\n');
+
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_corr.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_corr.m
new file mode 100755
index 0000000..92f9da4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_corr.m
@@ -0,0 +1,10 @@
+function a = compute_corr(f,g)
+%
+%  compute the circular correlation of f and g
+%  at points around zero
+%
+%
+
+ff = interp(f,4);
+gg = interp(g,4);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff.m
new file mode 100755
index 0000000..72bfe54
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff.m
@@ -0,0 +1,36 @@
+function B = compute_diff(Ja,Jfa,hw,hnb);
+%
+%    B = compute_diff(Ja,Jfa,hw,hnb)
+%
+%
+
+figure(1);%imagesc(Ja);axis('image');
+cs = round(ginput(1));
+
+B = zeros(2*hnb+1,2*hnb+1);
+
+scales = [1:5];filter_ids = [1:7];
+Jc = get_win(Ja,cs,[hw,hw]);
+Jfc= get_win5(Jfa,cs,[hw,hw]);
+H2c = hist2d(Jc,Jfc,scales,filter_ids);
+
+figure(2);imagesc(Ja);axis('image');colormap(gray);
+hold on; plot(cs(1),cs(2),'g*');
+
+
+for ii=-hnb:hnb,
+  for jj=-hnb:hnb,
+     J1 = get_win(Ja,cs+4*[jj,ii],[hw,hw]);
+     Jf1= get_win5(Jfa,cs+4*[jj,ii],[hw,hw]);
+     figure(2);plot(cs(1)+4*jj,cs(2)+4*ii,'ro');drawnow;
+     %figure(3);imagesc(J1);drawnow;
+
+     H2 = hist2d(J1,Jf1,scales,filter_ids);
+     d = hist_diff(H2/prod(size(Jc)),H2c/prod(size(Jc)));
+     disp(sprintf('d=%f',d));
+     B(ii+hnb+1,jj+hnb+1) = d;
+
+   end
+end
+
+figure(2);hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch.m
new file mode 100755
index 0000000..260b93a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch.m
@@ -0,0 +1,34 @@
+function a = compute_diff_patch(gx1,gy1,gx2,gy2,I1,I2)
+%
+%    a = compute_diff_patch(gx1,gy1,gx2,gy2,I1,I2)
+%
+%
+
+%ws = size(gx1);
+%mask = smooth(ones(ws),2*max(ws));
+%mask = mask/sum(sum(mask));
+
+%mag1= sum(sum(sqrt((mask.*gx1).^2 + (mask.*gy1).^2)));
+%mag2= sum(sum(sqrt((mask.*gx2).^2 + (mask.*gy2).^2)));
+
+mag1= sum(sum(sqrt((gx1).^2 + (gy1).^2)));
+mag2= sum(sum(sqrt((gx2).^2 + (gy2).^2)));
+
+P_tx1 = sigmoid(mag1,400,80);
+P_tx2 = sigmoid(mag2,400,80);
+
+diff_I = mean(reshape(I1,prod(size(I1)),1))-...
+         mean(reshape(I2,prod(size(I2)),1));
+diff_I = abs(diff_I);
+
+s_g1 = [sum(sum(abs(gx1))),sum(sum(abs(gy1)))];
+s_g2 = [sum(sum(abs(gx2))),sum(sum(abs(gy2)))];
+
+s_g1 = s_g1/(norm(s_g1));
+s_g2 = s_g2/(norm(s_g2));
+
+a = (1-P_tx1)*(1-P_tx2)*exp(-diff_I/0.1) +...
+    P_tx1*P_tx2*(dot(s_g1,s_g2));
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch2.m
new file mode 100755
index 0000000..9d2b528
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_diff_patch2.m
@@ -0,0 +1,45 @@
+function [a,phi1,phi2] = compute_diff_patch(gx1,gy1,gx2,gy2,I1,I2)
+%
+%    a = compute_diff_patch(gx1,gy1,gx2,gy2,I1,I2)
+%
+%
+
+%ws = size(gx1);
+%mask = smooth(ones(ws),2*max(ws));
+%mask = mask/sum(sum(mask));
+
+%mag1= sum(sum(sqrt((mask.*gx1).^2 + (mask.*gy1).^2)));
+%mag2= sum(sum(sqrt((mask.*gx2).^2 + (mask.*gy2).^2)));
+
+mag1= sum(sum(sqrt((gx1).^2 + (gy1).^2)))/prod(size(gx1));
+mag2= sum(sum(sqrt((gx2).^2 + (gy2).^2)))/prod(size(gx1));
+
+P_tx1 = sigmoid(mag1,2,0.5);
+P_tx2 = sigmoid(mag2,2,0.5);
+
+diff_I = mean(reshape(I1,prod(size(I1)),1))-...
+         mean(reshape(I2,prod(size(I2)),1));
+diff_I = abs(diff_I);
+
+[l1,l2,phi1] = mwis(gx1,gy1);
+[k1,k2,phi2] = mwis(gx2,gy2);
+
+ratio1 = min([l1,l2])/max([l1,l2]);
+ratio2 = min([k1,k2])/max([k1,k2]);
+
+r1 = 1-sigmoid(ratio1,0.35,0.05);
+r2 = 1-sigmoid(ratio2,0.35,0.05);
+
+s1 = [cos(phi1),sin(phi1)];
+s2 = [cos(phi2),sin(phi2)];
+
+angle = acos(abs(dot(s1,s2)))*180/pi;
+
+a1 = (1-P_tx1*P_tx2)*exp(-diff_I/0.1);
+a2 = P_tx1*P_tx2*(r1*r2*(90-angle)/90);
+a3 = P_tx1*P_tx2*((1-r1*r2)*(1-sigmoid(abs(r1-r2),0.3,0.04)));
+
+a = a1+a2+a3;
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter.m
new file mode 100755
index 0000000..04e78e1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter.m
@@ -0,0 +1,84 @@
+function [filter_output,filters] = compute_filter(I,sig,r,sz);
+%
+%
+%
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+as = ori_offset:ori_incr:180+ori_offset-ori_incr;
+
+filter_output = [];
+filters = [];
+
+wsz = 2*round(sz) + 1;
+M1 = wsz(1);M2 = wsz(2);
+
+%%%%% prepare FFT of image  %%%%%%%%%%%%%
+
+[N1,N2]=size(I);
+tmp=zeros(size(I)+[M1-1 M2-1]);
+tmp(1:N1,1:N2)=I;
+IF=fft2(tmp);
+
+
+%%%%%%%%%% filtering stage %%%%%%%%%%%
+if size(sig,2)== 1,
+
+  for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = mdoog2(sig,r,angle,round(sz));
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    filters(:,:,j) = g;
+
+    gF  = fft2(g,N1+M1-1,N2+M2-1);
+    IgF = If.*gF;
+    Ig  = real(ifft2(IgF));
+    Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+    %c = conv2(I,g,'valid');
+
+    filter_output(:,:,j) = Ig;
+  end
+else
+
+  % there are multiple scales
+  sigs = sig;
+  szs = sz;
+  for k = 1:size(sigs,2),
+     sig = sigs(k);
+     sz = szs(k);
+     fprintf('%d',k);
+     for j = 1:length(as),
+        fprintf('.');
+        angle = as(j);
+
+        g = mdoog2(sig,r,angle,round(sz));
+        g = g - mean(reshape(g,prod(size(g)),1));
+        g = g/sum(sum(abs(g)));
+
+        gF  = fft2(g,N1+M1-1,N2+M2-1);
+        IgF = If.*gF;
+        Ig  = real(ifft2(IgF));
+        Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+        %c = conv2(I,g,'valid');
+        %c = conv2(I,g,'same');
+
+        filter_output(:,:,j,k) = Ig;
+        filters(:,:,j,k) = g; 
+     end
+
+
+  end
+
+end
+
+fprintf('\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter_fft.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter_fft.m
new file mode 100755
index 0000000..359c6ba
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/compute_filter_fft.m
@@ -0,0 +1,84 @@
+function [filter_output,filters] = compute_filter_fft(I,sig,r,sz,num_ori);
+%
+%
+%
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+as = ori_offset:ori_incr:180+ori_offset-ori_incr;
+
+filter_output = [];
+filters = [];
+
+wsz = 2*round(sz(end)) + 1;
+M1 = wsz;M2 = wsz;
+
+%%%%% prepare FFT of image  %%%%%%%%%%%%%
+
+[N1,N2]=size(I);
+tmp=zeros(size(I)+[M1-1 M2-1]);
+tmp(1:N1,1:N2)=I;
+IF=fft2(tmp);
+
+
+%%%%%%%%%% filtering stage %%%%%%%%%%%
+if size(sig,2)== 1,
+
+  for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = mdoog2(sig,r,angle,round(sz));
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    filters(:,:,j) = g;
+
+    gF  = fft2(g,N1+M1-1,N2+M2-1);
+    IgF = IF.*gF;
+    Ig  = real(ifft2(IgF));
+    Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+    %c = conv2(I,g,'valid');
+
+    filter_output(:,:,j) = Ig;
+  end
+else
+
+  % there are multiple scales
+  sigs = sig;
+  szs = sz;
+  for k = 1:size(sigs,2),
+     sig = sigs(k);
+     sz = szs(end);
+     fprintf('%d',k);
+     for j = 1:length(as),
+        fprintf('.');
+        angle = as(j);
+
+        g = mdoog2(sig,r,angle,round(sz));
+        g = g - mean(reshape(g,prod(size(g)),1));
+        g = g/sum(sum(abs(g)));
+
+        gF  = fft2(g,N1+M1-1,N2+M2-1);
+        IgF = IF.*gF;
+        Ig  = real(ifft2(IgF));
+        Ig  = Ig(ceil((M1+1)/2):ceil((M1+1)/2)+N1-1,ceil((M2+1)/2):ceil((M2+1)/2)+N2-1);
+   
+        %c = conv2(I,g,'valid');
+        %c = conv2(I,g,'same');
+
+        filter_output(:,:,j,k) = Ig;
+        filters(:,:,j,k) = g; 
+     end
+
+
+  end
+
+end
+
+fprintf('\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/conv_trim.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/conv_trim.m
new file mode 100755
index 0000000..30d16a9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/conv_trim.m
@@ -0,0 +1,6 @@
+% trims an array to remove meaningless pixels after a convolution with
+% an r * c window
+
+function[B] = conv_trim(A, r, c)
+
+B = A(r+1:size(A,1)-r, c+1:size(A,2)-c);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/corr_hist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/corr_hist.m
new file mode 100755
index 0000000..c538dc1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/corr_hist.m
@@ -0,0 +1,9 @@
+function alpha = corr_hist(hists)
+
+[y,x,v] = find(hists);
+mx = sum(x.*v)/sum(v);
+my = sum(y.*v)/sum(v);
+
+top = sum( (x-mx).*(y-my).*v);
+bottom = sqrt(sum( ((x-mx).^2).*v))*sqrt(sum( ((y-my).^2).*v));
+alpha = top/bottom;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/crop_im_fil.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/crop_im_fil.m
new file mode 100755
index 0000000..5472171
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/crop_im_fil.m
@@ -0,0 +1,11 @@
+function [J,f,rect] = crop_im_fil(Ja,Jfa,fig_id)
+%
+%
+
+figure(fig_id);
+imagesc(Ja);axis('image');
+
+[J,rect] = imcrop;rect = round(rect);
+J = Ja(rect(2):rect(2)+rect(4),rect(1):rect(1)+rect(3));
+f = Jfa(rect(2):rect(2)+rect(4),rect(1):rect(1)+rect(3),:,:);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutoff.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutoff.m
new file mode 100755
index 0000000..58c6b94
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutoff.m
@@ -0,0 +1,13 @@
+function I = cutoff(I,wc)
+%
+%
+
+nr = size(I,1);
+nc = size(I,2);
+
+if ndims(I) == 3,
+I = I(wc+1:nr-wc,wc+1:nc-wc,:,:);
+else
+I = I(wc+1:nr-wc,wc+1:nc-wc,:,:);
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutout.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutout.m
new file mode 100755
index 0000000..b80f27b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/cutout.m
@@ -0,0 +1,3 @@
+function a = cutout(I,ct,wz);
+
+a = I(ct(2)-wz(2):ct(2)+wz(2),ct(1)-wz(1):ct(1)+wz(1),:);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_Imask.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_Imask.m
new file mode 100755
index 0000000..dbd7fdb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_Imask.m
@@ -0,0 +1,20 @@
+function Imasks = disp_Imask(Is,nr,nc,hw,masks)
+%
+%   Imasks = disp_Imask(Is,nr,nc,hw,masks)
+%
+
+%hw = 2; %nr = 43;nc=68;
+gap = 2*hw+1;
+
+x = [1:nc*gap];
+y = [1:nr*gap];
+
+xs = (x-hw-1)/gap + 1;ys = (y-hw-1)/gap + 1;
+
+for gid=1:size(masks,3),
+  tmp = interp2(reshape(masks(:,:,gid),nr,nc),xs,ys');
+  
+  Imasks(:,:,gid) = (tmp>0.52).* ((Is).^0.8);
+  subplot(3,3,gid);
+  im(Imasks(:,:,gid));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_diff.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_diff.m
new file mode 100755
index 0000000..090c273
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_diff.m
@@ -0,0 +1,37 @@
+function disp_diff(H1,H2)
+%
+%  disp_diff(H1,H2)
+%
+
+ns = size(H1,3);
+nf = size(H1,4);
+
+H1 = H1/49;
+H2 = H2/49;
+
+
+sI= [1,0,1];sI = exp(-sI);
+sI = sI/sum(sI);
+
+for j = 1:ns,
+  for k = 1:nf,
+        h1 = H1(:,:,j,k);
+        h2 = H2(:,:,j,k);
+
+        subplot(ns,nf,(j-1)*nf+k);
+        h1s = conv2(conv2(h1,sI','same'),sI,'same');  
+        h2s = conv2(conv2(h2,sI','same'),sI,'same');  
+
+        [is,js] = find( (h1>0) | (h2>0));
+        ids = (js-1)*size(h1,1) + is;
+
+        hdiff = abs(h1s-h2s).*((h1>0) | (h2>0));
+
+        xdiff = ((h1(ids)-h2(ids)).*(h1(ids)-h2(ids)))./(h1(ids)+h2(ids));
+
+        xdiffs = ((h1s(ids)-h2s(ids)).*(h1s(ids)-h2s(ids)))./(h1s(ids)+h2s(ids));
+        imagesc(hdiff);colorbar;axis('off');
+%        title(sprintf('%3.3f, %3.3f',sum(sum(hdiff))/49,sum(sum(abs(h1-h2)))/49));drawnow;
+        title(sprintf('%3.3f, %3.3f',sum(xdiff),sum(xdiffs)));drawnow
+   end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult.m
new file mode 100755
index 0000000..e07556a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult.m
@@ -0,0 +1,435 @@
+%fn = '134035';
+%fn = '130040';
+%fn = '334074';
+fn = '130065';
+
+%basedir = 'plaatje_data/olddata/';
+%  basedir = 'data/'; nr = 49;nc =30;
+
+basedir = 'plaatje_data/';
+
+fname = sprintf('%s%s_eigvec.pfm',basedir,fn);
+eigv =  readpfm(fname);
+fname = sprintf('%s%s_eigval.pfm',basedir,fn);
+eigval = readpfm(fname);
+
+fname = sprintf('%s%s_ncutvec.pfm',basedir,fn);
+ncutv = readpfm(fname);
+fname = sprintf('%s%s_ncutval.pfm',basedir,fn);
+ncutval = readpfm(fname);
+
+%fname = sprintf('images/130039.pgm');
+fname = sprintf('images/%s.pgm',fn);
+I = readpgm(fname);
+cutsz = 20; I = cutoff(I,cutsz);
+figure(3);im(I);colormap(gray);
+
+new = 0;
+
+if ~new,
+ 
+ %nr = 49;nc = 30;
+ nr = 30;nc = 49;
+
+%nr = 68;nc = 43;
+%nc = 68;nr = 43;
+
+else
+
+  fn1 = fn;
+  fn = 'test';
+  fname = sprintf('plaatje_data/%s_gcs.pfm',fn);
+  gcs = readpfm(fname);
+
+  fname = sprintf('plaatje_data/%s_gce.pfm',fn);
+  gce = readpfm(fname);
+
+  fname = sprintf('plaatje_data/%s_grs.pfm',fn);
+  grs = readpfm(fname);
+
+  fname = sprintf('plaatje_data/%s_gre.pfm',fn);
+  gre = readpfm(fname);
+
+  nr = max(gre(:))+1;
+  nc = max(gce(:))+1;
+
+  fn = fn1;
+
+end
+
+figure(6);
+for j=1:8,
+  subplot(3,3,j);
+  im(reshape(ncutv(:,j+1),nr,nc));colorbar
+  title(num2str(ncutval(j+1,1)));
+end
+%cm = sprintf('print -dps ncut_%s',fn);disp(cm);eval(cm);
+subplot(3,3,9);im(I);axis('off');
+
+figure(7);clf
+for j=1:12,
+  subplot(3,4,j);
+  im(reshape(eigv(:,j),nr,nc));colorbar;%axis('off');
+  title(sprintf('%3.4e',eigval(j,1)));
+end
+%cm = sprintf('print -dps eig_%s',fn);disp(cm);eval(cm);
+
+%%%%%%%%%%%
+
+ev = eigval(:,1);
+figure(5);hold off;clf;subplot(1,2,1);
+%semilogy((ev(1:end-1) - ev(2:end))./ev(1:end-1),'x-');grid on;
+plot((ev(1:end-1) - ev(2:end))./ev(1:end-1),'x-');grid on;
+%semilogy(0.01*ones(size(ev(2:end-1))),'r-');semilogy(0.005*ones(size(ev(2:end-1))),'r-');semilogy(0.0025*ones(size(ev(2:end-1))),'r-');grid on;hold off;
+subplot(1,2,2);
+%semilogy(ev(1:end-1)-ev(2:end),'p-');grid on;
+semilogy((ev(1:end-1) - ev(2:end))/ev(1),'x-');grid on;
+
+
+if 0,
+
+fname = sprintf('plaatje_data/ncutval_%s.pfm',fn);
+nval =  readpfm(fname);
+fname = sprintf('plaatje_data/ncutvec_%s.pfm',fn);
+nv = readpfm(fname);
+
+figure(2);
+nvv = size(nv,2);
+for j=1:min(5,nvv-1),
+  subplot(1,min(5,nvv-1),j);
+  ims(nv(:,j+1),nr,nc);
+end
+
+
+%figure(5);
+%subplot(2,2,1);plot(eigval(:,1),'x-');
+
+
+if 0,
+
+fname = 130039;
+for j=0:20,
+ cm = sprintf('!cp plaatje_data/%d_%d.pfm plaatje_data/test_%d.pfm ',fname,j,j);
+ disp(cm);eval(cm);
+end
+
+%%%%%%%%
+fnout = 'test';fn_t = '334003';
+cm = sprintf('!cp plaatje_data/%s_eigval.pfm %s_eigval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp plaatje_data/%s_eigvec.pfm %s_eigvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp plaatje_data/%s_ncutvec.pfm %s_ncutvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp plaatje_data/%s_ncutval.pfm %s_ncutval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+
+
+
+
+end
+
+disp_flag = 0;
+if disp_flag,
+  [I1,bnr,bnc] = proj_back_id(ncutv(:,2),gcs,gce,grs,gre);
+  imvs(I,I1>0.002,bnr,bnc);
+end
+
+if 0,
+
+ nv = 3;
+ A = eigv(:,1:nv)*eigv(:,1:nv)';
+ [v,d] = ncut(abs(A),min(nv,5));
+
+ figure(3);
+ for j=1:min(nv,5),
+   subplot(2,2,j);
+   ims(v(:,j),nr,nc);
+ end
+
+end
+
+%%%%%%%%
+
+figure(4);%im(I);colorbar;
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));
+ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);im(abs(reshape(A(idx,:),nr,nc)));%colorbar;
+
+
+
+  %%%%%
+
+  fname = 'test2';
+  fn = sprintf('plaatje_data/ncut_%s.pfm',fname);
+  ncutv1 = readpfm(fn);
+ nr = 30; nc=49;
+
+ figure(1);
+ for j=1:min(4,size(ncutv1,2)),
+   subplot(2,2,j);
+   ims(ncutv1(:,j+1),nr,nc);
+ end
+
+
+
+%%%%%%%%%%
+
+  id = 0;
+  fn = sprintf('plaatje_data/test_Aa%d.pfm',id);
+  disp(sprintf('A = readpfm(%s);',fn));
+  A = readpfm(fn);
+
+  cm = sprintf('[v%d,d%d] = eigs(A,12);',id,id);
+  disp(cm);eval(cm);
+
+  writepfm('test_eigv0.pfm',v0);
+  writepfm('test_eigva0.pfm',diag(d0));
+
+
+
+
+vs = zeros(size(v1,1),size(v1,2),6);
+ds = zeros(length(d1),6);
+
+for j=0:5,
+  cm = sprintf('vs(:,:,%d) = v%d;',j+1,j);
+  disp(cm);eval(cm);
+  cm = sprintf('d = diag(d%d);',j);
+  disp(cm);eval(cm);
+  cm = sprintf('ds(:,%d) = d(:);',j+1);
+  disp(cm);eval(cm);
+
+
+end
+
+%save evsum vs ds
+
+figure(1);nr = 49;nc=30;evid = 3;
+for j=1:12,subplot(3,4,j);ims(vs(:,j,evid),nr,nc);end
+
+I = readpgm('images/334039.pgm');I = cutoff(I,20);
+
+As = zeros(6,nr*nc);
+
+figure(3);%im(I);colormap(gray);
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);
+
+figure(4);nvs = [6,9,12,12,12,12];
+for evid = 1:5,As(evid,:) = squeeze(vs(idx,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))';end
+for evid =1:5,subplot(2,3,evid);im(abs(reshape(As(evid,:),nr,nc)));colorbar;end
+subplot(2,3,6);ims(sum(abs(As)),nr,nc);colorbar
+
+%%%%%%%%%
+
+%%%%%%  eig of the As over all scales %%
+
+A = zeros(nr*nc,nr*nc);
+
+for evid=1:5,  disp(evid);
+  A = A + abs(squeeze(vs(:,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))');
+end
+
+[v,d] = eigs(A,12);
+figure(1); for j=1:12, subplot(3,4,j);ims(v(:,j),nr,nc);end
+
+[vn,dn] = ncut_b(A,12);
+figure(3); for j=1:12, subplot(3,4,j);ims(-vn(:,j),nr,nc);end
+
+nv = 6;
+A = abs(eigv(:,1:nv)*eigv(:,1:nv)');
+[v,d] = ncut_b(A,nv+1);
+figure(1);
+nv = 4;
+for j=1:nv,subplot(2,nv,j);ims(v(:,j+1),nr,nc);title(sprintf('%3.3e',d(j+1)));end
+
+for j=1:nv,subplot(2,nv,j+nv);ims(eigv(:,j),nr,nc);title(sprintf('%3.3e',eigval(j,1)));end
+
+%%%%%%%%%%%%%
+
+while 1,
+figure(3);%im(I);colormap(gray);
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(1);
+ims(exp(-(A(idx,:))/(0.02^2)),nr,nc);colorbar
+end
+
+
+
+%%%%%%%%%%%%%%
+
+figure(3);
+hw = 3;st_sz = 2*hw+1;
+np = 20;
+ct = round(ginput(np));
+ct_chank =[];
+ct_chank(:,1) = round((ct(:,1)-hw-1)/st_sz) + 1;
+ct_chank(:,2) = round((ct(:,2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+%As = readpfm_id('plaatje_data/130040_AX.pfm',idx,2924);
+As = readpfm_idf('plaatje_data/tmp/134035_AX3.pfm',idx,nr*nc);
+
+%save dist_data2 As idx ct_chank ct hw nr nc eigv eigval
+
+%load dist_data1a
+
+set(gcf,'DefaultLineLinewidth',5);
+
+minA = min(min(As));
+figure(1);clf; hold off;
+set(gcf,'DefaultLineLinewidth',2);
+for id = 1:np,
+subplot(4,5,id);
+%image(2.8e-2*((-minA)+reshape(As(id,:),nr,nc)));axis('image');axis('off');hold on
+ims(-As(id,:),nr,nc);axis('off');hold on
+plot(ct_chank(id,1)+1,ct_chank(id,2)+1,'rx');hold off;
+end
+
+figure(1);clf;hold off;
+nvv = 6
+set(gcf,'DefaultLineLinewidth',1);
+for id=1:np,
+ At = abs(eigv(idx(id),1:nvv)*eigv(:,1:nvv)');
+ subplot(4,5,id);
+ %image(2.5e4*reshape(At,nr,nc));axis('image');axis('off');hold on
+ ims(At,nr,nc);axis('off');hold on;
+ plot(ct_chank(id,1)+1,ct_chank(id,2)+1,'rx');hold off;
+end
+
+
+print_flag =0;
+if print_flag,
+  fn = '130040';
+
+  figure(4);clf;
+  colormap(gray);
+  set(gcf,'DefaultLineLinewidth',7);
+
+  for id =1:np,
+    %image(2.8e-2*((-minA)+reshape(As(id,:),nr,nc)));axis('image');axis('off');hold on
+    ims(-As(id,:),nr,nc);axis('off');
+    hold on;plot(ct_chank(id,1)+1,ct_chank(id,2)+1,'rp');hold off;
+    cm = sprintf('print -deps dist_x1_%s_%d',fn,id);
+    disp(cm);eval(cm);
+  end
+
+  nvv = 5;
+  set(gcf,'DefaultLineLinewidth',7);
+  figure(4);colormap(gray);
+  for id=1:np,
+    At = abs(eigv(idx(id),1:nvv)*eigv(:,1:nvv)');
+    %image(1.5e4*reshape(At,nr,nc));axis('image');axis('off');%hold on
+    ims(At,nr,nc);axis('off');%hold on;
+    %plot(ct_chank(id,1)+1,ct_chank(id,2)+1,'rp');hold off;
+    cm = sprintf('print -deps dist_d_%s_%d',fn,id);
+    disp(cm);eval(cm);
+  end
+
+  % print eigvects 
+  for j=1:size(eigv,2),
+    ims(eigv(:,j),nr,nc);axis('off');
+    cm = sprintf('print -deps eigv_%s_%d',fn,j);
+    disp(cm);eval(cm);
+  end
+ 
+  for j=1:size(ncutv,2),
+    ims(ncutv(:,j),nr,nc);axis('off');
+    cm = sprintf('print -deps ncutv_%s_%d',fn,j);
+    disp(cm);eval(cm);
+  end
+
+
+end
+
+basedir ='plaatje_data/newdata/';
+fname = sprintf('%s%s_eigvec.pfm',basedir,fn);
+eigv =  readpfm(fname);
+
+ix = 1;
+figure(5);colormap(gray);clf
+for j=1:7,
+  for k=[2,3,4,6,9,12];
+   subplot(7,6,ix);
+   At = abs(eigv(idx(j),1:k)*eigv(:,1:k)');
+   ims(At,nr,nc);axis('off');%colorbar;
+   if (k==2),
+    hold on; plot(ct_chank(j,1),ct_chank(j,2),'rp');hold off;
+    title(num2str(j));
+   end
+   ix = ix+1;
+  end
+end
+
+figure(4);clf;colormap(gray);
+set(gcf,'DefaultLineLinewidth',7);
+for j=1:20,
+  for k=[2,3,4,6,9,12];
+   
+   At = abs(eigv(idx(j),1:k)*eigv(:,1:k)');
+   ims(At,nr,nc);axis('off');%colorbar;
+   if (k==2),
+    hold on; plot(ct_chank(j,1),ct_chank(j,2),'rp');hold off;
+   end
+
+   cm = sprintf('print -deps dist_scale_65_%d_%d',j,k);
+   disp(cm);eval(cm);
+
+  end
+end
+
+base_dir = 'plaatje_data/';
+
+% cts are the centers,
+
+wz = [hw,hw];
+gap = 2*hw+1;
+dist = zeros(size(cts,1),(nr+1)*(nc+1));
+
+for j=1:1:24,
+  fn = sprintf('%s134035_%d.pfm',base_dir,j);
+  disp(fn);
+  F = readpfm(fn);
+
+  dists = compute_Lf(F,cts,wz,nr,nc);
+  dist = max(dists,dist);
+ 
+end
+
+
+figure(4);clf;colormap(gray);
+set(gcf,'DefaultLineLinewidth',7);
+
+ids = [8,1,12,5,10,15];
+
+for j=1:6,
+ 
+   d = reshape(dist(j,:),nr+1,nc+1);
+   d = d(1:end-1,2:end);
+   im(-d);axis('off'); hold on;
+   plot(ct_chank(ids(j),1),ct_chank(ids(j),2),'p');
+   hold off
+
+   cm = sprintf('print -deps dist_lf_65_%d',j);
+   disp(cm);eval(cm);
+
+   pause;
+end
+
+
+
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult2.m
new file mode 100755
index 0000000..46239ef
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresult2.m
@@ -0,0 +1,215 @@
+fn = '130042';
+
+fname = sprintf('data/%s_eigvec.pfm',fn);
+eigv =  readpfm(fname);
+fname = sprintf('data/%s_eigval.pfm',fn);
+eigval = readpfm(fname);
+
+fname = sprintf('data/%s_ncutvec.pfm',fn);
+ncutv = readpfm(fname);
+fname = sprintf('data/%s_ncutval.pfm',fn);
+ncutval = readpfm(fname);
+
+%fname = sprintf('images/130038.pgm');
+fname = sprintf('images/%s.pgm',fn);
+I = readpgm(fname);
+cutsz = 20; I = cutoff(I,cutsz);
+figure(3);im(I);colormap(gray);
+
+new = 0;
+
+if new,
+  fn1 = fn;
+  fn = 'test';
+  fname = sprintf('data/%s_gcs.pfm',fn);
+  gcs = readpfm(fname);
+
+  fname = sprintf('data/%s_gce.pfm',fn);
+  gce = readpfm(fname);
+
+  fname = sprintf('data/%s_grs.pfm',fn);
+  grs = readpfm(fname);
+
+  fname = sprintf('data/%s_gre.pfm',fn);
+  gre = readpfm(fname);
+
+  nr = max(gre(:))+1;
+  nc = max(gce(:))+1;
+
+  fn = fn1;
+
+else
+ %nr = 49;nc = 30;
+ nr = 30;nc = 49;
+
+end
+
+figure(6);
+for j=1:8,
+  subplot(3,3,j);
+  im(reshape(ncutv(:,j+1),nr,nc));colorbar
+  title(num2str(ncutval(j+1,1)));
+end
+%cm = sprintf('print -dps ncut_%s',fn);disp(cm);eval(cm);
+subplot(3,3,9);im(I);axis('off');
+
+figure(7);clf
+for j=1:9,
+  subplot(3,3,j);
+  im(reshape(eigv(:,j),nr,nc));colorbar;%axis('off');
+  title(sprintf('%3.4e',eigval(j,1)));
+end
+%cm = sprintf('print -dps eig_%s',fn);disp(cm);eval(cm);
+
+
+
+if 0,
+
+fname = 130042;
+for j=0:30,
+ cm = sprintf('!cp plaatje_data/%d_%d.pfm data/%d_%d.pfm ',fname,j,fname,j);
+ disp(cm);eval(cm);
+end
+
+%%%%%%%%
+fnout = '130042';fn_t = '130042';
+cm = sprintf('!cp data/%s_eigval.pfm %s_eigval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_eigvec.pfm %s_eigvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_ncutvec.pfm %s_ncutvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_ncutval.pfm %s_ncutval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+
+
+
+
+end
+
+disp_flag = 0;
+if disp_flag,
+  [I1,bnr,bnc] = proj_back_id(ncutv(:,2),gcs,gce,grs,gre);
+  imvs(I,I1>0.002,bnr,bnc);
+end
+
+if 0,
+
+ nv = 3;
+ A = eigv(:,1:nv)*eigv(:,1:nv)';
+ [v,d] = ncut(abs(A),min(nv,5));
+
+ figure(3);
+ for j=1:min(nv,5),
+   subplot(2,2,j);
+   ims(v(:,j),nr,nc);
+ end
+
+
+%%%%%%%%
+
+figure(4);%im(I);colorbar;
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));
+ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);im(abs(reshape(A(idx,:),nr,nc)));%colorbar;
+
+
+
+  %%%%%
+
+  fname = 'test2';
+  fn = sprintf('data/ncut_%s.pfm',fname);
+  ncutv1 = readpfm(fn);
+ nr = 30; nc=49;
+
+ figure(1);
+ for j=1:min(4,size(ncutv1,2)),
+   subplot(2,2,j);
+   ims(ncutv1(:,j+1),nr,nc);
+ end
+
+
+
+%%%%%%%%%%
+
+  id = 0;
+  fn = sprintf('data/test_Aa%d.pfm',id);
+  disp(sprintf('A = readpfm(%s);',fn));
+  A = readpfm(fn);
+
+  cm = sprintf('[v%d,d%d] = eigs(A,12);',id,id);
+  disp(cm);eval(cm);
+
+  writepfm('test_eigv0.pfm',v0);
+  writepfm('test_eigva0.pfm',diag(d0));
+
+
+
+
+vs = zeros(size(v1,1),size(v1,2),6);
+ds = zeros(length(d1),6);
+
+for j=0:5,
+  cm = sprintf('vs(:,:,%d) = v%d;',j+1,j);
+  disp(cm);eval(cm);
+  cm = sprintf('d = diag(d%d);',j);
+  disp(cm);eval(cm);
+  cm = sprintf('ds(:,%d) = d(:);',j+1);
+  disp(cm);eval(cm);
+
+
+end
+
+%save evsum vs ds
+
+figure(1);nr = 49;nc=30;evid = 3;
+for j=1:12,subplot(3,4,j);ims(vs(:,j,evid),nr,nc);end
+
+I = readpgm('images/334039.pgm');I = cutoff(I,20);
+
+As = zeros(6,nr*nc);
+
+figure(3);%im(I);colormap(gray);
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);
+
+figure(4);nvs = [6,9,12,12,12,12];
+for evid = 1:5,As(evid,:) = squeeze(vs(idx,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))';end
+for evid =1:5,subplot(2,3,evid);im(abs(reshape(As(evid,:),nr,nc)));colorbar;end
+subplot(2,3,6);ims(sum(abs(As)),nr,nc);colorbar
+
+%%%%%%%%%
+
+%%%%%%  eig of the As over all scales %%
+
+A = zeros(nr*nc,nr*nc);
+
+for evid=1:5,  disp(evid);
+  A = A + abs(squeeze(vs(:,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))');
+end
+
+[v,d] = eigs(A,12);
+figure(1); for j=1:12, subplot(3,4,j);ims(v(:,j),nr,nc);end
+
+[vn,dn] = ncut_b(A,12);
+figure(3); for j=1:12, subplot(3,4,j);ims(-vn(:,j),nr,nc);end
+
+nv = 6;
+A = abs(eigv(:,1:nv)*eigv(:,1:nv)');
+[v,d] = ncut_b(A,nv+1);
+figure(1);
+for j=1:nv,subplot(2,nv,j);ims(v(:,j+1),nr,nc);title(sprintf('%3.3e',d(j+1)));end
+
+for j=1:nv,subplot(2,nv,j+nv);ims(eigv(:,j),nr,nc);title(sprintf('%3.3e',eigval(j,1)));end
+
+
+
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresulthome.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresulthome.m
new file mode 100755
index 0000000..208b86a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_evresulthome.m
@@ -0,0 +1,237 @@
+fn = '334003';
+
+fname = sprintf('%s_eigvec.pfm',fn);
+disp(sprintf('reading %s',fname));
+eigv =  readpfm(fname);
+fname = sprintf('%s_eigval.pfm',fn);
+eigval = readpfm(fname);
+
+fname = sprintf('%s_ncutvec.pfm',fn);
+ncutv = readpfm(fname);
+fname = sprintf('%s_ncutval.pfm',fn);
+ncutval = readpfm(fname);
+
+%fname = sprintf('images/130038.pgm');
+fname = sprintf('images/%s.pgm',fn);
+I = readpgm(fname);
+cutsz = 20; I = cutoff(I,cutsz);
+figure(3);im(I);colormap(gray);
+
+new = 0;
+
+if new,
+  fn1 = fn;
+  fn = 'test';
+  fname = sprintf('%s_gcs.pfm',fn);
+  gcs = readpfm(fname);
+
+  fname = sprintf('%s_gce.pfm',fn);
+  gce = readpfm(fname);
+
+  fname = sprintf('%s_grs.pfm',fn);
+  grs = readpfm(fname);
+
+  fname = sprintf('%s_gre.pfm',fn);
+  gre = readpfm(fname);
+
+  nr = max(gre(:))+1;
+  nc = max(gce(:))+1;
+
+  fn = fn1;
+
+else
+ nr = 49;nc = 30;
+ %nr = 30;nc = 49;
+
+end
+
+figure(6);
+for j=1:8,
+  subplot(3,3,j);
+  im(reshape(ncutv(:,j+1),nr,nc));colorbar
+  title(num2str(ncutval(j+1,1)));
+end
+%cm = sprintf('print -dps ncut_%s',fn);disp(cm);eval(cm);
+%subplot(3,3,9);im(I);axis('off');
+
+figure(7);clf
+for j=1:9,
+  subplot(3,3,j);
+  im(reshape(eigv(:,j),nr,nc));colorbar;%axis('off');
+  title(sprintf('%3.4e',eigval(j,1)));
+end
+%cm = sprintf('print -dps eig_%s',fn);disp(cm);eval(cm);
+
+
+
+if 0,
+
+fname = 130042;
+for j=0:30,
+ cm = sprintf('!cp plaatje_data/%d_%d.pfm data/%d_%d.pfm ',fname,j,fname,j);
+ disp(cm);eval(cm);
+end
+
+%%%%%%%%
+fnout = '130042';fn_t = '130042';
+cm = sprintf('!cp data/%s_eigval.pfm %s_eigval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_eigvec.pfm %s_eigvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_ncutvec.pfm %s_ncutvec.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+cm = sprintf('!cp data/%s_ncutval.pfm %s_ncutval.pfm',fnout,fn_t);
+disp(cm);eval(cm);
+
+
+
+
+end
+
+disp_flag = 0;
+if disp_flag,
+  [I1,bnr,bnc] = proj_back_id(ncutv(:,2),gcs,gce,grs,gre);
+  imvs(I,I1>0.002,bnr,bnc);
+end
+
+if 0,
+
+ nv = 3;
+ A = eigv(:,1:nv)*eigv(:,1:nv)';
+ [v,d] = ncut(abs(A),min(nv,5));
+
+ figure(3);
+ for j=1:min(nv,5),
+   subplot(2,2,j);
+   ims(v(:,j),nr,nc);
+ end
+
+
+%%%%%%%%
+
+figure(4);%im(I);colorbar;
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));
+ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);im(abs(reshape(A(idx,:),nr,nc)));%colorbar;
+
+
+
+  %%%%%
+
+  fname = 'test2';
+  fn = sprintf('data/ncut_%s.pfm',fname);
+  ncutv1 = readpfm(fn);
+ nr = 30; nc=49;
+
+ figure(1);
+ for j=1:min(4,size(ncutv1,2)),
+   subplot(2,2,j);
+   ims(ncutv1(:,j+1),nr,nc);
+ end
+
+
+
+%%%%%%%%%%
+
+  id = 0;
+  fn = sprintf('test_Aa%d.pfm',id);
+  disp(sprintf('A = readpfm(%s);',fn));
+  A = readpfm(fn);
+
+  cm = sprintf('[v%d,d%d] = eigs(A,12);',id,id);
+  disp(cm);eval(cm);
+
+  writepfm('test_eigv0.pfm',v0);
+  writepfm('test_eigva0.pfm',diag(d0));
+
+
+
+
+vs = zeros(size(v1,1),size(v1,2),6);
+ds = zeros(length(d1),6);
+
+for j=0:5,
+  cm = sprintf('vs(:,:,%d) = v%d;',j+1,j);
+  disp(cm);eval(cm);
+  cm = sprintf('d = diag(d%d);',j);
+  disp(cm);eval(cm);
+  cm = sprintf('ds(:,%d) = d(:);',j+1);
+  disp(cm);eval(cm);
+
+
+end
+
+%save evsum vs ds
+
+figure(1);nr = 49;nc=30;evid = 3;
+for j=1:12,subplot(3,4,j);ims(vs(:,j,evid),nr,nc);end
+
+I = readpgm('images/334039.pgm');I = cutoff(I,20);
+
+As = zeros(6,nr*nc);
+
+figure(3);%im(I);colormap(gray);
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(5);
+
+figure(4);nvs = [6,9,12,12,12,12];
+for evid = 1:5,As(evid,:) = squeeze(vs(idx,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))';end
+for evid =1:5,subplot(2,3,evid);im(abs(reshape(As(evid,:),nr,nc)));colorbar;end
+subplot(2,3,6);ims(sum(abs(As)),nr,nc);colorbar
+
+%%%%%%%%%
+
+%%%%%%  eig of the As over all scales %%
+
+A = zeros(nr*nc,nr*nc);
+
+for evid=1:5,  disp(evid);
+  A = A + abs(squeeze(vs(:,1:nvs(evid),evid))*squeeze(vs(:,1:nvs(evid),evid))');
+end
+
+[v,d] = eigs(A,12);
+figure(1); for j=1:12, subplot(3,4,j);ims(v(:,j),nr,nc);end
+
+[vn,dn] = ncut_b(A,12);
+figure(3); for j=1:12, subplot(3,4,j);ims(-vn(:,j),nr,nc);end
+
+nv = 6;
+A = abs(eigv(:,1:nv)*eigv(:,1:nv)');
+[v,d] = ncut_b(A,nv+1);
+figure(1);
+for j=1:nv,subplot(2,nv,j);ims(v(:,j+1),nr,nc);title(sprintf('%3.3e',d(j+1)));end
+
+for j=1:nv,subplot(2,nv,j+nv);ims(eigv(:,j),nr,nc);title(sprintf('%3.3e',eigval(j,1)));end
+
+%%%%%%%%%%%%%%%%%
+
+while 1,
+figure(3);%im(I);colormap(gray);
+hw = 3;st_sz = 2*hw+1;
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr + ct_chank(:,2);
+
+figure(1);
+ims(exp(-(A(idx,:))/(0.03^2)),nr,nc);colorbar
+end
+
+disp_evresulthome;
+close(3);close(7);close(6);
+A = euclid_dist(ncutv(:,2:6));
+A =  exp(-A/(0.05^2));
+
+[v,d] = eigs(A,9);
+
+figure(2);
+for j=1:9,subplot(3,3,j);ims(v(:,j),nr,nc);colorbar;end
+
+
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_groups.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_groups.m
new file mode 100755
index 0000000..d087218
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_groups.m
@@ -0,0 +1,13 @@
+function disp_groups(groups,ids,nr,nc);
+
+np = ids(end);
+
+baseid =1;
+for j=1:length(ids),
+ mask = zeros(np,1);
+ mask(groups(baseid:ids(j))) = 1+mask(groups(baseid:ids(j)));
+
+ subplot(3,3,j);
+ ims(mask,nr,nc);
+ baseid = 1+ids(j);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_hist2d.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_hist2d.m
new file mode 100755
index 0000000..4313234
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/disp_hist2d.m
@@ -0,0 +1,15 @@
+function H2 = disp_hist2d(J,Jf,scales,filter_ids)
+
+ns = length(scales);
+nf = length(filter_ids);
+
+H2 = [];
+for j=1:ns,
+  for k=1:nf,
+    subplot(ns,nf,(j-1)*nf+k);
+    H2d = hist_I_f(J,Jf(:,:,filter_ids(k),scales(j)));
+    imagesc(H2d);axis('image');axis('off');drawnow;
+    H2(:,:,j,k) = H2d;
+  end
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair.m
new file mode 100755
index 0000000..1c7b2cf
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair.m
@@ -0,0 +1,28 @@
+function d = dist_pair(idx,fv,hb)
+%     (hb=sigs,bin_mins,bin_maxs,nbins)
+%
+%
+%  computes the pairwise distance between
+%  a point and everyone else using histogram binized feature
+% 
+
+
+[nf,np] = size(fv);
+
+d = zeros(1,np);
+nbins = [0,hb.nbins];
+
+
+for j=1:nf,
+        bin_min = hb.bmins(j);
+        bin_max = hb.bmaxs(j);
+        nbin    = nbins(j+1);
+        sig     = hb.sigs(j);
+        fprintf(sprintf('|%d',j));
+        b = binize(fv(j,:),sig,bin_min,bin_max,nbin);
+
+        a = binize(fv(j,idx),sig,bin_min,bin_max,nbin);
+
+        d = d + a'*b;
+end
+fprintf('\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair_chank.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair_chank.m
new file mode 100755
index 0000000..96a0d60
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/dist_pair_chank.m
@@ -0,0 +1,25 @@
+function d = dist_pair_chank(a,fvs,chank_size)
+%     (hb=sigs,bin_mins,bin_maxs,nbins)
+%
+%
+%  computes the pairwise distance between
+%  a point and everyone else using histogram binized feature
+% 
+
+
+[nf,np] = size(fvs);
+
+n_chanks = ceil(np/chank_size);
+
+d = [];
+for j=1:n_chanks,
+        fprintf('<');
+
+        cm = sprintf('load st_%d',j);
+        eval(cm);
+        fprintf(sprintf('%d',n_chanks-j));
+
+        d = [d,a*fh];
+end
+
+fprintf('\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/doog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/doog2.m
new file mode 100755
index 0000000..3ec22c1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/doog2.m
@@ -0,0 +1,43 @@
+function [G]=doog2(sig,r,th,N);
+% [G,H]=doog2(sig,r,th,N);
+% Make difference of offset gaussians kernel
+% theta is in degrees
+% (see Malik & Perona, J. Opt. Soc. Amer., 1990)
+%
+% Example:
+% >> imagesc(-doog2(.5,4,15,32))
+% >> colormap(gray)
+
+% by Serge Belongie
+
+no_pts=N;              % no. of points in x,y grid
+pad_pts=no_pts*sqrt(2); % pad grid dimensions for up to a 45 degree rotation
+siz=6;                  % range of x,y grid 
+
+[x,y]=meshgrid(linspace(-siz,siz,pad_pts),linspace(-siz,siz,pad_pts));
+
+a=-1;
+b=2;
+c=-1;
+
+ya=sig;
+yc=-ya;
+yb=0;
+sigy=sig;
+sigx=r*sig;
+
+Ga=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-ya)/sigy).^2));
+Gb=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yb)/sigy).^2));
+Gc=(1/(2*pi*sigx*sigy))*exp(-(((x-0)/sigx).^2+((y-yc)/sigy).^2));
+
+Go = a*Ga + b*Gb + c*Gc;
+%Ho = imag(hilbert(Go));
+G = Go;
+
+G = mimrotate(Go,th,'bilinear','crop');
+G = imcrop(G,[(pad_pts-no_pts)/2, (pad_pts-no_pts)/2, no_pts, no_pts]);
+
+%H = imrotate(Ho,th,'bilinear','crop');
+%H = imcrop(H,[(pad_pts-no_pts)/2, (pad_pts-no_pts)/2, no_pts, no_pts]);
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp.m
new file mode 100755
index 0000000..5209088
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp.m
@@ -0,0 +1,15 @@
+function [v,d] = eig_decomp(A)
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+D1 = ds'*ones(1,length(ds));
+A = D1'.*A.*D1;
+
+disp(sprintf('computing eig values'));
+tic;[v,d] = eig(A);toc;
+
+d = abs(diag(d));
+[tmp,idx] = sort(-d);
+d = d(idx);
+v = v(:,idx);
+v = D1.*v;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp_v5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp_v5.m
new file mode 100755
index 0000000..e0fab2c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_decomp_v5.m
@@ -0,0 +1,13 @@
+function [v,d] = eig_decomp_v5(A,nv)
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+D1 = ds'*ones(1,length(ds));
+A = D1'.*A.*D1;
+
+disp(sprintf('computing eig values'));
+tic;[v,d] = eigs(A,nv);toc;
+
+d = abs(diag(d));
+
+v = D1(:,1:size(v,2)).*v;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_proj.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_proj.m
new file mode 100755
index 0000000..78d5296
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eig_proj.m
@@ -0,0 +1,12 @@
+function v = eig_proj(u,data)
+
+% fd = feature dimension, nv = num. of eigvectors
+[fd,nv] = size(u);
+
+[fd2,nd] = size(data);
+
+if (fd ~= fd2),
+    error(sprintf('size don't match'));
+else
+        v = data'*u;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eigs_decomp.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eigs_decomp.m
new file mode 100755
index 0000000..7763124
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/eigs_decomp.m
@@ -0,0 +1,39 @@
+function [v,d,D,Ipara] = eigs_decomp(fn,num_eigs)
+%
+%   function [v,d,D,Ipara] = eigs_decomp(fn,num_eigs)
+%
+
+%fn = '2.ppm';
+fn = 'images/130049.pgm';
+
+
+% spatial gaussian parameter
+xscale = 3;
+
+% half size of the neighbourhood 
+xnb = 6;
+
+% setting the the HSV gaussian parameter:[h s v]
+Iscale = [0.008,0.01,0.01];
+
+Input_para = [xscale,xnb,Iscale];
+
+% compute the lower half the association matrix
+[A,D,Ipara] = compute_A_ppm(fn,Input_para);
+
+B = A+A';
+clear A;
+
+% eigen decompostion
+options.tol = 1e-7;
+num_eig_v = 4;
+fprintf('doing eigs ...\n');
+[v,d] = eigs(B,num_eig_v,options);
+
+d = diag(d);
+
+% to display the final result
+
+%nr = Ipara(1);nc = Ipara(2);
+%k = 1;imagesc(reshape(v(:,k).*D,nc,nr)');colorbar
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/euclid_dist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/euclid_dist.m
new file mode 100755
index 0000000..cbf0734
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/euclid_dist.m
@@ -0,0 +1,22 @@
+function [A,mag] = euclid_dist(v)
+
+
+
+A = 2*v*v';
+
+nv = size(v,2);
+if (nv>1)
+ mag = sum((v.*v)')';
+else
+ mag = v.*v;
+end
+
+np = length(mag);
+
+for j=1:np,
+  A(:,j) = mag-A(:,j);
+end
+
+for j=1:np,
+  A(j,:) = mag' + A(j,:);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_all_files.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_all_files.m
new file mode 100755
index 0000000..c1752e5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_all_files.m
@@ -0,0 +1,29 @@
+sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);
+
+load filenames;
+
+nfiles = size(filename,1);
+
+for j = 48:nfiles,
+   fname = ['images/',filename(j,:)];
+   fname
+   I = readpgm(fname);
+
+   text_des = compute_filter(I,sigs,r,szs);
+
+   data_name = sprintf('filter_%s.mat',filename(j,:));
+   cm = sprintf('save %s ',data_name);
+
+   disp(cm);
+   eval(cm);
+   clear;
+   
+   sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);
+
+   load filenames;
+
+   nfiles = size(filename,1);
+
+   
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_output.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_output.m
new file mode 100755
index 0000000..a489c6e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/filter_output.m
@@ -0,0 +1,38 @@
+function If = filter_output(I,sigs,szs,flag);
+%
+%  compute filter output for all orientation and scale,
+%
+
+%% flag = 1 if compute oriented filter output
+if (~exist('flag')),
+  flag = 1;
+end
+
+
+If = [];
+
+for j = 1:length(sigs),
+   sig = sigs(j);
+   sz = 2*round(4*sig)+1;
+
+   g = mkdog1(sig,sz);
+   fprintf('[');
+   fprintf('.');
+   If(:,:,1,j) = conv2(I,g,'same');
+
+   angles = [0:30:150];
+   r = 3;
+
+   if flag,
+   for k = 1:length(angles),
+     fprintf('.');
+     g = mdoog2(sig,r,angles(k),szs(j));
+     If(:,:,k+1,j) = conv2(I,g,'same');
+   end
+   end
+
+   fprintf(']');
+
+end
+
+fprintf('\n');
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_bst_cut.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_bst_cut.m
new file mode 100755
index 0000000..c85cc2b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_bst_cut.m
@@ -0,0 +1,24 @@
+function [cut_threshold,max_asso] = find_bst_cut(fn_base,para,threshold,Gmask)
+
+basedir = 'plaatje_data/';
+%basedir = './';
+
+fn = sprintf('%sbst_cut.tex',basedir);
+write_command(fn,fn_base,para);
+
+fn= sprintf('%sthreshold_%s.pfm',basedir,fn_base);
+writepfm(fn,threshold(:));
+
+fn= sprintf('%sGmask_%s.pfm',basedir,fn_base);
+writepfm(fn,Gmask(:));
+
+cd plaatje_data
+!./find_bestcut
+cd /home/barad-dur/vision/malik/jshi/proj/grouping/texture
+
+fn = sprintf('%sbst_asso_%s.pfm',basedir,fn_base);
+results = readpfm(fn);
+asso = results(1,:);
+[max_asso,id] = max(asso);
+cut_threshold = threshold(id);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_center.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_center.m
new file mode 100755
index 0000000..b5a127a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_center.m
@@ -0,0 +1,4 @@
+function [center_x,center_y] = find_center(size_x,size_y);
+
+center_x = 0.5*(size_x -1)+1;
+center_y = 0.5*(size_y -1)+1;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_cutpoint.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_cutpoint.m
new file mode 100755
index 0000000..5cab956
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/find_cutpoint.m
@@ -0,0 +1,13 @@
+function [cutpoints,x] = find_cutpoint(data,cmap,nbin)
+%
+%  [cutpoints,x] = find_cutpoint(data,cmap,nbin)
+%
+%
+
+x = id_cut(data,cmap,nbin);
+
+cutpoints = zeros(1,nbin);
+
+for j=1:nbin,
+ cutpoints(j) = max(data(x<=j));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/gen_filters.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/gen_filters.m
new file mode 100755
index 0000000..65e3c3a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/gen_filters.m
@@ -0,0 +1,47 @@
+function filters = gen_filters(sig,r,sz);
+
+
+as = 0:30:150;
+
+filters = [];
+
+if size(sig,2)== 1,
+
+  for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = mdoog2(sig,r,angle,round(sz));
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    filters(:,:,j) = g;
+  end
+else
+
+  % there are multiple scales
+  sigs = sig;
+  szs = sz;
+  for k = 1:size(sigs,2),
+     sig = sigs(k);
+     sz = szs(length(szs)-1);
+     fprintf('%d',k);
+     for j = 1:length(as),
+        fprintf('.');
+        angle = as(j);
+
+        g = mdoog2(sig,r,angle,round(sz));
+        g = g - mean(reshape(g,prod(size(g)),1));
+        g = g/sum(sum(abs(g)));
+
+        filters(:,:,j,k) = g;
+     end
+
+
+  end
+
+end
+
+fprintf('\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist.m
new file mode 100755
index 0000000..b05d680
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist.m
@@ -0,0 +1,9 @@
+function cumhists = get_cumhist(hists)
+%
+%
+%     cumhists = get_cumhist(hists)
+%
+
+cumhists.inten = cumsum(hists.inten)/sum(hists.inten);
+cumhists.text = cumsum(hists.text,1)./(ones(size(hists.text,1),1)*sum(hists.text,1));
+cumhists.mag = cumsum(hists.mag)/sum(hists.mag);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist_inten.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist_inten.m
new file mode 100755
index 0000000..911b4e6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_cumhist_inten.m
@@ -0,0 +1,7 @@
+function CH_inten = get_cumhist(hists)
+%
+%
+%     cumhists = get_cumhist(hists)
+%
+
+CH_inten = cumsum(hists)/sum(hists);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist.m
new file mode 100755
index 0000000..d480a3a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist.m
@@ -0,0 +1,24 @@
+function [hists,bins] = get_hists(J,Jbar)
+%
+%
+%  produce histogram output of the image J and its
+%    filter outputs Jbar
+%
+
+maxval = 60;
+bin = [1:4:maxval+1];
+
+w = size(J);
+
+
+[hists.inten,bins.inten] = hist(reshape(J,prod(w),1),[1:26:256]);
+
+for j=1:size(Jbar,3),
+   hists.text(:,j) = hist(reshape(abs(Jbar(:,:,j)),prod(w),1),bin);
+end
+
+bins.text = bin;
+
+[hists.mag,bins.mag] = hist(reshape(sum(abs(Jbar),3),prod(w),1),[1:10:161]);
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist_inten.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist_inten.m
new file mode 100755
index 0000000..c8357c6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_hist_inten.m
@@ -0,0 +1,15 @@
+function [Hinten,Hbins] = get_hists_inten(J,nbin)
+%
+%
+%  produce histogram output of the image J and its
+%    filter outputs Jbar
+%
+
+
+w = size(J);
+
+[Hinten,Hbins] = hist(reshape(J,prod(w),1),linspace(1,256,nbin));
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win.m
new file mode 100755
index 0000000..411a694
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win.m
@@ -0,0 +1,10 @@
+function J = get_win(I,center,wc)
+%
+%   J = get_win(I,center,wc)
+%
+%        center: [x,y]
+
+
+
+J = I(center(2)-wc(2):center(2)+wc(2),...
+      center(1)-wc(1):center(1)+wc(1));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win5.m
new file mode 100755
index 0000000..e8404e5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/get_win5.m
@@ -0,0 +1,11 @@
+function J = get_win5(I,center,wc)
+%
+%   J = get_win5(I,center,wc)
+%
+%        center: [x,y]
+
+
+
+J = I(center(2)-wc(2):center(2)+wc(2),...
+      center(1)-wc(1):center(1)+wc(1),:,:);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/grad.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/grad.m
new file mode 100755
index 0000000..6da0fbf
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/grad.m
@@ -0,0 +1,24 @@
+% gradient of an image
+% coordinates (r, c) follow matrix convention;
+% the gaussian is truncated at x = +- tail, and there are samples samples
+% inbetween, where samples = hsamples * 2 + 1
+
+function[gr,gc] = gradient(image, hsamples)
+
+tail=4;
+samples = hsamples * 2 + 1;
+
+x = linspace(-tail, tail, samples);
+gauss = exp(-x.^2);
+n = gauss * ones(samples,1);
+gauss = gauss/n;
+
+gaussderiv = -x.*gauss;
+n = -gaussderiv*linspace(1,samples,samples)';
+gaussderiv = gaussderiv/n;
+
+gr = conv2(conv2(image, gaussderiv','valid'), gauss,'valid');
+gc = conv2(conv2(image, gaussderiv,'valid'), gauss','valid');
+
+%gr = conv_trim(gr, hsamples, hsamples);
+%gc = conv_trim(gc, hsamples, hsamples);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/half_sigmoid.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/half_sigmoid.m
new file mode 100755
index 0000000..c187b6c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/half_sigmoid.m
@@ -0,0 +1,17 @@
+function a = half_sigmoid(x,offset,sig)
+%
+%   a = half_sigmoid(x,offset,sig)
+%      
+%       a = ones(size(x))./(1+exp(-(x-offset)/sig));
+%
+%   keep the sign of a
+
+sign_x = sign(x);
+x = abs(x);
+
+a = ones(size(x))./(1+exp(-(x-offset)/sig));
+
+off = 1/(1+exp(-(0-offset)/sig));
+
+a = sign_x.*(a-off);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist2d.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist2d.m
new file mode 100755
index 0000000..3f4db0c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist2d.m
@@ -0,0 +1,13 @@
+function H2 = hist2d(J,Jf,scales,filter_ids)
+
+ns = length(scales);
+nf = length(filter_ids);
+
+H2 = [];
+for j=1:ns,
+  for k=1:nf,
+      H2d = hist_I_f(J,Jf(:,:,filter_ids(k),scales(j)));
+      H2(:,:,j,k) = H2d;
+  end
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_I_f.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_I_f.m
new file mode 100755
index 0000000..a993661
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_I_f.m
@@ -0,0 +1,22 @@
+function h2d = hist_I_f(I,If,binI,binf)
+
+if (nargin == 2),
+  binI = [0:13:260];
+  binf = [-30:2.5*2:30];
+end
+
+%%% make 2d histogram bin
+h2d = [];
+
+for j = 2:length(binf),
+
+  [id_i,id_j] = find((If>binf(j-1)) & (If<=binf(j)));
+  if (length(id_i) >0), 
+    h = hist(I(id_i+(id_j-1)*size(I,1)),binI);
+  else
+    h = zeros(size(binI));
+  end
+
+  h2d = [h2d,h'];
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_diff.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_diff.m
new file mode 100755
index 0000000..6976c8f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_diff.m
@@ -0,0 +1,30 @@
+function hdiff = hist_diff(H1,H2)
+%
+%  hdiff = hist_diff(H1,H2)
+%
+
+ns = size(H1,3);
+nf = size(H1,4);
+
+sI= [1,0,1];sI = exp(-sI);
+sI = sI/sum(sI);
+
+hdiff = 0;
+for j = 1:ns,
+  for k = 1:nf,
+        h1 = H1(:,:,j,k);
+        h2 = H2(:,:,j,k);
+
+        h1s = conv2(conv2(h1,sI','same'),sI,'same');  
+        h2s = conv2(conv2(h2,sI','same'),sI,'same');  
+
+        [is,js] = find( (h1>0) | (h2>0));
+        ids = (js-1)*size(h1,1) + is;
+
+        xdiffs = ((h1s(ids)-h2s(ids)).*(h1s(ids)-h2s(ids)))./(h1s(ids)+h2s(ids));
+        hdiff = hdiff + sum(xdiffs);
+
+   end
+end
+
+hdiff = hdiff/(ns*nf);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_f.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_f.m
new file mode 100755
index 0000000..93f50a9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_f.m
@@ -0,0 +1,28 @@
+function h2d = hist_f(Ifs,f1,s1,f2,s2)
+
+
+binf = [-30:2.5*2:30];
+
+
+%%% make 2d histogram bin
+
+If1 = Ifs(:,:,f1,s1);
+If2 = Ifs(:,:,f2,s2);
+h2d = [];
+
+binf(1) = -100;
+binf(length(binf)) = 100;
+
+for j = 2:length(binf),
+
+  [id_i,id_j] = find((If1>binf(j-1)) & (If1<=binf(j)));
+  if (length(id_i) >0),
+    
+    h = hist(If2(id_i+(id_j-1)*size(If2,1)),binf);
+  else
+    h = zeros(size(binf));
+  end
+
+
+  h2d = [h2d,h'];
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_in_chank.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_in_chank.m
new file mode 100755
index 0000000..74661b2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_in_chank.m
@@ -0,0 +1,33 @@
+function covfh = hist_inner_chank(fv,chank_size,nbin)
+%  fh = hist_inner_chank(fv,hb,chank_file)
+%
+%         (hb = bin_mins,bin_maxs,nbins)
+%
+%  take which histogram value and turn it into histogram bin
+%  compute the inner product of the histogram bin features
+%
+
+[nf,np] = size(fv);
+
+tbins = nf*nbin;
+disp(sprintf('need matrix of %d x %d ',tbins,tbins));
+
+covfh = zeros(tbins,tbins);
+
+n_chanks = ceil(np/chank_size);
+for j=1:n_chanks,
+  fprintf('<');
+  
+  cm = sprintf('load st_%d',j);
+  eval(cm);
+  fprintf(sprintf('%d',n_chanks-j));
+
+  %ms = mean(fh');
+  %fh = fh- ms'*ones(1,size(fh,2));
+
+  covfh = covfh + fh*fh';
+  fprintf('>');
+end
+
+fprintf('\n');
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_inner.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_inner.m
new file mode 100755
index 0000000..6fd02b7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hist_inner.m
@@ -0,0 +1,40 @@
+function fh = hist_inner(fv,hb)
+%         (hb = bin_mins,bin_maxs,nbins)
+%
+%  take which histogram value and turn it into histogram bin
+%  compute the inner product of the histogram bin features
+%
+
+[nf,np] = size(fv);
+
+nbins = [0,hb.nbins];
+
+disp(sprintf('need matrix of %d x %d ',sum(nbins),sum(nbins)));
+
+fh = zeros(sum(nbins),sum(nbins));
+
+for j=1:nf,
+  bin_min = hb.bmins(j);
+  bin_max = hb.bmaxs(j);
+  nbin    = nbins(j+1);
+  sig     = hb.sigs(j);
+  fprintf('|');
+  b0 = binize(fv(j,:),sig,bin_min,bin_max,nbin);
+
+  fh(sum(nbins(1:j))+1:sum(nbins(1:j+1)),sum(nbins(1:j))+1:sum(nbins(1:j+1))) = b0*b0';
+  
+  for k=j+1:nf,
+        bin_min = hb.bmins(k);
+        bin_max = hb.bmaxs(k);
+        nbin    = nbins(k+1);
+        sig     = hb.sigs(k);
+        fprintf('.');
+        b = binize(fv(k,:),sig,bin_min,bin_max,nbin);
+        tmp = b0*b';
+
+        fh(sum(nbins(1:j))+1:sum(nbins(1:j+1)),sum(nbins(1:k))+1:sum(nbins(1:k+1))) = tmp;
+        fh(sum(nbins(1:k))+1:sum(nbins(1:k+1)),sum(nbins(1:j))+1:sum(nbins(1:j+1))) = tmp';
+  end
+end
+
+fprintf('\n');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/histbin_fv_chank.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/histbin_fv_chank.m
new file mode 100755
index 0000000..8cf2a35
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/histbin_fv_chank.m
@@ -0,0 +1,14 @@
+function histbin_fv_chank(fvs,hb,chank_size,fname_base)
+
+[nv,np] = size(fvs);
+
+k =1;
+for j=1:chank_size:np,
+    disp(sprintf('|%d',j));
+    fh = colize_hist(fvs(:,j:min(j+chank_size-1,np)),hb);
+    fname = sprintf('%s_%d.mat',fname_base,k);
+    cm = sprintf('save %s fh hb;',fname);
+    disp(cm);
+    eval(cm);
+    k = k+1;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hsv2clrs.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hsv2clrs.m
new file mode 100755
index 0000000..cfbb8b0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/hsv2clrs.m
@@ -0,0 +1,25 @@
+function [x,y,z] = hsv2clrs(h,s,v)
+%
+%  function [x,y,z] = hsv2clrs(h,s,v)
+%     if h is 3D matrix, output in 3D x
+%
+
+if (size(h,3) == 3),
+  s = h(:,:,2);
+  v = h(:,:,3);
+  h = h(:,:,1);
+
+  z = v;
+  xx = s.*v.*cos(2*pi*h);
+  y = s.*v.*sin(2*pi*h);
+
+  x(:,:,1) = xx;
+  x(:,:,2) = y;
+  x(:,:,3) = z;
+else
+  
+  z = v;
+  x = s.*v.*cos(2*pi*h);
+  y = s.*v.*sin(2*pi*h);
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/id_cut.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/id_cut.m
new file mode 100755
index 0000000..daf8f2b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/id_cut.m
@@ -0,0 +1,14 @@
+function [x,map] = idcut(data,cmap,nbin)
+%
+%
+%
+
+lc = size(cmap,1);
+
+data = data - min(data);
+data = 1+ ((lc-1)*data/max(data));
+
+r = cmap(data,1);g = cmap(data,2);b = cmap(data,3);
+
+[x,map] = vmquant(r,g,b,nbin);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im.m
new file mode 100755
index 0000000..6450120
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im.m
@@ -0,0 +1,3 @@
+function im(I)
+
+imagesc(I);axis('image');drawnow;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im3.m
new file mode 100755
index 0000000..b49e690
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im3.m
@@ -0,0 +1,3 @@
+function im3(d)
+
+imagesc(reshape(d,size(d,1),size(d,2)*size(d,3)));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im5.m
new file mode 100755
index 0000000..7f1b16d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im5.m
@@ -0,0 +1,16 @@
+function im5(data,nr,nc,mag)
+
+if nargin == 4,
+for j=1:size(data,3),
+   subplot(nr,nc,j);
+   imagesc(data(:,:,j)./mag);axis('image');axis('off');colorbar;drawnow;
+
+%   image(150*data(:,:,j));axis('image');axis('off');colorbar;drawnow;
+end
+
+else
+for j=1:size(data,3),
+   subplot(nr,nc,j);
+   imagesc(data(:,:,j));axis('image');axis('off');colorbar;drawnow;
+end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im_vect.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im_vect.m
new file mode 100755
index 0000000..d0a5b30
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/im_vect.m
@@ -0,0 +1,20 @@
+function a = im_vect(loca,v,scale);
+
+if ~exist('scale'),
+  scale = 50;
+end
+
+y = loca(1,:);
+x = loca(2,:);
+
+x = x - min(x);
+y = y - min(y);
+
+max_x = max(x);max_y = max(y);
+min_scale = min(max_x,max_y);
+
+x = scale*x/min_scale;
+y = scale*y/min_scale;
+
+
+a = sparse(y+1,x+1,v);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imrotate.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imrotate.m
new file mode 100755
index 0000000..167fd02
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imrotate.m
@@ -0,0 +1,119 @@
+function bout = imrotate(arg1,arg2,arg3,arg4)
+%IMROTATE Rotate image.
+%       B = IMROTATE(A,ANGLE,'method') rotates the image A by ANGLE
+%       degrees.  The image returned B will, in general, be larger 
+%       than A.  Invalid values on the periphery are set to one
+%       for indexed images or zero for all other image types. Possible 
+%       interpolation methods are 'nearest','bilinear' or 'bicubic'.
+%       'bilinear' is the default for intensity images, otherwise 
+%       'nearest' is used if no method is given.
+%
+%       B = IMROTATE(A,ANGLE,'crop') or IMROTATE(A,ANGLE,'method','crop')
+%       crops B to be the same size as A.
+%
+%       Without output arguments, IMROTATE(...) displays the rotated
+%       image in the current axis.
+%
+%       See also IMRESIZE, IMCROP, ROT90.
+
+%       Clay M. Thompson 8-4-92
+%       Copyright (c) 1992 by The MathWorks, Inc.
+%       $Revision: 1.14 $  $Date: 1993/09/01 21:27:38 $
+
+if nargin<2, error('Requires at least two input parameters.'); end
+if nargin<3,
+  if isgray(arg1), caseid = 'bil'; else caseid = 'nea'; end
+  docrop = 0;
+elseif nargin==3,
+  if isstr(arg3),
+    method = [lower(arg3),'   ']; % Protect against short method
+    caseid = method(1:3);
+    if caseid(1)=='c', % Crop string
+      if isgray(arg1), caseid = 'bil'; else caseid = 'nea'; end
+      docrop = 1;
+    else
+      docrop = 0;
+    end
+  else
+    error('''METHOD'' must be a string of at least three characters.');
+  end
+else
+  if isstr(arg3),
+    method = [lower(arg3),'   ']; % Protect against short method
+    caseid = method(1:3);
+  else
+    error('''METHOD'' must be a string of at least three characters.');
+  end
+  docrop = 1;
+end
+
+% Catch and speed up 90 degree rotations
+if rem(arg2,90)==0 & nargin<4,
+  phi = rem(arg2,360);
+  if phi==90,
+    b = rot90(arg1);
+  elseif phi==180,
+    b = rot90(arg1,2);
+  elseif phi==270,
+    b = rot90(arg1,-1);
+  else
+    b = arg1;
+  end
+  if nargout==0, imshow(b), else bout = b; end
+  return
+end
+      
+phi = arg2*pi/180; % Convert to radians
+
+% Rotation matrix
+T = [cos(phi) -sin(phi); sin(phi) cos(phi)];
+
+% Coordinates from center of A
+[m,n] = size(arg1);
+if ~docrop, % Determine limits for rotated image
+  siz = ceil(max(abs([(n-1)/2 -(m-1)/2;(n-1)/2 (m-1)/2]*T))/2)*2;
+  uu = -siz(1):siz(1); vv = -siz(2):siz(2);
+else % Cropped image
+  uu = (1:n)-(n+1)/2; vv = (1:m)-(m+1)/2;
+end
+nu = length(uu); nv = length(vv);
+
+blk = bestblk([nv nu]);
+nblks = floor([nv nu]./blk); nrem = [nv nu] - nblks.*blk;
+mblocks = nblks(1); nblocks = nblks(2);
+mb = blk(1); nb = blk(2);
+
+rows = 1:blk(1); b = zeros(nv,nu);
+for i=0:mblocks,
+  if i==mblocks, rows = (1:nrem(1)); end
+  for j=0:nblocks,
+    if j==0, cols = 1:blk(2); elseif j==nblocks, cols=(1:nrem(2)); end
+    if ~isempty(rows) & ~isempty(cols)
+      [u,v] = meshgrid(uu(j*nb+cols),vv(i*mb+rows));
+      % Rotate points
+      uv = [u(:) v(:)]*T'; % Rotate points
+      u(:) = uv(:,1)+(n+1)/2; v(:) = uv(:,2)+(m+1)/2;
+      if caseid(1)=='n', % Nearest neighbor interpolation
+        b(i*mb+rows,j*nb+cols) = interp6(arg1,u,v);
+      elseif all(caseid=='bil'), % Bilinear interpolation
+        b(i*mb+rows,j*nb+cols) = interp4(arg1,u,v);
+      elseif all(caseid=='bic'), % Bicubic interpolation
+        b(i*mb+rows,j*nb+cols) = interp5(arg1,u,v);
+      else
+        error(['Unknown interpolation method: ',method]);
+      end
+    end
+  end
+end
+
+d = find(isnan(b));
+if length(d)>0, 
+  if isind(arg1), b(d) = ones(size(d)); else b(d) = zeros(size(d)); end
+end
+
+if nargout==0,
+  imshow(b), return
+end
+bout = b;
+
+  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ims.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ims.m
new file mode 100755
index 0000000..2fb5f25
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ims.m
@@ -0,0 +1,3 @@
+function ims(I,nr,nc)
+
+im(reshape(I,nr,nc));
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imvs.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imvs.m
new file mode 100755
index 0000000..21dde73
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/imvs.m
@@ -0,0 +1,4 @@
+function imvs(I,v,nr,nc)
+
+v = reshape(v,nr,nc);
+im(I(1:size(v,1),1:size(v,2)).*v);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/is_step.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/is_step.m
new file mode 100755
index 0000000..4903778
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/is_step.m
@@ -0,0 +1,33 @@
+function P_step = is_step(gx,gy)
+% 
+%   P_step = is_step(gx,gy)
+%
+%      determine wheter gx,gy(which is first
+%      order filter output) is a response
+%      to a step function
+%
+
+M = zeros(2,2);
+M(1,1) = sum(sum(gx.*gx));
+M(2,2) = sum(sum(gy.*gy));
+M(1,2) = sum(sum(gx.*gy));
+M(2,1) = M(1,2);
+
+[v,d] = eig(M);
+d = diag(d);
+
+% make the first eig_value to be the smaller one
+if (d(2)< d(1)),
+   tmp = d(1);d(1) = d(2);d(2) = tmp;
+   tmp = v(:,1); v(:,1) = v(:,2); v(:,2) = tmp;
+end
+
+ratio = d(1)/d(2);
+threshold = 0;
+gx_ratio = sum(sum(gx.*(gx>threshold)))/(sum(sum(abs(gx).*(abs(gx)>threshold))));
+gx_ratio = max(gx_ratio,1-gx_ratio);
+
+gy_ratio = sum(sum(gy.*(gy>threshold)))/(sum(sum(abs(gy).*(abs(gy)>threshold))));
+gy_ratio = max(gy_ratio,1-gy_ratio);
+
+P_step = [ratio,gx_ratio,gy_ratio];
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ks_2d.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ks_2d.m
new file mode 100755
index 0000000..cfac005
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ks_2d.m
@@ -0,0 +1,20 @@
+function [na,nb,nc,nd] = ks_2d(cum_filter)
+
+[nb_filters,nb_bins] = size(cum_filter);
+
+T = 1;
+
+for j = [1:nb_filters],
+   for l = [1:nb_bins],
+        nc(j,l) = sum(cum_filter(1:j,l));
+        nd(j,l) = j*T - nc(j,l);
+
+        if (j~= nb_filters),
+          nb(j,l) = sum(cum_filter(j+1:nb_filters,l));
+          na(j,l) = (nb_filters-j)*T-nb(j,l);
+        else
+          nb(j,l) = 0;
+          na(j,l) = 0;
+        end
+    end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/load_result.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/load_result.m
new file mode 100755
index 0000000..0ff328c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/load_result.m
@@ -0,0 +1,39 @@
+
+fnameI = '130056';
+
+cm = sprintf('load filter_%s.pgm.mat',fnameI);
+disp(cm);
+eval(cm);
+
+text_des= reshape(text_des,size(text_des,1),size(text_des,2),size(text_des,3)*size(text_des,4));
+
+for j=1:size(text_des,3),
+   text_des(:,:,j) = abs(text_des(:,:,j)).*(text_des(:,:,j) <0);
+end
+
+%text_des = abs(text_des);
+
+
+  %%%% cutoff margins,
+margin = 6+10;
+
+Iw = cutoff(I,margin);
+
+
+T1 = cutoff(text_des,margin);
+
+%%%%% reduce resolution
+
+
+
+T1 = reduce_all(T1);
+T1 = reduce_all(T1);
+
+im5(T1,5,6);
+
+cm = sprintf('writepnm5(''%s_f.pnm'',%s)',fnameI,'T1/70');
+
+%  disp(cm);eval(cm);
+
+nr = size(T1,1);
+nc = size(T1,2);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/m_interp4.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/m_interp4.m
new file mode 100755
index 0000000..314f140
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/m_interp4.m
@@ -0,0 +1,49 @@
+function [F,mask] = m_interp4(z,s,t)
+%INTERP4 2-D bilinear data interpolation.
+%       ZI = INTERP4(Z,XI,YI) assumes X = 1:N and Y = 1:M, where
+%       [M,N] = SIZE(Z).
+%
+%       Copyright (c) 1984-93 by The MathWorks, Inc.
+%       Clay M. Thompson 4-26-91, revised 7-3-91, 3-22-93 by CMT.
+%
+%  modified to 
+
+ 
+[nrows,ncols] = size(z);
+
+
+if any(size(z)<[3 3]), error('Z must be at least 3-by-3.'); end
+if size(s)~=size(t), error('XI and YI must be the same size.'); end
+
+% Check for out of range values of s and set to 1
+sout = find((s<1)|(s>ncols));
+if length(sout)>0, s(sout) = ones(size(sout)); end
+
+% Check for out of range values of t and set to 1
+tout = find((t<1)|(t>nrows));
+if length(tout)>0, t(tout) = ones(size(tout)); end
+
+% Matrix element indexing
+ndx = floor(t)+floor(s-1)*nrows;
+
+% Compute intepolation parameters, check for boundary value.
+d = find(s==ncols); 
+s(:) = (s - floor(s));
+if length(d)>0, s(d) = s(d)+1; ndx(d) = ndx(d)-nrows; end
+
+% Compute intepolation parameters, check for boundary value.
+d = find(t==nrows);
+t(:) = (t - floor(t));
+if length(d)>0, t(d) = t(d)+1; ndx(d) = ndx(d)-1; end
+d = [];
+
+% Now interpolate, reuse u and v to save memory.
+F =  ( z(ndx).*(1-t) + z(ndx+1).*t ).*(1-s) + ...
+     ( z(ndx+nrows).*(1-t) + z(ndx+(nrows+1)).*t ).*s;
+
+mask = ones(size(z));
+
+% Now set out of range values to zeros.
+if length(sout)>0, F(sout) = zeros(size(sout));mask(sout)=zeros(size(sout));end
+if length(tout)>0, F(tout) = zeros(size(tout));mask(tout)=zeros(size(tout));end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/make_masks.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/make_masks.m
new file mode 100755
index 0000000..d2c8ba7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/make_masks.m
@@ -0,0 +1,12 @@
+function masks = make_masks(groups,ids,nr,nc);
+
+np = ids(end);
+
+baseid =1;
+for j=1:length(ids),
+ mask = zeros(np,1);
+ mask(groups(baseid:ids(j))) = 1+mask(groups(baseid:ids(j)));
+ baseid = 1+ids(j);
+
+ masks(:,:,j) = mask;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/makefilter.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/makefilter.m
new file mode 100755
index 0000000..a768269
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/makefilter.m
@@ -0,0 +1,14 @@
+function H = make_filter(txture,sze,noise)
+%  function H = make_filter(txture,sze)
+%
+%
+
+x = zeros(sze);
+tx_sze = size(txture);
+
+x(1:tx_sze(1),1:tx_sze(2)) = txture;
+x = reshape(x,1,sze(1)*sze(2));
+X = fft(x);
+figure(3);plot(abs(X).^2);
+
+H = X./(abs(X).^2+noise);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t1.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t1.m
new file mode 100755
index 0000000..256e609
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t1.m
@@ -0,0 +1,22 @@
+function [H,h] = make_filter(txture,sze,noise)
+%  function H = make_filter(txture,sze)
+%
+%
+
+tx_sze = size(txture);
+x = reshape(txture,1,tx_sze(1)*tx_sze(2));
+X = fft(x);
+H = X./(abs(X).^2+noise);
+h = reshape(real(ifft(H)),tx_sze(1),tx_sze(2));
+
+
+x = zeros(sze);
+x(1:tx_sze(1),1:tx_sze(2)) = h;
+figure(1);imagesc(x);drawnow;
+x = reshape(x,1,sze(1)*sze(2));
+H = fft(x);
+
+h = reshape(real(ifft(H)),sze(1),sze(2));
+figure(2);imagesc(h);
+
+figure(3); mesh(h);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t2.m
new file mode 100755
index 0000000..c84482d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_t2.m
@@ -0,0 +1,21 @@
+function [H,h] = make_filter(txture,sze,noise)
+%  function H = make_filter(txture,sze)
+%
+%
+
+x = zeros(sze);
+tx_sze = size(txture);
+
+[center_x,center_y] = find_center(sze(2),sze(1));
+tx_sze_h = round(0.5*tx_sze);
+
+x(center_y-tx_sze_h(1):center_y-tx_sze_h(1)+tx_sze(1)-1,...
+  center_x-tx_sze_h(2):center_x-tx_sze_h(2)+tx_sze(2)-1) = txture;
+figure(1);imagesc(x);drawnow;
+x = reshape(x,1,sze(1)*sze(2));
+X = fft(x);
+H = X./(abs(X).^2+noise);
+h = reshape(real(ifft(H)),sze(1),sze(2));
+figure(2);imagesc(h);
+
+figure(3); mesh(h);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_test.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_test.m
new file mode 100755
index 0000000..e8d0ad8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkf_test.m
@@ -0,0 +1,43 @@
+function [H,h] = make_filter(txture,sze,n_c,a,b,c)
+%  function H = make_filter(txture,sze)
+%
+%
+
+x = zeros(sze);
+tx_sze = size(txture);
+
+x(1:tx_sze(1),1:tx_sze(2)) = txture;
+%figure(1);imagesc(x);drawnow;
+x = reshape(x,1,sze(1)*sze(2));
+X = fft(x);
+power = abs(X).^2;
+
+figure(3);plot(power);
+len = length(X);
+
+t = [1:0.5*(length(X)-1),0.5*(length(X)-1):-1:1];
+
+top = max(power);
+if (c == -1),
+  c = top*5.0e-1
+end
+if (n_c == -1),
+  n_c = top*1.0e-1
+end
+
+nois = n_c +c*(exp(-a*(t.^b)));
+if (rem(len,2) == 1), 
+ noise = [c+n_c,nois];
+else 
+ noise = [c+n_c,nois,c+n_c];
+end
+
+figure(3);hold on;plot(noise,'r');
+hold off
+H = X./(abs(X).^2+noise);
+h = reshape(real(ifft(H)),sze(1),sze(2));
+figure(2);imagesc(h);
+
+figure(4);plot(abs(H).^2,'c')
+%figure(3); mesh(h);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkg.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkg.m
new file mode 100755
index 0000000..1fb1f7e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkg.m
@@ -0,0 +1,9 @@
+function g= mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%  
+%   function G = mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%
+ 
+size_wh = round(0.5*size_w);
+[x,y] = meshgrid([-size_wh:1:size_wh],[-size_wh:1:size_wh]);
+g = 1/(2*pi*sigma_x*sigma_y)*(exp(-( ((x-xo)/sigma_x).^2 + ((y-yo)/sigma_y).^2)));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkgaussian.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkgaussian.m
new file mode 100755
index 0000000..1213757
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkgaussian.m
@@ -0,0 +1,11 @@
+function G = mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%  
+%   function G = mkgaussian(xo,yo,sigma_x,sigma_y,size_w)
+%
+ 
+size_wh = round(0.5*size_w);
+
+[x,y] = meshgrid([-size_wh:1:size_wh],[-size_wh:1:size_wh]);
+
+G = 0.5/(sigma_x*sigma_y)*(exp(-( ((x-xo)/sigma_x).^2 + ((y-yo)/sigma).^2)));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkmulfilter.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkmulfilter.m
new file mode 100755
index 0000000..fe501b5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkmulfilter.m
@@ -0,0 +1,52 @@
+function H = make_multi_filter(templates,num_templates,sze,u,noise)
+%  function H = make_filter(templates,num_templates,sze,u,noise)
+%     templates are column vectors of template
+%     sze is the size of the filter  
+%     u is a vector of specified value
+%  
+%
+
+templates_size_x = size(templates,2)/num_templates;
+templates_size_y = size(templates,1);
+
+alpha = 1/num_templates;
+
+X = zeros(num_templates,sze(1)*sze(2));
+Spectrums = zeros(num_templates,sze(1)*sze(2));
+
+
+for k =1:num_templates,
+   tmp = zeros(sze);
+   tmp(1:templates_size_y,1:templates_size_x) =...
+     templates(:,(k-1)*templates_size_x+1:k*templates_size_x);
+   x(k,:) = reshape(tmp,1,sze(1)*sze(2));       
+   X(k,:) = fft(x(k,:));
+   Spectrums(k,:) = conj(X(k,:)).*X(k,:);
+end
+
+if num_templates > 1
+ sum_Spect = sum(Spectrums)*alpha;
+else
+ sum_Spect = Spectrums;
+end
+
+for row = 1:num_templates,
+  for column = 1:num_templates,
+
+     A(row,column) =  sum( ((conj(X(row,:)).*X(column,:))./...
+                           (sum_Spect + noise))')/(sze(1)*sze(2));
+  end
+end
+
+
+epsilon = inv(A)*u;
+
+top = epsilon(1)*X(1,:);
+for k = 2:num_templates,
+  top = top + epsilon(k)*X(k,:);
+end
+
+H = top./(sum_Spect + noise);
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkpoog2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkpoog2.m
new file mode 100755
index 0000000..4bd0366
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mkpoog2.m
@@ -0,0 +1,29 @@
+function doog2 = mkdoog2(sigma_w,r,theta,size_w)
+%
+% function doog2 = mkdoog2(sigma_w,r,theta,size_w)
+%
+%
+
+%scale_base = 2.8814; 
+scale_base = 2;
+
+a = -1*scale_base;
+b = 2*scale_base;
+c = -1*scale_base;
+
+sigma_x = r*sigma_w;
+sigma_y = sigma_w;
+
+ya = sigma_w;
+yc = -sigma_w;
+yb = 0;
+
+doog2 = a*mkg(0,ya,sigma_x,sigma_y,size_w) +...
+        b*mkg(0,yb,sigma_x,sigma_y,size_w) +...
+        c*mkg(0,yc,sigma_x,sigma_y,size_w);
+
+figure(3);colormap(hsv);
+subplot(1,3,1);mesh(a*mkg(0,ya,sigma_x,sigma_y,size_w)); 
+subplot(1,3,2);mesh(b*mkg(0,yb,sigma_x,sigma_y,size_w));
+subplot(1,3,3);mesh(c*mkg(0,yc,sigma_x,sigma_y,size_w));
+%doog2 = 255*5.1745*doog2;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn.m
new file mode 100755
index 0000000..15f947b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn.m
@@ -0,0 +1,10 @@
+function sgn = makesgn(sigma,sigma_x,sze)
+
+
+size_wh = round(0.5*sze);
+[x,y] = meshgrid([-size_wh:1:size_wh],[-size_wh:1:size_wh]);
+steps = 1/(1+2*sigma_x);
+
+fx = -1*(x<=-sigma_x) + (x>=sigma_x) + steps*((x+sigma_x).*(x>-sigma_x).*(x<sigma_x));
+
+sgn = fx.*(exp(- (y.*y)/(2*sigma)));
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn2.m
new file mode 100755
index 0000000..857360b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mksgn2.m
@@ -0,0 +1,9 @@
+function sgn = makesgn(sigma,sigma_x,sze)
+
+
+size_wh = round(0.5*sze);
+[x,y] = meshgrid([-size_wh(2):1:size_wh(2)],[-size_wh(1):1:size_wh(1)]);
+steps = 1/(1+sigma_x);
+
+fx = steps*((x+sigma_x).*(x>-sigma_x).*(x<1)) + steps*(sigma_x-x).*(x<sigma_x).*(x>=1);
+sgn = fx.*(exp(- (y.*y)/(2*sigma)));
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm.m
new file mode 100755
index 0000000..cf68a01
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm.m
@@ -0,0 +1,11 @@
+function I = read_pfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2)
+I = fscanf(fid,'%f',[A(1),A(2)]);
+
+
+I = I';
+size(I);
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm2.m
new file mode 100755
index 0000000..c01b25b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mreadpfm2.m
@@ -0,0 +1,9 @@
+function I = read_pfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2)
+I = fscanf(fid,'%f',[A(1),A(2)]);
+
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis.m
new file mode 100755
index 0000000..95de8f6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis.m
@@ -0,0 +1,16 @@
+function [l1,l2,phi] = mwis(gx,gy)
+%
+%  [l1,l2,phi] = mwis(gx,gy)
+%
+%
+
+sgx = sum(sum(gx.*gx));
+sgxy = sum(sum(gx.*gy));
+sgy = sum(sum(gy.*gy));
+
+tr = sgx + sgy;
+v1 = 0.5*sqrt(tr*tr - 4*(sgx*sgy-sgxy*sgxy));
+l1 = real(0.5*tr+v1);
+l2 = real(0.5*tr-v1);
+
+phi= 0.5*atan2(2*sgxy,sgx-sgy);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis_image.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis_image.m
new file mode 100755
index 0000000..c8e8b66
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/mwis_image.m
@@ -0,0 +1,25 @@
+function [l1,l2,phi] = mwis_image(gx,gy,hs)
+%
+%  [l1,l2,phi] = mwis_image(gx,gy)
+%
+%
+
+if (~exist('hs')),
+  hs = 10;
+end
+
+
+sgx = gx.*gx;
+sgxy = gx.*gy;
+sgy = gy.*gy;
+
+ssgx = smooth(sgx,hs);
+ssgxy = smooth(sgxy,hs);
+ssgy = smooth(sgy,hs);
+
+tr = ssgx + ssgy;
+v1 = 0.5*sqrt(tr.*tr - 4*(ssgx.*ssgy-ssgxy.*ssgxy));
+l1 = real(0.5*tr+v1);
+l2 = real(0.5*tr-v1);
+
+phi= 0.5*atan2(2*sgxy,sgx-sgy);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/myinterp.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/myinterp.m
new file mode 100755
index 0000000..6d8f055
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/myinterp.m
@@ -0,0 +1,18 @@
+function y = myinterp(d,rate)
+%
+
+
+if (size(d,1)>1),
+  d = [d;d(1)];
+else 
+  d = [d,d(1)];
+end
+
+lgn = length(d);
+
+x = 1:lgn;
+xx = linspace(1,lgn,rate*lgn);
+
+y = interp1(x,d,xx,'linear');
+
+y = y(1:(length(y)-1));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ncut_b.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ncut_b.m
new file mode 100755
index 0000000..c9eee35
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/ncut_b.m
@@ -0,0 +1,25 @@
+function [v,d] = ncut(A,nv)
+
+ds = sum(A);
+ds = ones(size(ds))./sqrt(ds);
+
+for j=1:size(A,1),
+  A(j,:) = A(j,:).*ds;
+end
+
+for j=1:size(A,2);
+  A(:,j) = A(:,j).*ds';
+end
+
+%D1 = ds'*ones(1,length(ds));
+%A = D1'.*A.*D1;
+
+disp(sprintf('computing eig values'));
+tic;[v,d] = eigs(A,nv);toc;
+
+d = abs(diag(d));
+
+for j=1:nv,
+  v(:,j) = v(:,j).*ds';
+end
+%v = D1(:,1:size(v,2)).*v;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/new_compute_J.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/new_compute_J.m
new file mode 100755
index 0000000..f4e376a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/new_compute_J.m
@@ -0,0 +1,32 @@
+function [JJ,mask] = compute_J(A,D,I,center,window_size_h)
+%%  function J = compute_J(A,D,I,center,window_size_h)
+%
+
+[size_y,size_x] = size(I);
+
+center_x = center(1);
+center_y = center(2);
+
+XX = meshgrid(1:size_x,1:size_y);
+YY = meshgrid(1:size_y,1:size_x)';
+x = reshape(XX,size_x*size_y,1);
+y = reshape(YY,size_x*size_y,1);
+index(:,1) = x-center_x;
+index(:,2) = y-center_y;
+
+position_new = A*index'+ [D(1),0;0,D(2)]*ones(2,size_x*size_y);
+position_new(1,:) = position_new(1,:)+center_x;
+position_new(2,:) = position_new(2,:)+center_y;
+
+position_new_x = reshape(position_new(1,:),size_y,size_x);
+position_new_y = reshape(position_new(2,:),size_y,size_x);
+
+[J,mask]= m_interp4(I,position_new_x,position_new_y);
+
+JJ = J(center(2)-window_size_h(2):center(2)+window_size_h(2),...
+       center(1)-window_size_h(1):center(1)+window_size_h(1));
+mask = mask(center(2)-window_size_h(2):center(2)+window_size_h(2),...
+       center(1)-window_size_h(1):center(1)+window_size_h(1));
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist.m
new file mode 100755
index 0000000..f8dbd32
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist.m
@@ -0,0 +1,45 @@
+function A = apply_image(gx,gy,I,wc)  
+%
+%  aout = apply_image(gx,gy,wc)
+%
+%
+
+[nr,nc] =size(gx);
+
+w = 2*wc+1;
+
+s1 = floor(nr/w);
+s2 = floor(nc/w);
+
+A = zeros(s1*s2,s1*s2);
+
+yid = 0;
+for j=wc+1:w:nr-wc-1,
+        yid = yid+1;
+        xid = 0;
+        for k=wc+1:w:nc-wc-1,
+                xid = xid + 1;
+                c1 = [k,j];
+
+                yyid = 0;
+                fprintf('.');
+                for jj=wc+1:w:nr-wc-1,
+                        yyid = yyid+1;
+                        xxid = 0;
+                        for kk=wc+1:w:nc-wc-1,
+                                xxid = xxid + 1;
+
+                                c2 = [kk,jj];
+
+                                a = compute_diff_patch(get_win(gx,c1,[wc,wc]),...
+                                get_win(gy,c1,[wc,wc]),...
+                                get_win(gx,c2,[wc,wc]),...
+                                get_win(gy,c2,[wc,wc]),...
+                                get_win(I,c1,[wc,wc]),...
+                                get_win(I,c2,[wc,wc]));
+
+                                A(yid+(xid-1)*s1,yyid+(xxid-1)*s1) = a;
+                        end
+                end
+        end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist2.m
new file mode 100755
index 0000000..dc31469
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist2.m
@@ -0,0 +1,46 @@
+function A = apply_image(gx,gy,I,wc)  
+%
+%  aout = apply_image(gx,gy,wc)
+%
+%
+
+[nr,nc] =size(gx);
+
+w = 2*wc+1;
+
+lws = 4;
+
+s1 = floor(nr/w);
+s2 = floor(nc/w);
+
+A = zeros(s1*s2,s1*s2);
+
+for j=1:s1,
+        for k=1:s2,
+            c1 = [(wc+1)+(k-1)*w,(wc+1)+(j-1)*w];
+            fprintf('.');
+
+            for jj=j-lws:j+lws,
+                for kk=k-lws:k+lws,
+
+                        c2 = [(wc+1)+(kk-1)*w,(wc+1)+(jj-1)*w];
+                        if ( (jj>0) & (kk>0) & (jj <= s1) & (kk <= s2)) 
+                        a = compute_diff_patch(get_win(gx,c1,[wc,wc]),...
+                                get_win(gy,c1,[wc,wc]),...
+                                get_win(gx,c2,[wc,wc]),...
+                                get_win(gy,c2,[wc,wc]),...
+                                get_win(I,c1,[wc,wc]),...
+                                get_win(I,c2,[wc,wc]));
+
+                                dx = abs(k-kk);dy = abs(j-jj);
+                                sp_diff = exp(-sqrt(dx.^2+dy.^2)/4);
+                                A(j+(k-1)*s1,jj+(kk-1)*s1) = a*sp_diff;
+                        
+                        end
+                        
+                end
+             end
+      end
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text.m
new file mode 100755
index 0000000..5cdb201
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text.m
@@ -0,0 +1,70 @@
+function A = apply_image(I,bars,wc)  
+%
+%  aout = apply_image(gx,gy,wc)
+%
+%
+
+[nr,nc] =size(I);
+
+w = 2*wc+1;
+
+lws = 4;
+
+gap = 10;
+
+s1 = floor((nr-w)/gap);
+s2 = floor((nc-w)/gap);
+
+A = zeros(s1*s2,s1*s2);
+
+sigma.text = 0.20;
+sigma.mag = 0.20;
+sigma.inten = 0.15;
+
+for j=1:s1,
+        for k=1:s2,
+
+            
+            c1 = [(wc+1)+(k-1)*gap,(wc+1)+(j-1)*gap];
+            fprintf('.');
+            for jj=j-lws:j+lws,
+                for kk=k-lws:k+lws,
+
+                        c2 = [(wc+1)+(kk-1)*gap,(wc+1)+(jj-1)*gap];
+                        if ( (jj>0) & (kk>0) & (jj <= s1) & (kk <= s2)),
+                                
+                                J1 = get_win(I,c1,[wc,wc]);
+                                J2 = get_win(I,c2,[wc,wc]);
+
+                                Jbars1 = get_win5(bars,c1,[wc,wc]);
+                                Jbars2 = get_win5(bars,c2,[wc,wc]);
+                                
+
+                                hists1 = get_hist(J1,Jbars1);
+                                hists2 = get_hist(J2,Jbars2);
+                                
+                                cumhists1 = get_cumhist(hists1);
+                                cumhists2 = get_cumhist(hists2);
+
+                                
+                                diff.inten = max(abs(cumhists1.inten-cumhists2.inten));
+                                diff.mag   = max(abs(cumhists1.mag-cumhists2.mag));
+                                diff.text  = max(max(abs(cumhists1.text-cumhists2.text)));
+
+                                diffs = max([diff.inten/sigma.inten,...
+                                             diff.mag/sigma.mag,...
+                                             diff.text/sigma.text]);
+
+                                dx = abs(k-kk);dy = abs(j-jj);
+                                sp_diff = sqrt(dx.^2+dy.^2)/4;
+                                
+                                A(j+(k-1)*s1,jj+(kk-1)*s1) = exp(-diffs-sp_diff);
+                        
+                        end
+                        
+                end
+             end
+      end
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text2.m
new file mode 100755
index 0000000..5c6e0d4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text2.m
@@ -0,0 +1,58 @@
+function A = apply_image(I,bars,wc)  
+%
+%  aout = apply_image(gx,gy,wc)
+%
+%
+
+[nr,nc] =size(I);
+
+w = 2*wc+1;
+
+lws = 4;
+
+gap = 10;
+
+mag = sum(bars,3);
+
+s1 = floor((nr-w)/gap)
+s2 = floor((nc-w)/gap)
+
+A = zeros(s1*s2,s1*s2);
+
+sigma.text = 0.20;
+sigma.mag = 0.20;
+sigma.inten = 0.15;
+
+for j=1:s1,
+        for k=1:s2,
+
+            
+            c1 = [(wc+1)+(k-1)*gap,(wc+1)+(j-1)*gap];
+            fprintf('.');
+            for jj=j-lws:j+lws,
+                for kk=k-lws:k+lws,
+
+                        c2 = [(wc+1)+(kk-1)*gap,(wc+1)+(jj-1)*gap];
+                        if ( (jj>0) & (kk>0) & (jj <= s1) & (kk <= s2)),
+
+                                ces = [c1,c2]-wc;
+                                cum = compute_dist_hist(I,mag,bars,[ces,w]);
+
+                                diffs = max([cum(1)/sigma.inten,...
+                                             cum(2)/sigma.mag,...
+                                             cum(3)/sigma.text]);
+
+                                dx = abs(k-kk);dy = abs(j-jj);
+                                sp_diff = sqrt(dx.^2+dy.^2)/4;
+                                
+                                A(j+(k-1)*s1,jj+(kk-1)*s1) = exp(-diffs-sp_diff);
+                        
+                        end
+                        
+                end
+             end
+             
+      end
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text3.m
new file mode 100755
index 0000000..a7ff408
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text3.m
@@ -0,0 +1,84 @@
+function [A,D] = pair_dist_text3(Cum,cumhists)  
+%
+%  A = pair_dist_text3(Cum,cumhists);
+%
+%
+
+s1 = Cum(2);
+s2 = Cum(1);
+
+st = Cum(3) + Cum(4) + 1;
+ed = size(cumhists,1);
+
+cumhists = cumhists(st:ed,:);
+
+np = size(cumhists,2);
+
+sigma.text = 0.20;
+sigma.mag = 0.20;
+sigma.inten = 0.15;
+
+lws = 4;
+
+
+k = sqrt(2)/2;
+M = 8*6;
+N = k*sqrt(M);
+
+r1 = 0.001;
+r2 = 0.001;
+
+c = N/(1+ (sqrt(1-0.5*(r1*r1+r2*r2)))*(0.25-0.75/N));
+
+D = zeros(1,s1*s2);
+
+nn = 1;
+for j =1:s1,
+   for k=1:s2,
+
+            id = j*s2+k;
+
+            cum_filter1 = reshape(cumhists(:,id),8,6)';
+            [na1,nb1,nc1,nd1] = ks_2d(cum_filter1);
+
+                                
+            fprintf('.');
+            for jj=j-lws:j+lws,
+                for kk=k-lws:k+lws,
+
+                        if ( (jj>0) & (kk>0) & (jj <= s1) & (kk <= s2)),
+
+                                idn = jj*s2+k;
+
+                                cum_filter2 = reshape(cumhists(:,idn),8,6)';
+                                [na2,nb2,nc2,nd2] = ks_2d(cum_filter2);
+
+                                
+                                diffa = abs(na2-na1);diffb =abs(nb2-nb1);
+                                diffc = abs(nc2-nc1);diffd = abs(nd2-nd1);
+                                maxs(1) = max(max(diffa));maxs(2) = max(max(diffb));
+                                maxs(3) = max(max(diffc));maxs(4) = max(max(diffd));
+
+
+                                maxs = maxs/6;
+
+                                d = min(1,signif(c*max(maxs)));
+
+                                ids(nn) = id;
+                                idns(nn) = idn;
+                                B(nn) = d;
+
+                                D(id) = D(id) + d;
+                                D(idn) = D(idn) + d;
+
+                                nn = nn+1;
+                                                        
+                        end
+                        
+                end
+             end
+             
+      end
+end
+
+A = sparse(ids,idns,b);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text4.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text4.m
new file mode 100755
index 0000000..83f3d49
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pair_dist_text4.m
@@ -0,0 +1,81 @@
+function [A] = pair_dist_text4(Iw,Cresult,cso)  
+%
+%  A = pair_dist_text3(Cum,cumhists);
+%
+%
+
+%s1 = Cum(2);s2 = Cum(1);
+
+figure(1);hold on;plot(cso(1),cso(2),'bo');
+
+ws = [10,10];
+
+lws = 3;
+gap = 7;
+
+J1 = get_win(Iw,round(cso),ws);
+Jbar1 = abs(get_win5(Cresult,round(cso),ws));
+hists1 = get_hist(J1,Jbar1);
+cumhists1 = get_cumhist(hists1);
+[na1,nb1,nc1,nd1] = ks_2d(cumhists1.text');
+
+figure(4);colormap(gray);
+imagesc(reshape(abs(Jbar1),size(Jbar1,1),size(Jbar1,2)*size(Jbar1,3)));
+colorbar;axis('image');drawnow;
+
+figure(2);
+subplot(2,5,1);imagesc(J1);%colormap(gray)
+subplot(2,5,2);imagesc(hists1.text');title('hist_1');colorbar;
+subplot(2,5,3);imagesc(cumhists1.text');title('cumhist_1');colorbar;
+subplot(2,5,4);imagesc(nc1);title('nc_1');colorbar
+subplot(2,5,5);imagesc(nb1);title('nb_1');colorbar
+drawnow;
+
+k = sqrt(2)/2;M = prod(size(cumhists1.text));
+N = k*sqrt(M);
+r1 = 0.001;r2 = 0.001;
+con = N/(1+ (sqrt(1-0.5*(r1*r1+r2*r2)))*(0.25-0.75/N));
+
+jid = 1;
+for jj=cso(2)-lws*gap:gap:cso(2)+lws*gap,
+   kid = 1;
+   for kk=cso(1)-lws*gap:gap:cso(1)+lws*gap,
+
+        figure(1);hold on; plot(kk,jj,'r*');drawnow;
+
+        J2 = get_win(Iw,round([kk,jj]),ws);
+        Jbar2 = get_win5(Cresult,round([kk,jj]),ws);
+
+        hists2 = get_hist(J2,Jbar2);
+        cumhists2 = get_cumhist(hists2);
+        [na2,nb2,nc2,nd2] = ks_2d(cumhists2.text');
+
+        figure(2);      
+        subplot(2,5,6);imagesc(J2);%colormap(gray)
+        subplot(2,5,7);imagesc(hists2.text');title('hist_2');colorbar
+        subplot(2,5,8);imagesc(cumhists2.text');title('cumhist_2');colorbar
+        
+        diffa = abs(na2-na1);diffb =abs(nb2-nb1);
+        diffc = abs(nc2-nc1);diffd = abs(nd2-nd1);
+
+        maxs(1) = max(max(diffa));maxs(2) = max(max(diffb));
+        maxs(3) = max(max(diffc));maxs(4) = max(max(diffd));
+
+        maxs = maxs/6;for j=1:4, sig(j) = signif(con*maxs(j)); end
+
+        subplot(2,5,9);imagesc(diffc);title('diff_{nc}');colorbar
+        subplot(2,5,10);imagesc(diffb);title('diff_{nb} ');colorbar
+
+        disp(sprintf('max diff is %f\n',min(sig)));
+
+        A(jid,kid) = min(1,min(sig));
+
+        %disp('press return to continue');
+        pause(3);
+
+        kid = kid+1;
+   end
+  jid = jid+1;
+end
+
+figure(1);hold off;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/patch_cat.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/patch_cat.m
new file mode 100755
index 0000000..0bf22c3
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/patch_cat.m
@@ -0,0 +1,9 @@
+function T = patch_cat(dotfilter,text_des)
+
+T(:,:,2:7,:) = text_des;
+clear text_des;
+
+for k=1:size(T,4),
+  T(:,:,1,k) = dotfilter(:,:,1,k);
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pgmread.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pgmread.m
new file mode 100755
index 0000000..620408a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/pgmread.m
@@ -0,0 +1,15 @@
+function img = pgmread(filename,size)
+% 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');
+
+for j=1:4,
+ a = fgetl(fid);
+end
+
+img = fscanf(fid,'%d',size);
+img = img';
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poisson.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poisson.m
new file mode 100755
index 0000000..99d49b6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poisson.m
@@ -0,0 +1,44 @@
+function [x] = Poisson(lambda);
+%Poisson        generates a random variable with a Poisson
+%               distribution
+%               Pr(x = n) = lambda^n exp(-lambda)/n
+%
+%               [x] = Poisson(lambda);
+%               lambda: the parameter for the Poisson distribution
+%                       (default is 1)
+%               x     : the output number
+%
+
+%%
+%%              (C) Thomas K. Leung
+%%              University of California at Berkeley
+%%              Apr 25, 1995.
+%%
+
+%%% Notice that in this implementation, we are comparing log(Z) with
+%%% T.  In fact, we can compare T = exp(-lambda) with Z, which will
+%%% save us from computing a large number of log's.  However, when
+%%% lambda is bigger than 709, exp(-lambda) = 0, which causes problem. 
+
+if nargin == 0
+        lambda = 1;
+end
+
+if lambda < 0
+        lambda = 1;
+end
+
+P = 0;
+N = 0;
+T = -lambda;
+
+rand('seed',sum(100*clock));
+
+while P >= T
+        Z = rand(1);
+        P = P + log(Z);
+        N = N + 1;
+end
+
+x = N;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poissonfield.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poissonfield.m
new file mode 100755
index 0000000..8051867
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/poissonfield.m
@@ -0,0 +1,53 @@
+function [x,y,success] = PoissonField(int,sx,sy,ir);
+%BF_HardCore    Generates a hard core Poisson field
+%               [x,y] = Poisson_HC(int,ir,sx,sy);
+%               int: intensity of the field.  (default is 1)
+%               ir : inhibition radius (default is 1)
+%               sx : size in x (default 100)
+%               sy : size in y (default 100)
+%               x  : x coordinates
+%               y  : y coordinates
+%
+
+%%
+%%              (C) Thomas K. Leung
+%%              University of California at Berkeley
+%%              April 26, 1995.
+%%              leungt@cajal.cs.berkeley.edu
+%%
+
+%%
+%% Generate each point in sequence and reject it if it's too close to
+%% previous ones. 
+%%
+
+if nargin <= 0
+        int = 1; 
+        sx  = 100;
+        sy  = 100;
+        ir  = 0;
+elseif nargin <= 1
+        sx  = 100;
+        sy  = 100;
+        ir  = 0;
+elseif nargin <= 2
+        sy = 100;
+        ir = 0;
+elseif nargin <= 3
+        ir = 0;
+end
+
+%% 
+%% Notice that the average number of a poisson process is the
+%% parameter lambda.  This is consistent with our definition of the
+%% intensity here.  Intensity is the mean number of points inside each
+%% unit area.  Therefore, in a window of sx by sy, we should get
+%% int*sx*sy number of points on average which is the mean number of
+%% arrivals in a Poisson Process
+%%
+
+[n] = poisson(int * sx * sy);
+
+[x,y,success] = binomialfield(n,sx,sy,ir);
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back.m
new file mode 100755
index 0000000..069fe61
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back.m
@@ -0,0 +1,24 @@
+function mask = proj_back(I,hw,mask_s)
+%
+%   mask = proj_back(I,hw,mask_s)
+%
+%
+
+
+[nr,nc] = size(I2);
+
+hw = 3;
+st_sz = 2*hw + 1;
+
+nr_chank = floor(nr/st_sz);
+nc_chank = floor(nc/st_sz);
+
+[x,y] = meshgrid(1:nc,1:nr);
+
+ct_chank_x = round((x-hw-1)/st_sz) + 1;
+ct_chank_y = round((y-hw-1)/st_sz) + 1;
+
+idx = (ct_chank_x - 1)*nr_chank + ct_chank_y;
+
+mask = full(sparse(y,x,mask_s(idx(:))));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back_id.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back_id.m
new file mode 100755
index 0000000..9db322e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/proj_back_id.m
@@ -0,0 +1,19 @@
+function [vbig,bnr,bnc] = proj_back_id(v,gcs,gce,grs,gre)
+%
+%   vbig = proj_back_id(v,gcs,gce,grs,gre)
+%
+
+nr = max(gre)+1;
+nc = max(gce)+1;
+
+sw = 3;
+gap = 2*sw+1;
+
+bnc = nc*gap;
+bnr = nr*gap;
+
+[x,y] = meshgrid(1:bnc,1:bnr);
+
+idx = grs(y(:))+1+gcs(x(:))*nr;
+
+vbig = full(sparse(y(:),x(:),v(idx)));
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/quant.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/quant.m
new file mode 100755
index 0000000..c2dfae6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/quant.m
@@ -0,0 +1,20 @@
+function [x,map] = quant(d1,d2,d3,nbin,ws)
+
+if (~exist('ws')),
+  ws = [1,1,1];
+end
+
+d1 = d1-min(d1);
+d2 = d2-min(d2);
+d3 = d3-min(d3);
+
+d1 = d1/max(d1);
+d2 = d2/max(d2);
+d3 = d3/max(d3);
+
+d1 = d1*ws(1);
+d2 = d2*ws(2);
+d3 = d3*ws(3);
+
+
+[x,map] = vmquant(d1,d2,d3,nbin);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpdm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpdm.m
new file mode 100755
index 0000000..9a1068e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpdm.m
@@ -0,0 +1,8 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%d',[A(1),A(2)]);
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm.m
new file mode 100755
index 0000000..48ecd78
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm.m
@@ -0,0 +1,10 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%f',[A(1),A(2)]);
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_id.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_id.m
new file mode 100755
index 0000000..5f5c4f7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_id.m
@@ -0,0 +1,21 @@
+function Is = readpfm(filename,ids,nodes)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+
+Is = zeros(length(ids),nodes);
+
+ix = 1;
+for j=1:max(ids),
+ I = fscanf(fid,'%f',nodes);
+ if (find(ids==j)),
+   Is(ix,:) = I(:)';
+   ix = ix+1;
+   fprintf('.');
+  end
+end
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_idf.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_idf.m
new file mode 100755
index 0000000..d7916e7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpfm_idf.m
@@ -0,0 +1,29 @@
+function Is = readpfm(filename,ids,nodes)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+
+Is = zeros(length(ids),nodes);
+
+idt = sort(ids);
+
+idh = 1;
+
+ix = 1;
+for j=1:length(ids),
+
+ gap = idt(j) - idh;
+ fprintf('%d',gap);
+
+ I = fscanf(fid,'%f',nodes*gap);
+ I = fscanf(fid,'%f',nodes);
+
+ Is(find(ids==idt(j)),:) = I(:)';
+ fprintf('.');
+
+ idh = idt(j)+1;
+end
+
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpgm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpgm.m
new file mode 100755
index 0000000..a5fd7f2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpgm.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/filter_hist/readpnm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpnm.m
new file mode 100755
index 0000000..ab78c2c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readpnm.m
@@ -0,0 +1,21 @@
+function I = readpnm(name)
+
+  fid = fopen(name, 'r');
+  fscanf(fid, 'P5\n');
+  cmt = '#';
+  while findstr(cmt, '#') == 1
+    cmt = fgets(fid);
+    if findstr(cmt, '#') ~= 1
+      YX = sscanf(cmt, '%d %d %d');
+      y = YX(1); x = YX(2); nb = YX(3);
+    end
+  end
+  fgets(fid); 
+  packed = fscanf(fid,'%f',[nb*y*x]);
+
+  for j = 1:nb,
+        I(:,:,j) = reshape(packed(j:nb:nb*y*x),y,x)';
+  end
+
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readppm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readppm.m
new file mode 100755
index 0000000..300f597
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/readppm.m
@@ -0,0 +1,19 @@
+function [r, g, b] = readppm(name)
+
+  fid = fopen(name, 'r');
+  fscanf(fid, 'P6\n');
+  cmt = '#';
+  while findstr(cmt, '#') == 1
+    cmt = fgets(fid);
+    if findstr(cmt, '#') ~= 1
+      YX = sscanf(cmt, '%d %d');
+      y = YX(1); x = YX(2);
+    end
+  end
+  fgets(fid); 
+  packed = fread(fid,[3*y,x],'uint8')';
+  r = packed(:,1:3:3*y);
+  g = packed(:,2:3:3*y);
+  b = packed(:,3:3:3*y);
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/record.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/record.m
new file mode 100755
index 0000000..9626038
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/record.m
@@ -0,0 +1,6 @@
+load patch1;
+doog2 = mkdoog2(2,10,0,80);
+dog2 = rotate_J(90,doog2);
+
+H = mkf_test(dog2,size(patch1),-1,0.00001,2,-1);
+o = BfilterS(patch1,H,size(dog));figure(4);colormap(gray);imagesc(o.*(o>0));
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/recursive_cut_tc.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/recursive_cut_tc.m
new file mode 100755
index 0000000..a8c9362
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/recursive_cut_tc.m
@@ -0,0 +1,140 @@
+%function [groups,ids] = recursive_cut(ncutv,fn_base)
+%
+%
+%  function [groups,ids] = recursive_cut(ncutv,threshold,spthresh)
+%
+%
+
+ncutv= ncutv_o(:,1:4);
+
+fn_base = fn;
+%fn_base = '130040';
+
+nvv = size(ncutv_o,2);
+nbin = 24;
+
+ids = [];
+groups = [];
+labels = [];
+
+load cmaps
+cmap = cmapg;
+
+j = 1;
+done = 0;
+np = size(ncutv,1);
+nv = size(ncutv,2);
+
+%%%%%% find the cut for the first ncut vector
+ev_id = 2;
+para = [nvv ev_id nr nc 100];
+Gmask = ones(nr,nc);
+%threshold = find_cutpoint(ncutv(:,ev_id),cmapg,nbin);threshold = threshold(1:end-1);
+threshold = linspace(min(ncutv(:,ev_id)),max(ncutv(:,ev_id)),nbin);
+[cut_threshold,max_asso] = find_bst_cut(fn_base,para,threshold,Gmask);
+disp(max_asso);
+
+id1 = find(ncutv(:,ev_id)<=cut_threshold);
+id2 = find(ncutv(:,ev_id)>cut_threshold);
+
+groups = [groups,id1(:)'];
+ids = [ids,length(id1)];
+
+groups = [groups,id2(:)'];
+ids = [ids,length(groups)];
+
+
+for j=3:nv,
+        fprintf('j = %d\n',j);
+        % expand the current level,
+        new_groups = [];
+        new_ids = [];
+
+        
+        figure(4);ims(ncutv(:,j),nr,nc);title(num2str(j));
+
+        figure(1);clf
+        disp_groups(groups,ids,nr,nc);
+        drawnow;
+
+        %figure(3);
+        % for each leaves,
+        mx = max(ncutv(:,j))-min(ncutv(:,j));
+        %mx = std(ncutv(:,j));
+
+        base_id =1;
+        for k=1:length(ids),
+                old_groups = groups(base_id:ids(k));
+
+                v = ncutv(old_groups,j);
+                change_v = max(v)-min(v);
+                %change_v = std(v);
+                n1 = sum(v>(min(v)+0.85*change_v));%n1 = n1/length(old_groups);
+                n2 = sum(v<=(min(v)+0.15*change_v));%n2 = n2/length(old_groups);
+                disp(sprintf('n1 = %f, n2 = %f',n1,n2));
+
+             figure(2);
+             Gmask = zeros(np,1);
+             Gmask(old_groups) = Gmask(old_groups)+1;
+             drawnow;
+             ims(ncutv(:,j).*Gmask,nr,nc);
+
+             disp(sprintf('!!!!!!!!!!!!!RATIO= %f',change_v/mx))
+
+             %pause;
+
+             if (((change_v/mx)>0.5) & (n1>10) &(n2>10)),
+                
+                ev_id = j;
+
+                %threshold = find_cutpoint(ncutv(old_groups,ev_id),cmapg,nbin);threshold = threshold(1:end-1);
+                threshold = linspace(min(ncutv(:,ev_id)),max(ncutv(:,ev_id)),nbin);
+                para = [nvv ev_id nr nc 100];
+                [cut_threshold,max_asso] = find_bst_cut(fn_base,para,threshold,Gmask);
+
+                disp(max_asso);
+
+                if (max_asso>1.2),
+                  id1 = find(ncutv(old_groups,ev_id)<=cut_threshold);
+                  id2 = find(ncutv(old_groups,ev_id)>cut_threshold);
+        
+                  figure(5);
+                  subplot(1,2,1);maskt= zeros(np,1);maskt(old_groups(id1))=1+maskt(old_groups(id1));ims(maskt,nr,nc);
+                  subplot(1,2,2);maskt= zeros(np,1);maskt(old_groups(id2))=1+maskt(old_groups(id2));ims(maskt,nr,nc);
+        
+                  new_groups = [new_groups,old_groups(id1)];
+                  new_ids = [new_ids,length(new_groups)];
+
+          
+                  new_groups = [new_groups,old_groups(id2)];
+                  new_ids = [new_ids,length(new_groups)];
+                else
+                  fprintf(' keep ');
+                  new_groups = [new_groups,old_groups];
+                  new_ids = [new_ids,length(new_groups)];
+                end
+              
+              else
+                  fprintf(' keep ');
+                  new_groups = [new_groups,old_groups];
+                  new_ids = [new_ids,length(new_groups)];
+              end
+              fprintf('\n');
+              base_id = ids(k) + 1;
+        end
+
+        
+        
+        groups = new_groups;
+        ids = new_ids;
+
+        figure(1);disp_groups(groups,ids,nr,nc);
+
+        fprintf('press return\n');
+        pause;
+        j= j+1;
+end
+
+fprintf('total group = %d \n',length(ids));
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/reduce_all.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/reduce_all.m
new file mode 100755
index 0000000..d7d31f8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/reduce_all.m
@@ -0,0 +1,8 @@
+function b = reduce_all(a)
+
+numband = size(a,3);
+
+for j=1:numband,
+
+    b(:,:,j) = reduce(a(:,:,j));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/rotate_J.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/rotate_J.m
new file mode 100755
index 0000000..12f29b6
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/rotate_J.m
@@ -0,0 +1,30 @@
+function J = compute_J(angle,I)
+%%  function J = compute_J(angle,I)
+%
+
+[size_y,size_x] = size(I);
+
+[center_x,center_y] = find_center(size_x,size_y);
+
+a = angle * pi/180;
+A = [cos(a),-sin(a);sin(a),cos(a)];
+
+[XX,YY] = meshgrid(1:size_x,1:size_y);
+
+x = reshape(XX,size_x*size_y,1);
+y = reshape(YY,size_x*size_y,1);
+index(:,1) = x-center_x;
+index(:,2) = y-center_y;
+
+position_new = A*index';
+position_new(1,:) = position_new(1,:)+center_x;
+position_new(2,:) = position_new(2,:)+center_y;
+
+position_new_x = reshape(position_new(1,:),size_y,size_x);
+position_new_y = reshape(position_new(2,:),size_y,size_x);
+
+J = m_interp4(I,position_new_x,position_new_y);
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/session.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/session.m
new file mode 100755
index 0000000..70fabbf
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/session.m
@@ -0,0 +1,4 @@
+t = rj(1:50,19:50); tt = interp4(t,1);
+sgn = mksgn2(182,2,[91,9]);
+H = mkf_test(sgn,size(tt),1,0.01,2,300);
+o = BfilterS(tt,H,size(sgn));figure(1);imagesc(o.*(o>0));axis('equal');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_cumhist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_cumhist.m
new file mode 100755
index 0000000..fe82e64
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_cumhist.m
@@ -0,0 +1,28 @@
+function show_hist(cumhists,bins,fig_id,hold_flag,ct)
+%%
+%    show_hist(cumhists,bins,fig_id,ct)
+%
+%
+
+        if (~exist('ct')),
+                ct = 'b-o';
+        end
+
+        figure(fig_id);
+
+        subplot(3,3,1);plot(bins.inten,cumhists.inten,ct);
+
+        if (hold_flag == 1), hold on;else hold off; end
+
+        for j=1:size(cumhists.text,2),
+           subplot(3,3,1+j);
+           plot(bins.text,cumhists.text(:,j),ct);
+           if (hold_flag == 1), hold on;else hold off; end
+        end
+
+        subplot(3,3,8);
+        plot(bins.mag,cumhists.mag,ct);
+        if (hold_flag == 1), hold on;else hold off; end
+
+        
+        
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_hist.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_hist.m
new file mode 100755
index 0000000..efaf899
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/show_hist.m
@@ -0,0 +1,23 @@
+function show_hist(hists,bins,fig_id)
+%%
+%    show_hist(hists,bins,fig_id)
+%
+%
+
+        figure(fig_id);
+
+        subplot(3,3,1);bar(bins.inten,hists.inten);
+
+        %maxval = max(max(max(abs(Jbar))));
+
+        for j=1:size(hists.text,2),
+           subplot(3,3,1+j);% hist(reshape(abs(Jbar(:,:,j)),prod(w),1),[1:10:maxval+1]);
+           bar(bins.text,hists.text(:,j));
+        end
+
+        subplot(3,3,8);%hist(reshape(sum(abs(Jbar),3),prod(w),1),[1:10:161]);
+        bar(bins.mag,hists.mag);
+
+
+        
+        
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm.m
new file mode 100755
index 0000000..1833062
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm.m
@@ -0,0 +1,45 @@
+function [T,A,M2,TAM]=showsmm(L1,L2,phi,maxM);
+% [T,A,M]=showsmm(L1,L2,phi,maxM);
+
+if (~exist('maxM')),
+% needs to know upper bound on M for given window function in smm
+maxM=0.18; % temporary
+end
+
+
+A=1-L2./(L1+eps);
+T=2*(phi+pi/2)/(2*pi);
+M=L1+L2;
+M2=min(M/maxM,1); % keep it from exceeding 1
+%M2 = sigmoid(M,maxM,30);
+
+matlab5on = 1;
+
+if matlab5on == 1,
+  TAM=hsv2rgb(T,A,M2);
+
+  figure(3);
+  image(TAM);
+  axis('tightequal');
+else
+  H = [reshape(T,prod(size(T)),1),...
+       reshape(A,prod(size(A)),1),...
+       reshape(M2,prod(size(M2)),1)];
+  M = hsv2rgb(H);
+  [Ic,map] =vmquant(M(:,1),M(:,2),M(:,3),256);
+
+  image(reshape(Ic,size(T,1),size(T,2)));colormap(map);
+end
+
+if 0
+   plot3(A(:).*M(:).*cos(2*pi*T(:)),A(:).*M(:).*sin(2*pi*T(:)),M(:),'.','markersize',15)
+   axis([-1 1 -1 1 0 1])
+   [x,y,z] = cylinder(ones(1,5));
+   x=x.*z;
+   y=y.*z;
+   hold on
+   h=mesh(x,y,z);
+   set(h,'edgecolor',[.2 .2 .2]);
+   hidden off
+   hold off
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm_v5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm_v5.m
new file mode 100755
index 0000000..937303d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/showsmm_v5.m
@@ -0,0 +1,34 @@
+function [T,A,M2,TAM]=showsmm(L1,L2,phi,maxM);
+% [T,A,M]=showsmm(L1,L2,phi,maxM);
+
+if (~exist('maxM')),
+% needs to know upper bound on M for given window function in smm
+maxM=0.18; % temporary
+end
+
+
+A=1-L2./(L1+eps);
+T=2*(phi+pi/2)/(2*pi);
+M=L1+L2;
+M2=min(M/maxM,1); % keep it from exceeding 1
+%M2 = sigmoid(M,maxM,30);
+
+  TAM=hsv2rgb(T,A,M2);
+
+  figure(3);
+  image(TAM);
+  axis('tightequal');
+
+
+if 0
+   plot3(A(:).*M(:).*cos(2*pi*T(:)),A(:).*M(:).*sin(2*pi*T(:)),M(:),'.','markersize',15)
+   axis([-1 1 -1 1 0 1])
+   [x,y,z] = cylinder(ones(1,5));
+   x=x.*z;
+   y=y.*z;
+   hold on
+   h=mesh(x,y,z);
+   set(h,'edgecolor',[.2 .2 .2]);
+   hidden off
+   hold off
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/sigmoid.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/sigmoid.m
new file mode 100755
index 0000000..996f7fe
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/sigmoid.m
@@ -0,0 +1,10 @@
+function a = sigmoid(x,offset,sig)
+%
+%   a = sigmoid(x,offset,sig)
+%      
+%       a = ones(size(x))./(1+exp(-(x-offset)/sig));
+%
+
+
+a = ones(size(x))./(1+exp(-(x-offset)/sig));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif.m
new file mode 100755
index 0000000..f49eefc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif.m
@@ -0,0 +1,22 @@
+function a = signif(b)
+
+js = [1:101];
+
+
+if 0,
+d = (-ones(size(js))).^(js-1);
+d1 = exp(-2*(js.*js)*b*b);
+
+a = 2*sum(d.*d1);
+
+end
+
+d1 = exp(-2*(js.*js)*b*b);
+d2 = 4*(js.*js)*b*b - 1;
+
+a = 2*sum(d1.*d2);
+
+if (b<0.03),a = 1;end
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif_N.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif_N.m
new file mode 100755
index 0000000..82dc914
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/signif_N.m
@@ -0,0 +1,10 @@
+function a = signif_N(b,N)
+%
+%
+%
+
+Ne = sqrt(N*0.5);
+
+cof = Ne + 0.155 + 0.24/Ne;
+
+a= signif(cof*b);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/smooth.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/smooth.m
new file mode 100755
index 0000000..919b53a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/smooth.m
@@ -0,0 +1,20 @@
+% smooth an image
+% coordinates (r, c) follow matrix convention;
+% the gaussian is truncated at x = +- tail, and there are samples samples
+% inbetween, where samples = hsamples * 2 + 1
+
+function g = smooth(image, hsamples)
+
+tail=4;
+samples = hsamples * 2 + 1;
+
+x = linspace(-tail, tail, samples);
+gauss = exp(-x.^2);
+n = gauss * ones(samples,1);
+gauss = gauss/n;
+
+
+g = conv2(conv2(image, gauss), gauss');
+
+g = conv_trim(g, hsamples, hsamples);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/startup.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/startup.m
new file mode 100755
index 0000000..8d38803
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/startup.m
@@ -0,0 +1,9 @@
+%path(path,'/usr/sww/matlab-4.2c/toolbox/images');
+home_dir = '/home/barad-dur/vision/malik/jshi/';
+path(path,[home_dir,'matlab/toolbox/io'])
+path(path,[home_dir,'matlab/pyramid']);
+path(path,[home_dir,'matlab/toolbox/filter'])
+path(path,[home_dir,'matlab/toolbox/disp'])
+path(path,[home_dir,'matlab/vision/vision94/tracking/'])
+path(path,[home_dir,'proj/grouping/laso']);
+path(path,[home_dir,'proj/grouping/eig']);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarp.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarp.m
new file mode 100755
index 0000000..60a4530
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarp.m
@@ -0,0 +1,9 @@
+function J = swarp(I)
+
+[nr,nc] = size(I);
+
+center_x = round(0.5*nc);
+center_y = round(0.5*nr);
+
+J = [I(center_y:nr,center_x:nc),I(center_y:nr,1:center_x-1);...
+     I(1:center_y-1,center_x:nc),I(1:center_y-1,1:center_x-1)]; 
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarpback.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarpback.m
new file mode 100755
index 0000000..513bc25
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/swarpback.m
@@ -0,0 +1,12 @@
+function J = swarpback(I);
+
+[nr,nc] = size(I);
+
+center_x = round(0.5*nc);
+center_y = round(0.5*nr);
+
+cx= center_x -1;
+cy= center_y -1;
+
+J = [I(nr-cy+1:nr,nc-cx+1:nc),I(nr-cy+1:nr,1:(nc-center_x+1));...
+     I(1:(nr-center_y+1),nc-cx+1:nc),I(1:(nr-center_y+1),1:(nc-center_x+1))];
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test.m
new file mode 100755
index 0000000..12470eb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test.m
@@ -0,0 +1,110 @@
+
+fn = '130065'; nr = 30;nc = 49;
+%nr = 49;nc = 30;
+%nc = 68;nr = 43;
+%nr =49;nc = 30;
+
+basedir = 'plaatje_data/newdata/';
+if 1,
+fname = sprintf('%s%s_eigvec.pfm',basedir,fn);
+eigv =  readpfm(fname);
+fname = sprintf('%s%s_eigval.pfm',basedir,fn);
+eigval = readpfm(fname);
+
+fname = sprintf('%s%s_ncutvec.pfm',basedir,fn);
+ncutv = readpfm(fname);
+fname = sprintf('%s%s_ncutval.pfm',basedir,fn);
+ncutval = readpfm(fname);
+else
+fname = sprintf('%sncutvec_%s.pfm',basedir,fn);
+ncutv = readpfm(fname);
+fname = sprintf('%sncutval_%s.pfm',basedir,fn);
+ncutval = readpfm(fname);
+end
+
+
+fname = sprintf('images/%s.pgm',fn);
+I = readpgm(fname);
+cutsz = 20; I = cutoff(I,cutsz);
+figure(3);im(I);colormap(gray);
+
+figure(6);
+for j=1:min(8,size(ncutv,2)-1),
+  subplot(3,3,j);
+  im(reshape(ncutv(:,j+1),nr,nc));colorbar
+  title(num2str(ncutval(j+1,1)));
+end
+%cm = sprintf('print -dps ncut_%s',fn);disp(cm);eval(cm);
+subplot(3,3,9);im(I);axis('off');
+
+ev = eigval(:,1);
+figure(5);hold off;clf;subplot(1,2,1);
+%semilogy((ev(1:end-1) - ev(2:end))./ev(1:end-1),'x-');grid on;
+plot((ev(1:end-1) - ev(2:end))./ev(1:end-1),'x-');grid on;
+%semilogy(0.01*ones(size(ev(2:end-1))),'r-');semilogy(0.005*ones(size(ev(2:end-1))),'r-');semilogy(0.0025*ones(size(ev(2:end-1))),'r-');grid on;hold off;
+subplot(1,2,2);
+%semilogy(ev(1:end-1)-ev(2:end),'p-');grid on;
+semilogy((ev(1:end-1) - ev(2:end))/ev(1),'x-');grid on;
+
+
+ncutv_o = ncutv;
+
+recursive_cut_tc;
+
+%[groups,ids] = recursive_cut(ncutv(:,1:4),fn);
+
+masks = make_masks(groups,ids,nr,nc);
+
+cm = sprintf('save masks_%s masks ncutv_o groups ids nr nc',fn);
+disp(cm);
+
+eval(cm);
+
+
+%%%%%%%%%%%%%%%%%%
+fn = '130040';
+cm = sprintf('load masks_%s',fn);
+disp(cm);
+eval(cm);
+
+fn = '130040';
+fname= sprintf('images/%s.pgm',fn);
+I = readpgm(fname);cutsz = 20; I = cutoff(I,cutsz);
+figure(3);im(I);colormap(gray);
+hw = 2; %nr = 43;nc=68;
+gap = 2*hw+1;
+%nr = 30;nc=49;
+Is = I(1:nr*gap,1:nc*gap);
+figure(3);im(Is);axis('off');
+
+%cm = sprintf('print -deps I_%s',fn);disp(cm);eval(cm);
+
+
+
+%masks = make_masks(groups,ids,nr,nc);
+figure(2);disp_groups(groups,ids,nr,nc);
+
+figure(1);
+Imasks = disp_Imask(Is,nr,nc,hw,masks);
+
+for j=1:length(ids),
+ figure(4);colormap(gray);clf
+ im(Imasks(:,:,j));axis('off');
+ cm = sprintf('print -deps result_cut_%s_%d',fn,j);
+ disp(cm);eval(cm);
+
+ %print -deps result_cut_134011_1
+end
+
+
+if 0,
+
+%load st_134013
+
+fn = '134013_t';
+
+I_max = 250;
+tex_max = 40;
+
+writeout_feature(I1,T1,fn,I_max,tex_max);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test1.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test1.m
new file mode 100755
index 0000000..691b63e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test1.m
@@ -0,0 +1,175 @@
+
+fnameI = '130068';
+
+cm = sprintf('load filter_%s.pgm.mat',fnameI);
+disp(cm);
+eval(cm);
+
+text_des = abs(text_des);
+
+
+  %%%% cutoff margins,
+margin = 6+10;
+
+Iw = cutoff(I,margin);
+
+T1= reshape(text_des,size(text_des,1),size(text_des,2),size(text_des,3)*size(text_des,4));
+T1 = cutoff(T1,margin);
+
+%%%%% reduce resolution
+
+
+
+T1 = reduce_all(T1);
+T1 = reduce_all(T1);
+
+im5(T1,5,6);
+
+cm = sprintf('writepnm5(''%s_f.pnm'',%s)',fnameI,'T1/70');
+
+%  disp(cm);eval(cm);
+
+nr = size(T1,1);
+nc = size(T1,2);
+
+%  D = mreadpfm('D_134011_f.pnm.pfm');
+
+%  figure(3);imagesc(reshape(D,nc,nr)');axis('image');colorbar
+
+if 0,
+figure(7);
+subplot(3,1,1);hist(reshape(I1,prod(size(I1)),1),binI);
+subplot(3,1,2);hist(reshape(I2,prod(size(I2)),1),binI);
+subplot(3,1,3);hist(reshape(I3,prod(size(I3)),1),binI);
+
+
+If1 = filter_output(I1,sigs,szs);
+If2 = filter_output(I2,sigs,szs);
+If3 = filter_output(I3,sigs,szs);
+
+I1a = cutoff(I1,5); If1 = cutoff(If1,5);
+I2a = cutoff(I2,5); If2 = cutoff(If2,5);
+I3a = cutoff(I3,5); If3 = cutoff(If3,5);
+
+
+
+figure(4);
+bint = [-0.15:0.02:0.15];
+id = 4;
+
+If = If1;
+for j=1:5,
+   subplot(5,2,2*(j-1)+1);
+   hist(reshape(If(:,:,id,j)./s1,prod(size(If(:,:,id,j))),1),bint);
+end
+
+If = If2;
+for j=1:5,
+   subplot(5,2,2*j);
+   hist(reshape(If(:,:,id,j)./s2,prod(size(If(:,:,id,j))),1),bint);
+end
+
+
+%%% make 2d histogram bin
+figure(5);
+idmax = 5;
+filt_id = 4;
+
+for id=1:idmax,
+
+  subplot(idmax,3,(id-1)*3+1);
+  h2d1 = hist_I_f(I1a,If1(:,:,filt_id,id),binI,bintex);
+  imagesc(h2d1);axis('image')
+  subplot(idmax,3,(id-1)*3+2);
+  h2d2 = hist_I_f(I2a,If2(:,:,filt_id,id),binI,bintex);
+  imagesc(h2d2);axis('image')
+
+  subplot(idmax,3,id*3);
+  imagesc(h2d2/sum(sum(h2d2)) + h2d1/sum(sum(h2d1)));axis('image')
+  colorbar
+end
+
+%%%%%%%%%%%%%%%%%%%%% three types %%%%%%%%
+figure(4);
+idmax = 5;
+filt_id = 2;
+
+width = 4;
+
+for id=1:idmax,
+
+  subplot(idmax,width,(id-1)*width+1);
+  h2d1 = hist_I_f(I1a,If1(:,:,filt_id,id),binI,bintex);
+  h2d1 = h2d1/sum(sum(h2d1));
+  imagesc(h2d1);axis('image');
+
+  subplot(idmax,width,(id-1)*width+2);
+  h2d2 = hist_I_f(I2a,If2(:,:,filt_id,id),binI,bintex);
+  h2d2 = h2d2/sum(sum(h2d2));
+  imagesc(h2d2);axis('image')
+
+  subplot(idmax,width,(id-1)*width+3);
+  h2d3 = hist_I_f(I3a,If3(:,:,filt_id,id),binI,bintex);
+  h2d3 = h2d3/sum(sum(h2d3));
+  imagesc(h2d3);axis('image')
+
+  subplot(idmax,width,id*width);
+  imagesc(h2d1+h2d2+h2d3);axis('image')
+  colorbar
+end
+
+
+%%%%%%%%%%%% smaller window  %%%%
+hw = round(4*sigs(1));
+
+figure(5);%imagesc(I1a);axis('image');
+cs = round(ginput(1));
+
+J = get_win(I1a,cs,[hw,hw]);figure(7);imagesc(J);axis('image');
+
+Jf = get_win5(If1,cs,[hw,hw]);
+scales = 1:5; nscales = length(scales);
+filters = 1:7; nfilters = length(filters);
+
+figure(8);
+for j=1:nscales,
+   for k=1:nfilters,
+       subplot(nscales,nfilters,(j-1)*nfilters+k);
+       h2d = hist_I_f(J,Jf(:,:,(j-1)*7+k));h2d = h2d/sum(sum(h2d));
+       imagesc(h2d);axis('image');colorbar;axis('off');
+   end
+end
+
+
+if 0,
+
+   figure(3);
+   cs = ginput(1);
+
+   ws = [15,15];
+   J = get_win(I,cs,ws);
+   figure(6);imagesc(J);axis('image');
+
+   t1 = get_win5(text_des,cs,ws);
+
+   t1p = abs(t1);
+   %t1p = abs(t1);
+   %t1p = t1.*(t1>0);
+
+   figure(2);im5(t1p,5,6);
+
+   t1p = reshape(t1p,size(t1p,1)*size(t1p,2),size(t1p,3))';
+
+   t1pm = mean(t1p')';
+   t1ps = t1p- t1pm*ones(1,size(t1p,2));
+
+   B = t1ps*t1ps';
+   [v,d] = eig(B);d = diag(d);
+   figure(4);plot(d,'x-');
+
+   figure(5);
+   subplot(2,2,1);vid = 30;plot(reshape(v(:,vid),6,5),'x-');
+
+end
+
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test2.m
new file mode 100755
index 0000000..c70446a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test2.m
@@ -0,0 +1,220 @@
+
+%fnameI = '130056';
+%fnameI = '134013';
+fnameI = '134007';
+
+%%%% flags %%%%%%%%%
+read_image = 1;
+
+margin = 10+6;
+sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];
+r =3;
+szs = round(3*r*sigs);
+
+
+
+%%% image read %%%
+if read_image,
+  cm = sprintf('I = readpgm(''images/%s.pgm'');',fnameI);
+  disp(cm);
+  eval(cm);
+
+  Iw = cutoff(I,margin);
+  figure(1);imagesc(Iw);axis('image');
+end
+
+%%%%  image crop %%%
+figure(1);J = imcrop;
+figure(2);imagesc(J);axis('image');drawnow;
+
+Jf = filter_output(J,sigs,szs);
+margin = 5;
+Ja = cutoff(J,margin);Jfa = cutoff(Jf,margin);
+figure(2);imagesc(Ja);axis('image');
+
+figure(3);
+imagesc(Jfa(:,:,1,3));axis('image');drawnow;
+
+Jfb = reshape(Jfa,size(Jfa,1),size(Jfa,2),size(Jfa,3)*size(Jfa,4));
+mag = sum(abs(Jfb),3);
+
+%%%%%% Joint hist. %%%%%%%%%
+
+filter_id = 1;
+scale = 1;
+h2d = hist_I_f(Ja,Jfa(:,:,filter_id,scale));
+
+figure(4);
+imagesc(h2d/sum(sum(h2d)));axis('image');colorbar;colormap(hot);
+
+
+%%%%%%%%%%  Jointe hist of cropped area %%%%%
+%%% block 1
+fig_id = 1;
+[J3,f3,rect] = crop_im_fil(Ja,Jfa,fig_id);
+
+filter_id = 1;scale = 1;H1 = hist_I_f(J1,f1(:,:,filter_id,scale));
+
+
+%%% block 2
+fig_id = 1;
+[J2,f2,rect] = crop_im_fil(Ja,Jfa,fig_id);
+
+filter_id = 1;scale = 1;H2 = hist_I_f(J2,f2(:,:,filter_id,scale));
+
+
+%%%%% disp result %%%%%
+
+scales = [1:5];
+filter_ids = [1:7];
+
+figure(6);disp_hist2d(J2,f2,scales,filter_ids);
+
+figure(4);disp_hist2d(J1,f1,scales,filter_ids);
+
+%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%% smaller window
+hw = round(4*sigs(1));
+
+figure(2);%imagesc(Ja);axis('image');
+cs = round(ginput(1));
+
+J1 = get_win(Ja,cs,[hw,hw]);Jf1 = get_win5(Jfa,cs,[hw,hw]);
+figure(4);imagesc(J1);axis('image');drawnow;
+scales = [1:5];filter_ids = [1:7];
+figure(9);H2 = disp_hist2d(J1,Jf1,scales,filter_ids);
+
+figure(6); disp_diff(H2,H2o);
+
+
+%%%%%%  difference in the neighbourhood %%
+hw = round(4*sigs(1));
+hnb = 3;
+
+B = compute_diff(Ja,Jfa,hw,hnb);
+
+
+%%%%%%%%%%
+
+if 0,
+
+figure(4);
+bint = [-0.15:0.02:0.15];
+id = 4;
+
+If = If1;
+for j=1:5,
+   subplot(5,2,2*(j-1)+1);
+   hist(reshape(If(:,:,id,j)./s1,prod(size(If(:,:,id,j))),1),bint);
+end
+
+If = If2;
+for j=1:5,
+   subplot(5,2,2*j);
+   hist(reshape(If(:,:,id,j)./s2,prod(size(If(:,:,id,j))),1),bint);
+end
+
+
+%%% make 2d histogram bin
+figure(5);
+idmax = 5;
+filt_id = 4;
+
+for id=1:idmax,
+
+  subplot(idmax,3,(id-1)*3+1);
+  h2d1 = hist_I_f(I1a,If1(:,:,filt_id,id),binI,bintex);
+  imagesc(h2d1);axis('image')
+  subplot(idmax,3,(id-1)*3+2);
+  h2d2 = hist_I_f(I2a,If2(:,:,filt_id,id),binI,bintex);
+  imagesc(h2d2);axis('image')
+
+  subplot(idmax,3,id*3);
+  imagesc(h2d2/sum(sum(h2d2)) + h2d1/sum(sum(h2d1)));axis('image')
+  colorbar
+end
+
+%%%%%%%%%%%%%%%%%%%%% three types %%%%%%%%
+figure(4);
+idmax = 5;
+filt_id = 2;
+
+width = 4;
+
+for id=1:idmax,
+
+  subplot(idmax,width,(id-1)*width+1);
+  h2d1 = hist_I_f(I1a,If1(:,:,filt_id,id),binI,bintex);
+  h2d1 = h2d1/sum(sum(h2d1));
+  imagesc(h2d1);axis('image');
+
+  subplot(idmax,width,(id-1)*width+2);
+  h2d2 = hist_I_f(I2a,If2(:,:,filt_id,id),binI,bintex);
+  h2d2 = h2d2/sum(sum(h2d2));
+  imagesc(h2d2);axis('image')
+
+  subplot(idmax,width,(id-1)*width+3);
+  h2d3 = hist_I_f(I3a,If3(:,:,filt_id,id),binI,bintex);
+  h2d3 = h2d3/sum(sum(h2d3));
+  imagesc(h2d3);axis('image')
+
+  subplot(idmax,width,id*width);
+  imagesc(h2d1+h2d2+h2d3);axis('image')
+  colorbar
+end
+
+
+%%%%%%%%%%%% smaller window  %%%%
+hw = round(4*sigs(1));
+
+figure(5);%imagesc(I1a);axis('image');
+cs = round(ginput(1));
+
+J = get_win(I1a,cs,[hw,hw]);figure(7);imagesc(J);axis('image');
+
+Jf = get_win5(If1,cs,[hw,hw]);
+scales = 1:5; nscales = length(scales);
+filters = 1:7; nfilters = length(filters);
+
+figure(8);
+for j=1:nscales,
+   for k=1:nfilters,
+       subplot(nscales,nfilters,(j-1)*nfilters+k);
+       h2d = hist_I_f(J,Jf(:,:,(j-1)*7+k));h2d = h2d/sum(sum(h2d));
+       imagesc(h2d);axis('image');colorbar;axis('off');
+   end
+end
+
+
+if 0,
+
+   figure(3);
+   cs = ginput(1);
+
+   ws = [15,15];
+   J = get_win(I,cs,ws);
+   figure(6);imagesc(J);axis('image');
+
+   t1 = get_win5(text_des,cs,ws);
+
+   t1p = abs(t1);
+   %t1p = abs(t1);
+   %t1p = t1.*(t1>0);
+
+   figure(2);im5(t1p,5,6);
+
+   t1p = reshape(t1p,size(t1p,1)*size(t1p,2),size(t1p,3))';
+
+   t1pm = mean(t1p')';
+   t1ps = t1p- t1pm*ones(1,size(t1p,2));
+
+   B = t1ps*t1ps';
+   [v,d] = eig(B);d = diag(d);
+   figure(4);plot(d,'x-');
+
+   figure(5);
+   subplot(2,2,1);vid = 30;plot(reshape(v(:,vid),6,5),'x-');
+
+end
+
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test3.m
new file mode 100755
index 0000000..040ed3d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test3.m
@@ -0,0 +1,4 @@
+fn = 'lightsmall.ppm';
+nr = Ipara(1);nc = Ipara(2);
+
+k = 1;imagesc(reshape(v(:,k).*D,nc,nr)');colorbar
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_best_cut.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_best_cut.m
new file mode 100755
index 0000000..e04c6ff
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_best_cut.m
@@ -0,0 +1,12 @@
+
+fn_base = '134035';
+ev_id = 4;
+para = [12 ev_id nr nc 100];
+Gmask = ones(nr,nc);
+threshold = find_cutpoint(ncutv(:,ev_id),cmapg,12);
+threshold = threshold(1:end-1);
+
+cut_threshold = find_bst_cut(fn_base,para,threshold,Gmask);
+
+figure(8);ims(ncutv(:,ev_id)<cut_threshold,nr,nc);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex.m
new file mode 100755
index 0000000..fc420c7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex.m
@@ -0,0 +1,361 @@
+
+setup_flag = 0;
+cut_window_flag = 0;
+run_flag = 0;
+other_flag = 0;
+test_flag = 1;
+
+
+%%%%%%%%%%%%%%%%%
+if setup_flag == 1,
+I = readpgm('images/134035.pgm');
+
+sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);  
+%[text_des,TIw] = compute_filter(I,sigs,r,szs); 
+load filter_134035
+
+text_des = cutoff(text_des,szs(1));
+
+I_max = 255;
+tex_max = 75;
+
+TIw = cutoff(I,szs(1));
+figure(1);im(TIw);
+
+fts = gen_filters(sigs,r,szs);
+Ft  = reshape(fts,size(fts,1)*size(fts,2),size(fts,3)*size(fts,4));
+Ft = Ft';
+PFt1 = pinv(Ft);
+
+temp1 = zeros(2*szs(end-1)+1,2*szs(end-1)+1);
+temp1(szs(end-1)+1,szs(end-1)+1) = 1;
+Ft2 = [Ft;temp1(:)'];
+PFt2 = pinv(Ft2);
+
+
+end
+
+%%%%%%%%%%
+
+if cut_window_flag == 1,
+wz = [30,30];
+
+figure(1);
+ct0 = round(ginput(1))
+
+t0 = cutout(text_des,ct0,wz)/tex_max;
+I0 = cutout(TIw,ct0,wz)/I_max;
+
+figure(3);im(I0);colorbar
+
+figure(1);
+ct1 = round(ginput(1))
+
+t1 = cutout(text_des,ct1,wz)/tex_max;
+I1 = cutout(TIw,ct1,wz)/I_max;
+
+figure(3);im(I1);colorbar
+
+%%%%%%%%%%
+ts = cat(2,t0,t1);
+Is = [I0,I1];
+
+figure(3);
+subplot(2,1,1);im(Is);
+subplot(2,1,2);im(ts(:,:,15));
+
+end
+
+%%%%%%
+
+if run_flag == 1,
+
+neigs = 15;
+
+fv1 = colize(t1,I1);
+cov1 = fv1*fv1';
+
+[u1,s1] = eigs(cov1,neigs); s1= diag(s1);
+figure(4);plot(s1,'p-');
+
+fv0 = colize(t0,I0);
+cov0 = fv0*fv0';
+
+[u0,s0] = eigs(cov0,neigs); s0 = diag(s0);
+figure(4);hold on; plot(s0,'rp-');
+
+fvs = colize(ts,Is);
+covs = fvs*fvs';
+
+[us,ss] = eigs(covs,neigs); ss = diag(ss);
+figure(4);plot(ss,'mo-');
+hold off
+
+figure(5);
+subplot(2,2,1);
+im(cov0);colorbar;
+subplot(2,2,2);
+im(cov1);colorbar;
+subplot(2,2,3);
+im(covs);colorbar;
+
+%%%%%%%%%%%%%%%%%
+
+ivss = ones(size(ss))./sqrt(ss);
+ivs0 = ones(size(s0))./sqrt(s0);
+ivs1 = ones(size(s1))./sqrt(s1);
+
+
+nv = 10;
+v1 = fv1'*u1(:,1:nv)*diag(ivs1(1:nv));
+v0 = fv0'*u0(:,1:nv)*diag(ivs0(1:nv));
+vs = fvs'*us(:,1:nv)*diag(ivss(1:nv));
+
+
+%v1s = v1(1:50:3721,:);
+vss = vs(1:10:7442,:);
+
+if nv == 1,
+  mag = vss';
+else
+  mag = sum(vss'.*vss');
+end
+
+tmp1 = vss*vss';
+figure(5);im(abs(tmp1));
+
+if 0,
+magx = mag'*ones(1,length(mag));
+magy = ones(length(mag),1)*mag;
+tmp2 = tmp1./(magx+magy);
+figure(5);subplot(1,2,2);im(abs(tmp2));colorbar;axis('off');
+end
+
+
+end
+
+%%%%%%%%%%%%%%%%%%%
+if other_flag == 1,
+
+%tmp1 = PFt*us(:,1);
+%im(reshape(tmp1,sqrt(length(tmp1)),sqrt(length(tmp1))));
+PFt = PFt1;
+
+
+BI0 = back_proj(PFt,u0);
+BI1 = back_proj(PFt,u1);
+BIs = back_proj(PFt,us);
+
+figure(7);im5(BI0(:,:,1:9),3,3);
+figure(8);im5(BI1(:,:,1:9),3,3);
+figure(9);im5(BIs(:,:,1:9),3,3);
+
+
+%%%%%%%%%%
+figure(12);
+im(Is);
+
+figure(13);
+clf
+ x1 = vs(1:3721,2);
+ y1 = vs(1:3721,3);
+ subplot(1,2,1);
+ plot(x1,y1,'.')
+ axis('image');hold on;
+ subplot(1,2,2);
+ x2 = vs(3722:end,2);
+ y2 = vs(3722:end,3);
+ plot(x2,y2,'.')
+ axis('image');hold on;
+
+
+figure(11);
+ subplot(1,2,1);hist0 = im_vect([y1,x1]',ones(size(y1)));
+ im(hist0);axis('xy');
+ subplot(1,2,2);hist1 = im_vect([y2,x2]',ones(size(y1)));
+ im(hist1);axis('xy');
+
+end
+
+if test_flag == 1,
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(1)-1)*size(Is,1) + ct(2);
+
+  cl = 'r';
+  figure(13);subplot(1,2,1);
+  plot(vs(idx,2),vs(idx,3),[cl,'o']);
+  subplot(1,2,2);plot(vs(idx,2),vs(idx,3),[cl,'o']);
+
+  tmp = vs(idx,1:2)*vs(:,1:2)';tmp = reshape(tmp,size(Is,1),size(Is,2));
+  figure(14);subplot(3,2,1);
+  im(abs(tmp));colorbar
+  subplot(3,2,2); im((Is+0.5).*(abs(tmp)>0.2*max(max(abs(tmp)))));
+
+  tmp = vs(idx,1:3)*vs(:,1:3)';tmp = reshape(tmp,size(Is,1),size(Is,2));
+  subplot(3,2,3);
+  im(abs(tmp));colorbar;
+  subplot(3,2,4); im((Is+0.5).*(abs(tmp)>0.2*max(max(abs(tmp)))));
+
+  tmp = vs(idx,1:5)*vs(:,1:5)';
+  tmp = reshape(tmp,size(Is,1),size(Is,2));
+  subplot(3,2,5);
+  im(abs(tmp));colorbar
+  subplot(3,2,6); im((Is+0.5).*(abs(tmp)>0.2*max(max(abs(tmp)))));
+  
+end
+
+
+%%%%%%%%%%
+test_tmp = 0;
+if test_tmp,
+x = -10:0.02:20;
+sig = 4;
+d = exp(-(x.^2)/sig);
+figure(2);plot(x,d);
+
+ers = [];
+for j=0:0.5:10,
+ d1 = exp(-(x-j).^2/sig);
+ hold on
+ plot(x,d1,'r');
+ ers = [ers,sum((d1-d).^2)];
+end
+hold off;
+
+figure(1);plot(ers(end)-ers);
+
+
+
+
+
+
+end
+
+%%%%%%%%%%%%%%%%%%%
+
+
+fvs = colize(ts,Is);
+
+nf = 24;np = 0.5*7442;
+hb.sigs = 0.02*ones(1,nf);
+hb.bmins= -0.6*ones(1,nf);
+hb.bmaxs= 0.6*ones(1,nf);
+hb.nbins= 20*ones(1,nf);
+
+%fh = colize_hist(fvs(1:nf,1:10:end),hb);
+
+fh2 = hist_inner(fvs(1:nf,1:np),hb);
+
+[u,d] = eigs(fh2,60); d = diag(d);
+
+%%%%%%%%%%%%
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(:,1)-1)*size(Is,1) + ct(:,2);
+
+dist = dist_pair(idx,fvs(1:nf,:),hb);
+figure(4);
+im(reshape(dist,size(Is,1),size(Is,2)));colorbar
+
+
+%%%%%%%%%
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(:,1)-1)*size(Is,1) + ct(:,2);
+
+a = colize_hist(fvs(1:nf,idx'),hb);
+
+figure(5);
+cl = 'brgm';
+for j=1:length(idx);
+  plot(a(:,j),cl(j));
+  hold on;
+end
+hold off
+
+%%%%%%%%%%%
+
+%% use chanked feature vectors
+
+chank_size = 1000;
+fname = 'st';
+histbin_fv_chank(fvs(1:nf,:),hb,chank_size,fname);
+
+
+covfh2 = hist_in_chank(fvs(1:nf,:),chank_size,hb.nbins(1));
+[u2,d2] = eigs(covfh2,60); d2 = diag(d2);
+
+figure(4);
+semilogy(d,'p-');
+
+figure(3);imagesc(u);
+
+back_v = backproj_outer_chank(fvs,u,d,chank_size);
+
+back_v2 = backproj_outer_chank2(fvs,u,d,chank_size);
+
+
+%%%%%%%%%%
+figure(2);
+for j = 1:16,
+  subplot(4,4,j);
+  im(reshape(back_v(:,j),size(Is,1),size(Is,2)));
+  axis('off');title(num2str(j));
+end
+
+binv = linspace(-0.6,0.6,20);
+
+figure(4);
+for j=1:16,
+  subplot(4,4,j);
+  imagesc(reshape(u(:,j),20,24));title(num2str(j));drawnow;
+end
+
+
+figure(6);
+for j=1:16,
+  subplot(4,4,j);
+  plot(binv,(reshape(u(:,j),20,24)));title(num2str(j));drawnow;
+end
+
+
+%%%%%%%%%%
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(:,1)-1)*size(Is,1) + ct(:,2);
+
+figure(5);
+for j = 1:7*2,
+  subplot(7,2,j);
+  nvv = 2*j;
+  dist = back_v(idx,1:nvv)*back_v(:,1:nvv)';
+  im(reshape(abs(dist).^2,size(Is,1),size(Is,2)));colorbar
+  axis('off');title(num2str(nvv));
+end
+
+
+a = colize_hist(fvs(1:nf,idx'),hb)';
+
+dist_raw = dist_pair_chank(a,fvs,chank_size);
+figure(3);im(reshape(dist_raw.^2,size(Is,1),size(Is,2)));
+
+
+
+%%%%%%%%%%%%%%
+figure(12);
+  ct_t3 = round(ginput(5));
+  idx_t3 = (ct_t3(:,1)-1)*size(Is,1) + ct_t3(:,2);
+
+a1 = colize_hist(fvs(1:nf,idx_t1'),hb)';
+a2 = colize_hist(fvs(1:nf,idx_t2'),hb)';
+a3 = colize_hist(fvs(1:nf,idx_t3'),hb)';
+
+
+%%%%%%%%%%%
+figure(1);
+for j=1:9,
+        subplot(3,3,j);
+        hist(back_v(:,j))
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex2.m
new file mode 100755
index 0000000..9e1d3e7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex2.m
@@ -0,0 +1,136 @@
+
+%%%%%%%%% test histogram on gray levels %%%%%%%%%%%%%
+
+%load st
+%fvs = colize(Is,Is);
+
+nf = 1;np = 7442;nbins = 10;
+
+hb.sigs = 0.02*ones(1,nf);
+hb.bmins= 0*ones(1,nf);
+hb.bmaxs= 1*ones(1,nf);
+hb.nbins= nbins*ones(1,nf);
+
+fh = colize_hist(fvs(1:nf,1:np),hb);
+
+fh_inner = fh*fh';
+
+nv = nbins-1;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(6);
+for j=1:nv,
+        subplot(4,4,j);
+        plot(u(:,j));
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fh'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(7);
+for j=1:nv,
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+figure(1);
+plot(d,'p-');
+figure(2);
+im(u);
+
+
+%%%%%%%%%  try the joint x-I histogram bin  %%%%%%%%%%%%%
+
+x = [1:size(Is,1)]'*ones(1,size(Is,2));
+x = reshape(x,size(Is,1),size(Is,2));
+
+joint_f(:,:,1) = x;
+joint_f(:,:,2) = Is;
+
+fvs = colize(joint_f,Is);
+
+nf = 2;np = 7442;nbins = [5,10];
+
+hb.sigs = [4,0.02].*ones(1,nf);
+hb.bmins= [1,0].*ones(1,nf);
+hb.bmaxs= [size(Is,1),1].*ones(1,nf);
+hb.nbins= nbins.*ones(1,nf);
+
+fh = colize_joint_hist(fvs,hb);
+fh = reshape(fh,50,np);
+
+fh_inner = fh*fh';
+
+nv = 30;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(3);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(u(:,j),5,10));axis('off');
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fh'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(4);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+
+%%%%%%%%  
+
+
+joint_f = [];
+
+joint_f(:,:,1) = Is;
+joint_f(:,:,2) = ts(:,:,1);
+
+fvs = colize(joint_f,Is);
+
+nf = 2;np = 7442;nbins = [10,10];
+
+hb.sigs = [0.02,0.02].*ones(1,nf);
+hb.bmins= [0,-0.6].*ones(1,nf);
+hb.bmaxs= [1,0.6].*ones(1,nf);
+hb.nbins= nbins.*ones(1,nf);
+
+fh = colize_joint_hist(fvs,hb);
+
+fh = reshape(fh,size(fh,1)*size(fh,2),np);
+
+fh_inner = fh*fh';
+
+nv = 30;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(3);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(u(:,j),10,10));axis('off');
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fh'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(4);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex3.m
new file mode 100755
index 0000000..2f245ec
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex3.m
@@ -0,0 +1,169 @@
+%%%%%%%%% test histogram on gray levels %%%%%%%%%%%%%
+
+%load st
+
+nf = 24;np = 7442;nbins = 10;
+fvs = colize(ts(:,:,1:nf),Is);
+
+hb.sigs = 0.02*ones(1,nf);
+hb.bmins= -0.6*ones(1,nf);
+hb.bmaxs= 0.6*ones(1,nf);
+hb.nbins= nbins*ones(1,nf);
+
+fh = colize_hist(fvs(1:nf,1:np),hb);
+
+nw = 4;
+fhs = colize_histneighb(fh,Is,nw);
+
+%%%%%%%%%%%%%%%%%%
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(:,1)-1)*size(Is,1) + ct(:,2);
+
+figure(1);
+subplot(1,2,1);
+imagesc(reshape(fhs(:,idx),nbins,nf))
+subplot(1,2,2);
+imagesc(reshape(fh(:,idx),nbins,nf))
+%%%%%%%%%%
+
+fh = fhs;
+fhs = sqrt(fhs);
+
+fh_inner = fhs*fhs';
+
+nv = 30;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(3);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        %plot(u(:,j));
+        im(reshape(u(:,j),nbins,nf));
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fhs'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(4);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+figure(1);
+semilogy(d,'p-');
+%figure(2);imagesc(u);
+
+%%%%%%%%%
+figure(12);
+  ct = round(ginput(1));
+  idx = (ct(:,1)-1)*size(Is,1) + ct(:,2);
+
+figure(5);
+for j = 1:min(14,nv),
+  subplot(7,2,j);
+  nvv = j;
+  dist = back_v(idx,1:nvv)*back_v(:,1:nvv)';
+  im(reshape(abs(dist).^2,size(Is,1),size(Is,2)));colorbar
+  axis('off');title(num2str(nvv));
+end
+
+
+
+
+%%%%%%%%%  try the joint x-I histogram bin  %%%%%%%%%%%%%
+
+x = [1:size(Is,1)]'*ones(1,size(Is,2));
+x = reshape(x,size(Is,1),size(Is,2));
+
+joint_f(:,:,1) = x;
+joint_f(:,:,2) = Is;
+
+fvs = colize(joint_f,Is);
+
+nf = 2;np = 7442;nbins = [5,10];
+
+hb.sigs = [4,0.02].*ones(1,nf);
+hb.bmins= [1,0].*ones(1,nf);
+hb.bmaxs= [size(Is,1),1].*ones(1,nf);
+hb.nbins= nbins.*ones(1,nf);
+
+fh = colize_joint_hist(fvs,hb);
+fh = reshape(fh,50,np);
+
+fh_inner = fh*fh';
+
+nv = 30;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(3);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(u(:,j),5,10));axis('off');
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fh'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(4);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+
+%%%%%%%%  
+
+
+joint_f = [];
+
+joint_f(:,:,1) = Is;
+joint_f(:,:,2) = ts(:,:,1);
+
+fvs = colize(joint_f,Is);
+
+nf = 2;np = 7442;nbins = [10,10];
+
+hb.sigs = [0.02,0.02].*ones(1,nf);
+hb.bmins= [0,-0.6].*ones(1,nf);
+hb.bmaxs= [1,0.6].*ones(1,nf);
+hb.nbins= nbins.*ones(1,nf);
+
+fh = colize_joint_hist(fvs,hb);
+
+fh = reshape(fh,size(fh,1)*size(fh,2),np);
+
+fh_inner = fh*fh';
+
+nv = 30;
+
+[u,d] = eigs(fh_inner,nv); d = diag(d);
+
+figure(3);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(u(:,j),10,10));axis('off');
+        title(num2str(j));
+end
+
+s = 1./sqrt(d);
+
+back_v = (fh'*u(:,1:nv)).*(ones(np,1)*s(1:nv)');
+
+figure(4);
+for j=1:min(16,nv),
+        subplot(4,4,j);
+        im(reshape(back_v(:,j),size(Is,1),size(Is,2)));axis('off');
+        title(num2str(j));
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex4.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex4.m
new file mode 100755
index 0000000..61f1307
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex4.m
@@ -0,0 +1,353 @@
+
+setup_flag = 0;
+cut_window_flag = 0;
+run_flag = 0;
+other_flag = 0;
+test_flag = 1;
+
+
+%%%%%%%%%%%%%%%%%
+if setup_flag == 1,
+% = readpgm('images/134035.pgm');
+
+load st3
+
+I_max = 255;
+tex_max = 40;
+
+I2 = min(1,I2/I_max);
+t2 = t2/tex_max;
+t2 = t2.*(t2<=1) + 1*(t2>1);
+t2 = t2.*(t2>=-1) + (-1)*(t2<-1);
+
+
+end
+
+%%%%%%%%%%
+
+%% for a given sampling rate, get the index for window center
+%%
+
+[nr,nc] = size(I2);
+
+hw = 3;
+st_sz = 2*hw + 1;
+
+nr_chank = floor(nr/st_sz);
+nc_chank = floor(nc/st_sz);
+
+id_chank = [];
+for k=1+hw:st_sz:nc-hw,
+   for j=1+hw:st_sz:nr-hw,   
+        id = j+(k-1)*nr;
+        id_chank = [id_chank,id];
+   end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%% F1 difference  %%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+fvs = 2*I2(:)'; fvs = fvs -1;
+
+nf = 1;
+hb.sigs = 0.02*ones(1,nf);
+hb.bmins= -1*ones(1,nf);
+hb.bmaxs= 1*ones(1,nf);
+hb.nbins= 10*ones(1,nf);
+
+fh = colize_hist(fvs(1:nf,:),hb);
+fhs = colize_histnb_s(fh,I2,nw,hw);
+
+A = fhs'*fhs;
+figure(1);im(A);colorbar;
+
+B = A;
+
+%% display %%%
+figure(12);
+ct = round(ginput(1));
+ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(3);
+im(reshape(A(idx,:),nr_chank,nc_chank));colorbar;
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%% F2 difference  %%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+nw = 4;hw =3;
+
+tnf = size(t2,3);
+fst = 1;
+r_id = 1;
+for j=1:fst:tnf,
+   nf = fst;
+   hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+   hb.bmaxs= 1*ones(1,nf);    hb.nbins= 10*ones(1,nf);
+
+   fvs = colize(t2(:,:,j:j+fst-1),I2);
+   fh = colize_hist(fvs,hb);
+   fhs = colize_histnb_s(fh,I2,nw,hw);
+   A = fhs'*fhs;
+   cm = sprintf('save F%d A fhs',r_id+1);
+   disp(cm);eval(cm);
+   clear fh;
+   
+   B = B + A;
+
+   clear A;
+   
+
+   r_id = r_id +1;
+end
+
+
+%%%%%% debug + display %%%%%%%%
+
+figure(6);
+for j=2:30,
+ subplot(5,6,j);
+ im(t2(:,:,j-1));axis('off');title(num2str(j-1));
+end
+subplot(5,6,1);im(I2);axis('off');
+
+
+figure(6);
+B = zeros(size(A));
+for j = 1:31,
+ %subplot(5,6,j);
+ cm = sprintf('load F%d;',j);
+ disp(cm);eval(cm);
+ 
+ fhs1 = sqrt(fhs); A = fhs1'*fhs1;
+%  im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');title(num2str(j-1));colorbar;
+ B = B+A;
+end
+
+
+%%%%%% disp dist. %%%%%%
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(2);
+im(reshape(B(idx,:),nr_chank,nc_chank));axis('off');title('B');colorbar; 
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%  F3 features  %%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%% Joint Intensity with filters %%%%
+
+
+tnf = size(t2,3);
+
+plaatjeon = 1;
+if plaatjeon,
+     for j=7:tnf,
+       cm = sprintf('!touch /disks/plaatje/scratch/jshi/FJ%d.mat',j);
+        disp(cm);
+        eval(cm);cm = sprintf('!ln -s /disks/plaatje/scratch/jshi/FJ%d.mat .',j);
+        disp(cm);eval(cm);
+     end
+else
+     for j=1:1,
+        cm = sprintf('!touch ~/store/st/FJ%d.mat',j);   
+        disp(cm);eval(cm);
+        cm = sprintf('!ln -s ~/store/st/FJ%d.mat .',j);
+        disp(cm);eval(cm);
+     end
+end
+
+for j=7:tnf,
+   nf = 2;
+   hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+   hb.bmaxs= 1*ones(1,nf);    hb.nbins= 10*ones(1,nf);
+
+   fvs = colize(cat(3,t2(:,:,j),I2));
+
+   fhs = colize_histnb_sf(fvs,I2,hb,nw,hw);
+   fhs = sqrt(fhs);
+   A = fhs'*fhs;
+   cm = sprintf('save FJ%d A fhs',j);
+   disp(cm);eval(cm);
+  
+end
+
+%%%% reload data %%%%%%%%%%%%%%
+B = zeros(size(A));
+
+figure(3);
+
+for j=1:tnf,
+  cm = sprintf('load FJ%d;',j);
+  disp(cm);eval(cm);
+
+  subplot(5,6,j);
+  im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');title(num2str(j));
+
+  B = B + A;
+end
+
+figure(2);im(reshape(B(idx,:),nr_chank,nc_chank));axis('off');title('B');
+
+
+%%%%%% disp dist. %%%%%%
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(2);
+im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');colorbar; 
+
+%%%%%% disp Joint Hist %%%%%%%%%
+
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(1);
+im(reshape(fhs(:,idx),10,10));axis('off');colorbar; 
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%  F4: Joint filters %%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+
+tnf = size(t2,3);
+
+nw = 4;hw =3;
+
+for scale=1:5,
+  for angle = 1:3,
+        cm = sprintf('!touch /disks/plaatje/scratch/jshi/FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+        cm = sprintf('!ln -s /disks/plaatje/scratch/jshi/FFJ_%d_%d_%d_%d.mat .',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+  end
+end
+
+
+for scale = 1:5,
+  for angle = 1:3,
+     nf = 2;
+     hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+     hb.bmaxs= 1*ones(1,nf);    hb.nbins= 10*ones(1,nf);
+
+     fvs = colize(cat(3,t2(:,:,(scale-1)*6+angle),...
+                        t2(:,:,(scale-1)*6+angle+3)));
+
+     fhs = colize_histnb_sf(fvs,I2,hb,nw,hw);
+     fhs = sqrt(fhs);
+     A = fhs'*fhs;
+     cm = sprintf('save FFJ_%d_%d_%d_%d A fhs',angle,angle+3,scale,scale);
+     disp(cm);eval(cm);
+   end
+end
+
+
+%%%%%%%%%  load results  %%%%%%%%%%%
+%B = zeros(size(A));
+
+figure(3);
+for scale=1:5,
+  for angle = 1:3,
+        cm = sprintf('load FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+
+        subplot(3,5,scale+(angle-1)*5);
+        im(reshape(A(idx,:),nr_chank,nc_chank));
+        axis('off');title(sprintf('%d-%d,%d',angle,angle+3,scale));
+
+        %B = B + A;
+  end
+end
+
+
+
+
+%%%  disp results 
+
+angle = 1;scale = 1;
+cm = sprintf('load FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+disp(cm);eval(cm);
+
+        
+
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+%figure(1);im(reshape(fhs(:,idx),10,10));axis('off');%colorbar; 
+%figure(2);im(reshape(A(idx,:),nr_chank,nc_chank));%axis('off');%title('B');
+figure(4);im(reshape(B(idx,:),nr_chank,nc_chank));%axis('off');%title('B');
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%% reduction  %%%%%%%%%%%%%%%%%
+nv = 50;
+[uB,dB] = eigs(B,nv);dB = diag(dB);
+
+figure(1);subplot(2,1,1);plot(dB,'p-');
+subplot(2,1,2);semilogy(dB,'p-');
+
+figure(2);
+
+for j=1:20,
+  subplot(4,5,j);
+  im(reshape(uB(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+
+%%%%% Ncut without reduction %%%%
+[uNu,dNu] = eig_decomp_v5(B,20);
+
+figure(1);subplot(2,1,1);plot(dNu,'p-');
+subplot(2,1,2);semilogy(dNu,'p-');
+
+figure(2);
+for j=2:6,
+  subplot(1,5,j-1);
+  im(reshape(-uNu(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+%%%%%% Ncut with reduction %%%%%%%%%
+nvv = 6;
+B1 = uB(:,1:nvv)*uB(:,1:nvv)';
+
+
+[uN,dN] = eig_decomp_v5(abs(B1),20);
+
+figure(1);subplot(2,1,1);plot(dN,'p-');
+subplot(2,1,2);semilogy(dN,'p-');
+
+figure(3);
+for j=2:6,
+  subplot(1,5,j-1);
+  im(reshape(uN(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+
+%%%%%% 
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex5.m
new file mode 100755
index 0000000..2b86e0a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_evtex5.m
@@ -0,0 +1,446 @@
+
+setup_flag = 0;
+cut_window_flag = 0;
+run_flag = 0;
+other_flag = 0;
+test_flag = 1;
+
+
+%%%%%%%%%%%%%%%%%
+if setup_flag == 1,
+
+sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);  
+szs = szs(length(szs))*ones(1,length(szs));
+num_ori = 6;
+
+compute_flag = 0;
+if compute_flag,
+fnames = [134002,134007,134011,134013,130065,130038,130039,130040,130042,...
+          130045,130046,130056,130068];
+
+for j=1:length(fnames),
+   fname = sprintf('images/%d.pgm',fnames(j));
+
+   cm = sprintf('!touch /disks/plaatje/scratch/jshi/Fe_%d.mat',fnames(j));
+   disp(cm);eval(cm);
+
+   cm = sprintf('!ln -s /disks/plaatje/scratch/jshi/Fe_%d.mat .',fnames(j));
+   disp(cm);eval(cm);
+
+   disp(fname);
+   I = readpgm(fname);figure(3);im(I);title(num2str(fname));drawnow;
+   [text_des,filters] = compute_filter_fft(I,sigs,r,szs,num_ori); 
+
+   cm = sprintf('save Fe_%d text_des filters fname sigs r szs num_ori',fnames(j));
+   disp(cm);eval(cm);
+
+   clear text_des filters I
+end
+
+end
+else
+%%%%%%%%%%%%%
+  fname = 134013;
+
+  Iname = sprintf('images/%d.pgm',fname);
+  I = readpgm(Iname);
+
+  cm = sprintf('load Fe_%d.mat',fname);
+  disp(cm);eval(cm);
+
+  figure(1);im(I);
+
+
+  cutsz =20;
+  I = cutoff(I,cutsz);figure(1);im(I);
+  text_des = cutoff(text_des,cutsz);
+
+  figure(2);
+  for j =1:30,
+    subplot(5,6,j);im(text_des(:,:,j));axis('off');
+  end
+
+  I1 = I(20:200,70:240);
+  T1 = text_des(20:200,70:240,:);
+
+  save st_134013 I1 T1 fname sigs szs r num_ori
+
+end
+
+
+  
+%%%%%%%%%%% normalization %%%%%%%%%%%
+
+
+I_max = 250;
+tex_max = 40;
+
+I1 = min(1,I1/I_max);
+T1 = T1/tex_max;
+T1 = T1.*(T1<=1) + 1*(T1>1);
+T1 = T1.*(T1>=-1) + (-1)*(T1<-1);
+
+
+end
+
+%%%%%%%%%%
+
+%% for a given sampling rate, get the index for window center
+%%
+
+[nr,nc] = size(I1);
+
+hw = 3;
+st_sz = 2*hw + 1;
+
+nr_chank = floor(nr/st_sz);
+nc_chank = floor(nc/st_sz);
+
+id_chank = [];
+for k=1+hw:st_sz:nc-hw,
+   for j=1+hw:st_sz:nr-hw,   
+        id = j+(k-1)*nr;
+        id_chank = [id_chank,id];
+   end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%% F1 difference  %%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+fvs = 2*I1(:)'; fvs = fvs -1;
+
+nf = 1;
+hb.sigs = 0.02*ones(1,nf);
+hb.bmins= -1*ones(1,nf);
+hb.bmaxs= 1*ones(1,nf);
+hb.nbins= 10*ones(1,nf);
+nw = 4;hw =3;
+
+fh = colize_hist(fvs(1:nf,:),hb);
+fhs = colize_histnb_s(fh,I1,nw,hw);
+
+fhs = sqrt(fhs);
+A = fhs'*fhs;
+figure(2);im(A);colorbar;
+
+B = A;
+
+%% display %%%
+figure(2);
+ct = round(ginput(1));
+ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(3);im(reshape(A(idx,:),nr_chank,nc_chank));colorbar;
+
+subplot(1,2,1);im(reshape(A1(idx,:),nr_chank,nc_chank));colorbar;
+subplot(1,2,2);im(reshape(A2(idx,:),nr_chank,nc_chank));colorbar;
+
+
+%%%%%%%%%%
+save_flag = 0;
+
+fn = 134013;
+
+if save_flag,
+  cm = sprintf('save F1_%d fhs hw nw nr_chank nc_chank',fn);
+  disp(cm);eval(cm);
+
+end
+
+load_flag = 1;
+if load_flag,
+  cm = sprintf('load F1_%d',fn);
+  disp(cm);eval(cm);
+
+  A=fhs'*fhs;
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%% F2 difference  %%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+nw = 4;hw =3;
+
+tnf = size(T1,3);
+fst = 1;
+
+for j=1:fst:1,
+   nf = fst;
+   hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+   hb.bmaxs= 1*ones(1,nf);    hb.nbins= 15*ones(1,nf);
+
+   fvs = colize(T1(:,:,j:j+fst-1),I1);
+   fh = colize_hist(fvs,hb);
+   fhs = colize_histnb_s(fh,I1,nw,hw);
+   fhs = sqrt(fhs);
+
+   A = fhs'*fhs;
+
+   cm = sprintf('save F2_%d_%d fhs hw nw nr_chank nc_chank',j,fn);
+   disp(cm);eval(cm);
+   clear fh;
+   
+   B = B + A;
+
+   clear A;
+   
+end
+
+
+%%%%%% debug + display %%%%%%%%
+
+figure(6);
+for j=2:30,
+ subplot(5,6,j);
+ im(T1(:,:,j-1));axis('off');title(num2str(j-1));
+end
+subplot(5,6,1);im(I1);axis('off');
+
+
+figure(6);
+B = zeros(size(A));
+for j = 1:31,
+ %subplot(5,6,j);
+ cm = sprintf('load F%d;',j);
+ disp(cm);eval(cm);
+ 
+ fhs1 = sqrt(fhs); A = fhs1'*fhs1;
+%  im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');title(num2str(j-1));colorbar;
+ B = B+A;
+end
+
+
+%%%%%% disp dist. %%%%%%
+weight= 5;
+A = weight*B+B2;
+
+figure(1);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(2);
+im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');colorbar; %title('B');
+%figure(3);
+
+
+save_flag = 0;
+if save_flag ,
+  B2 = B;
+  save tmp B2 nr_chank nc_chank
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%  F3 features  %%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%% Joint Intensity with filters %%%%
+
+
+tnf = size(T1,3);
+
+plaatjeon = 1;
+if plaatjeon,
+     for j=7:tnf,
+       cm = sprintf('!touch /disks/plaatje/scratch/jshi/FJ%d.mat',j);
+        disp(cm);
+        eval(cm);cm = sprintf('!ln -s /disks/plaatje/scratch/jshi/FJ%d.mat .',j);
+        disp(cm);eval(cm);
+     end
+else
+     for j=1:1,
+        cm = sprintf('!touch ~/store/st/FJ%d.mat',j);   
+        disp(cm);eval(cm);
+        cm = sprintf('!ln -s ~/store/st/FJ%d.mat .',j);
+        disp(cm);eval(cm);
+     end
+end
+
+for j=7:tnf,
+   nf = 2;
+   hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+   hb.bmaxs= 1*ones(1,nf);    hb.nbins= 10*ones(1,nf);
+
+   fvs = colize(cat(3,T1(:,:,j),I1));
+
+   fhs = colize_histnb_sf(fvs,I1,hb,nw,hw);
+   fhs = sqrt(fhs);
+   A = fhs'*fhs;
+   cm = sprintf('save FJ%d A fhs',j);
+   disp(cm);eval(cm);
+  
+end
+
+%%%% reload data %%%%%%%%%%%%%%
+B = zeros(size(A));
+
+figure(3);
+
+for j=1:tnf,
+  cm = sprintf('load FJ%d;',j);
+  disp(cm);eval(cm);
+
+  subplot(5,6,j);
+  im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');title(num2str(j));
+
+  B = B + A;
+end
+
+figure(2);im(reshape(B(idx,:),nr_chank,nc_chank));axis('off');title('B');
+
+
+%%%%%% disp dist. %%%%%%
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(2);
+im(reshape(A(idx,:),nr_chank,nc_chank));axis('off');colorbar; 
+
+%%%%%% disp Joint Hist %%%%%%%%%
+
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+figure(1);
+im(reshape(fhs(:,idx),10,10));axis('off');colorbar; 
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%  F4: Joint filters %%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+
+tnf = size(T1,3);
+
+nw = 4;hw =3;
+
+for scale=1:5,
+  for angle = 1:3,
+        cm = sprintf('!touch /disks/plaatje/scratch/jshi/FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+        cm = sprintf('!ln -s /disks/plaatje/scratch/jshi/FFJ_%d_%d_%d_%d.mat .',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+  end
+end
+
+
+for scale = 1:5,
+  for angle = 1:3,
+     nf = 2;
+     hb.sigs = 0.02*ones(1,nf); hb.bmins= -1*ones(1,nf);
+     hb.bmaxs= 1*ones(1,nf);    hb.nbins= 10*ones(1,nf);
+
+     fvs = colize(cat(3,T1(:,:,(scale-1)*6+angle),...
+                        T1(:,:,(scale-1)*6+angle+3)));
+
+     fhs = colize_histnb_sf(fvs,I1,hb,nw,hw);
+     fhs = sqrt(fhs);
+     A = fhs'*fhs;
+     cm = sprintf('save FFJ_%d_%d_%d_%d A fhs',angle,angle+3,scale,scale);
+     disp(cm);eval(cm);
+   end
+end
+
+
+%%%%%%%%%  load results  %%%%%%%%%%%
+%B = zeros(size(A));
+
+figure(3);
+for scale=1:5,
+  for angle = 1:3,
+        cm = sprintf('load FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+        disp(cm);eval(cm);
+
+        subplot(3,5,scale+(angle-1)*5);
+        im(reshape(A(idx,:),nr_chank,nc_chank));
+        axis('off');title(sprintf('%d-%d,%d',angle,angle+3,scale));
+
+        %B = B + A;
+  end
+end
+
+
+
+
+%%%  disp results 
+
+angle = 1;scale = 1;
+cm = sprintf('load FFJ_%d_%d_%d_%d.mat',angle,angle+3,scale,scale);
+disp(cm);eval(cm);
+
+        
+
+figure(12);
+ct = round(ginput(1));ct_chank(1) = round((ct(1)-hw-1)/st_sz) + 1;
+ct_chank(2) = round((ct(2)-hw-1)/st_sz) + 1;
+idx = (ct_chank(:,1)-1)*nr_chank + ct_chank(:,2);
+
+%figure(1);im(reshape(fhs(:,idx),10,10));axis('off');%colorbar; 
+%figure(2);im(reshape(A(idx,:),nr_chank,nc_chank));%axis('off');%title('B');
+figure(4);im(reshape(B(idx,:),nr_chank,nc_chank));%axis('off');%title('B');
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%% reduction  %%%%%%%%%%%%%%%%%
+nv = 50;
+[uA,dA] = eigs(A,nv);dA = diag(dA);
+
+figure(4);suAplot(2,1,1);plot(dA,'p-');
+subplot(2,1,2);semilogy(dA,'p-');
+
+figure(3);
+
+for j=1:20,
+  subplot(4,5,j);
+  im(reshape(uA(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+
+%%%%% Ncut without reduction %%%%
+
+[uNu,dNu] = eig_decomp_v5(A,20);
+
+figure(4);subplot(2,1,1);plot(dNu,'p-');
+subplot(2,1,2);semilogy(dNu,'p-');
+
+figure(3);
+for j=2:6,
+  subplot(1,5,j-1);
+  im(reshape(-uNu(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+%%%%%% Ncut with reduction %%%%%%%%%
+nvv = 7;
+A1 = uA(:,1:nvv)*uA(:,1:nvv)';
+
+
+[uN,dN] = eig_decomp_v5(abs(A1),20);
+
+figure(1);subplot(2,1,1);plot(dN,'p-');
+subplot(2,1,2);semilogy(dN,'p-');
+
+figure(3);
+for j=2:6,
+  subplot(1,5,j-1);
+  im(reshape(uN(:,j),nr_chank,nc_chank));axis('off');colorbar;title(num2str(j));
+end
+
+
+%%%%%% 
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion.m
new file mode 100755
index 0000000..91c97f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion.m
@@ -0,0 +1,117 @@
+
+im_sz = [40,40];
+
+ob_szh = [6,3];
+
+ob_c  = [15,12];
+
+bg_color = 0.2;
+
+ob_color = 0.8;
+
+mag = 0.2;
+
+I_bg = bg_color + mag*randn(im_sz);
+
+I_obj = ob_color + mag*randn(2*ob_szh+1);
+
+
+w = 3;
+
+v5 = 1;
+Js = [];
+
+if ~v5,
+  for j=1:5,
+    fc = sprintf('J%d = I_bg;',j);
+    eval(fc);
+
+    fc = sprintf('J%d(ob_c(1)-ob_szh(1):ob_c(1)+ob_szh(1),ob_c(2)-ob_szh(2):ob_c(2)+ob_szh(2)) = I_obj;',j);
+    eval(fc);
+
+    ob_c = ob_c+[0,2];
+  end
+else
+  nf  = 4;
+  for j = 1:nf,
+
+     J = I_bg;
+     J(ob_c(1)-ob_szh(1):ob_c(1)+ob_szh(1),ob_c(2)-ob_szh(2):ob_c(2)+ob_szh(2)) = I_obj;
+
+     if (j==1),
+       [gy,gx] = grad(J,w);
+     end
+
+     ob_c = ob_c+[0,2];
+
+     Jw = cutoff(J,w);
+     Js(:,:,j) = Jw;
+  end
+
+end
+
+[nr,nc] = size(gx);
+
+for j=1:nf,
+  subplot(1,nf,j);
+  imagesc(Js(:,:,j));axis('tightequal');
+end
+
+
+writepnm5('test_motion.pnm',Js);
+writepnm5('test_motion_gx.pnm',gx);
+writepnm5('test_motion_gy.pnm',gy);
+%imagesc(J1);colorbar;
+
+
+inpara = [2,5,0.5,1,0.5];
+
+[A,D,Ipara] = cas('test_motion',inpara);
+
+B= A+ A';
+clear A;
+
+%BB = B(1:19^2,19^2+(1:19^2));
+%imagesc(BB);
+
+[v,d] = eigs(B);d = diag(d);
+
+k = 2;
+
+figure(1);
+%nf = 5;
+
+nr = nr-5;
+nc = nc-5;
+
+n = nr* nc;
+
+for j =1:nf,
+  subplot(1,nf,j);
+  imagesc(reshape(v((j-1)*n+(1:n),k).*D(1:n),nr,nc)');axis('tightequal');
+end
+
+%%%%%
+
+
+figure(3);
+T = readpnm('test_motion.pnm');
+nf = size(T,3);
+for j=1:nf,
+  subplot(1,nf,j);
+  imagesc(T(:,:,j));axis('tightequal');
+end
+
+
+figure(2);
+Gx = readpnm('test_motion_gx.pnm');
+
+[nr,nc] =size(Gx);
+n = nr*nc;
+
+imagesc(reshape(B(n+1:2*n,6*nc+7),nc,nr)');colorbar
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion2.m
new file mode 100755
index 0000000..2959fa8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_motion2.m
@@ -0,0 +1,127 @@
+
+im_sz = [40,40];
+
+ob_szh = [4,3];
+
+ob_c  = [12,12];
+
+ob_co = [30,28];
+
+bg_color = 0.2;
+
+ob_color = 0.8;
+
+mag = 0.2;
+
+I_bg = bg_color + mag*randn(im_sz);
+
+I_obj = ob_color + mag*randn(2*ob_szh+1);
+
+
+w = 3;
+
+v5 = 1;
+Js = [];
+
+if ~v5,
+  for j=1:5,
+    fc = sprintf('J%d = I_bg;',j);
+    eval(fc);
+
+    fc = sprintf('J%d(ob_c(1)-ob_szh(1):ob_c(1)+ob_szh(1),ob_c(2)-ob_szh(2):ob_c(2)+ob_szh(2)) = I_obj;',j);
+    eval(fc);
+
+    fc = sprintf('J%d(ob_co(1)-ob_szh(1):ob_co(1)+ob_szh(1),ob_co(2)-ob_szh(2):ob_co(2)+ob_szh(2)) = I_obj;',j);
+    eval(fc);
+
+    ob_c = ob_c+[0,2];
+    ob_co = ob_co-[0,2];
+
+  end
+else
+  nf  = 4;
+  for j = 1:nf,
+
+     J = I_bg;
+     J(ob_c(1)-ob_szh(1):ob_c(1)+ob_szh(1),ob_c(2)-ob_szh(2):ob_c(2)+ob_szh(2)) = I_obj;
+     J(ob_co(1)-ob_szh(1):ob_co(1)+ob_szh(1),ob_co(2)-ob_szh(2):ob_co(2)+ob_szh(2)) = I_obj;
+    
+     if (j==1),
+       [gy,gx] = grad(J,w);
+     end
+
+     ob_c = ob_c+[0,2];
+     ob_co = ob_co-[0,2];
+
+     Jw = cutoff(J,w);
+     Js(:,:,j) = Jw;
+  end
+
+end
+
+[nr,nc] = size(gx);
+
+for j=1:nf,
+  subplot(1,nf,j);
+  imagesc(Js(:,:,j));axis('tightequal');
+end
+
+
+writepnm5('test_motion.pnm',Js);
+writepnm5('test_motion_gx.pnm',gx);
+writepnm5('test_motion_gy.pnm',gy);
+%imagesc(J1);colorbar;
+
+
+inpara = [2,5,0.5,1,0.5];
+
+[A,D,Ipara] = cas('test_motion',inpara);
+
+B= A+ A';
+clear A;
+
+%BB = B(1:19^2,19^2+(1:19^2));
+%imagesc(BB);
+
+[v,d] = eigs(B);d = diag(d);
+
+k = 2;
+
+figure(1);
+%nf = 5;
+
+nr = nr-5;
+nc = nc-5;
+
+n = nr* nc;
+
+for j =1:nf,
+  subplot(1,nf,j);
+  imagesc(reshape(v((j-1)*n+(1:n),k).*D(1:n),nr,nc)');axis('tightequal');
+end
+
+%%%%%
+
+
+figure(3);
+T = readpnm('test_motion.pnm');
+nf = size(T,3);
+for j=1:nf,
+  subplot(1,nf,j);
+  imagesc(T(:,:,j));axis('tightequal');
+end
+
+
+figure(2);
+Gx = readpnm('test_motion_gx.pnm');
+
+[nr,nc] =size(Gx);
+n = nr*nc;
+
+imagesc(reshape(B(n+1:2*n,6*nc+7),nc,nr)');colorbar
+
+
+%%%%%%%%%%%%%
+
+
+K = zeros(im_sz);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_period.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_period.m
new file mode 100755
index 0000000..2994d15
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_period.m
@@ -0,0 +1,58 @@
+flag = 2;
+
+if flag ==1,
+
+ws = [50,50];
+
+figure(1);J = get_win(I,ginput(1),ws);
+figure(4);imagesc(J);
+
+J = J - mean(mean(reshape(J,prod(size(J)),1)));
+X = fftshift(fft2(J));
+
+figure(3);imagesc(abs(X));colorbar
+figure(2);mesh(abs(X));
+
+else
+
+fn = '1.pgm';
+
+% spatial gaussian parameter
+xscale = 1;
+
+% half size of the neighbourhood 
+xnb = 5;
+
+% setting the the HSV gaussian parameter:[h s v]
+Iscale = [0.01];
+
+Input_para = [xscale,xnb,Iscale];
+
+% compute the lower half the association matrix
+[A,D,Ipara] = compute_A_pgm(fn,Input_para);
+
+nr = Ipara(1);nc = Ipara(2);
+
+B = A+A';
+clear A;
+
+
+% eigen decompostion
+options.tol = 1e-4;
+num_eig_v = 10;
+fprintf('doing eigs ...\n');
+[v,d] = eigs(B,num_eig_v,options);
+
+k = 1;imagesc(reshape(v(:,k).*D,nc,nr)');colorbar
+
+
+end
+
+
+
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_text.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_text.m
new file mode 100755
index 0000000..4cc5759
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/test_text.m
@@ -0,0 +1,434 @@
+
+
+%case = 1;
+
+read_flag = 1;
+compute_flag = 0;
+load_flag = 0;
+decomp_flag = 0;
+hist_flag = 0;
+
+test_real = 0;
+
+
+if read_flag,
+if caseid == 1,
+    ifn = 'images/130049.pgm';
+elseif caseid == 2,
+    ifn = 'images/130055.pgm';
+elseif caseid == 3,
+    ifn = 'images/130056.pgm';
+elseif caseid == 4,
+    ifn = 'images/130057.pgm';
+elseif caseid == 5,
+    ifn = 'images/130060.pgm';
+elseif caseid == 6,
+    ifn = 'images/130061.pgm';
+elseif caseid == 7,
+    ifn = 'images/130062.pgm';
+elseif caseid == 8,
+    ifn = 'images/130065.pgm';
+elseif caseid == 9,
+    ifn = 'images/130066.pgm';
+elseif caseid == 10,
+    ifn = 'images/130068.pgm';
+elseif caseid == 11,
+    ifn = 'images/130070.pgm';
+else
+    ifn = 'images/130070.pgm';
+end
+
+I = readpgm(ifn);
+figure(1);
+imagesc(I);colormap(gray);drawnow;
+axis('tightequal');
+
+end
+
+%%%%% load %%%
+
+if load_flag,
+  fn = sprintf('load cresult_%d;',caseid);
+  eval(fn);
+end
+
+
+%%%%%%%%%%%%% compute %%%%%%%%%%%
+sig = 0.5;
+r = 3;
+sz = 15;
+Iw = cutoff(I,0.5*sz);
+figure(1);imagesc(Iw);
+axis('image');
+
+if compute_flag,
+
+as = 0:30:150;
+
+Cresult = [];
+
+for j = 1:length(as),
+    fprintf('.');
+    angle = as(j);
+
+    g = doog2(  sig,r,angle,sz);
+
+    g = g - mean(reshape(g,prod(size(g)),1));
+
+    g = g/sum(sum(abs(g)));
+
+    c = conv2(I,g,'valid');
+
+    Cresult(:,:,j) = c;
+end
+
+
+fprintf('\n');
+
+
+figure(2);
+
+subplot(2,3,1);
+imagesc(Cresult(:,:,1).^2);axis('tightequal');colorbar
+
+subplot(2,3,2);
+imagesc(Cresult(:,:,2).^2);axis('tightequal');colorbar
+
+subplot(2,3,3);
+imagesc(Cresult(:,:,3).^2);axis('tightequal');colorbar
+
+subplot(2,3,4);
+imagesc(Cresult(:,:,4).^2);axis('tightequal');colorbar
+
+subplot(2,3,5);
+imagesc(Cresult(:,:,5).^2);axis('tightequal');colorbar
+
+subplot(2,3,6);
+imagesc(Cresult(:,:,6).^2);axis('tightequal');colorbar
+
+Cs = [];
+Mcs = [];
+for j=1:length(as),
+    Cs(:,:,j) = reduce(reduce(abs(Cresult(:,:,j))));
+
+    Mcs = [Mcs,max(max(Cs(:,:,j)))];
+    
+end
+
+ms = max(Mcs);
+
+figure(3);
+for j=1:6,
+    fn = sprintf('Cs(:,:,%d) = Cs(:,:,%d)/ms;',j,j);
+    eval(fn);
+    fn = sprintf('subplot(2,3,%d);imagesc(Cs(:,:,%d));',j,j);
+    eval(fn);axis('tightequal');colorbar
+end
+
+fn = sprintf('save cresult_%d.mat Cresult Cs',caseid);
+disp(fn);
+eval(fn);
+
+end
+
+%%%%%%%%%%%%%%%%% decomp %%%%%%%%%%%%
+
+
+if decomp_flag,
+
+%writepnm5('txt_2.pnm',Cs);
+
+%writepnm5('130068.pnm',Cs);
+
+
+%I_scale = 0.0025;
+%X_scale = 3^2;
+%[A,D,Ipara] = compute_A_sparmul2(10,I_scale,X_scale);
+
+
+I_scale = 0.02;
+X_scale = 2;
+[A,D,Ipara] = compute_A_pnm('130068.pnm',[X_scale,I_scale]);
+
+nr = Ipara(1);nc = Ipara(2);
+imagesc(reshape(D,nc,nr)');colorbar;
+
+B = A+A';clear A;
+
+options.tol = 1e-7;
+
+[v,d] = eigs(B,9,options);
+
+figure(4);
+k = 1; imagesc(reshape(v(:,k).*D,nc,nr)'); 
+
+end
+
+
+%%%%  histogram %%%%
+
+%hist_flag = 1;
+
+%figure(1);imagesc(Iw);axis('image');
+if hist_flag ==1,
+
+
+ws = [12,12];
+
+figure(7);
+
+cs = ginput(1);
+
+cs = 10*(cs-1)+w/2;
+
+%cs(1,:) = w+(floor((cs(1,:)-w)/gap)*gap);
+%cs(2,:) = w+(floor((cs(2,:)-w)/gap)*gap);
+
+J = get_win(Iw,cs(1,:),ws);
+Jbar = get_win5(Cresult,cs(1,:),ws);
+
+
+figure(2);
+subplot(3,3,1);imagesc(J);colorbar
+for j=1:6,subplot(3,3,1+j);imagesc(abs(Jbar(:,:,j)));colorbar; end
+
+[hists,bins] = get_hist(J,Jbar);show_hist(hists,bins,4);
+cumhists = get_cumhist(hists);show_cumhist(cumhists,bins,6,1,'b-o');
+
+J2 = get_win(Iw,cs(2,:),ws);
+Jbar2 = get_win5(Cresult,cs(2,:),ws);
+
+figure(3);
+subplot(3,3,1);imagesc(J2);colorbar
+for j=1:6,subplot(3,3,1+j);imagesc(abs(Jbar2(:,:,j)));colorbar; end
+
+[hists2,bins2] = get_hist(J2,Jbar2);show_hist(hists2,bins2,5);
+cumhists2 = get_cumhist(hists2);show_cumhist(cumhists2,bins2,6,0,'r-*');
+
+diff.inten = max(abs(cumhists.inten-cumhists2.inten));
+diff.mag   = max(abs(cumhists.mag-cumhists2.mag));
+diff.text  = max(max(abs(cumhists.text-cumhists2.text)));
+
+figure(7);
+
+disp([diff.inten,diff.mag,diff.text]);
+maxdiff = max([diff.inten,diff.mag,diff.text]);
+disp(1-sigmoid(diff.inten,0.22,0.02));
+
+
+if 0,
+%A = pair_dist_text(Iw,Cresult,15);
+
+r =4;w = 22;gap = 5;sig_x= 20.0;
+inpara = [r,w,gap,sig_x,0.16,0.2,0.2];
+[Cum,tm] = cAh(Iw,mag,abs(Cresult),inpara);
+
+[Cum,Nb,Nc] = cAh4(Iw,mag,abs(Cresult),inpara);
+
+
+B = A+ A';clear A;
+
+figure(1);
+c = ginput(1);
+cx = floor(c(1)/gap);
+cy = floor(c(2)/gap);
+[cx,cy]
+figure(7)
+imagesc(reshape(B(cy*Cum(1)+cx,:),Cum(1),Cum(2))');colorbar
+
+
+cutoff = [0.22,0.2,0.2];
+sig_hist = [0.02,0.04,0.05];
+
+inpara2 = [r,5,cutoff,sig_hist];
+[A,D] = compute_A_hist3(tm,Cum,inpara2);
+
+B = A+A';clear A;
+imagesc(reshape(D,Cum(1),Cum(2))');
+
+
+[v,d] = eigs(B);
+
+
+end
+
+end
+
+%%%%%%%%%%%%%  trans_texture %%%%%%%%%%%%
+trans_text = 0;
+
+
+if trans_text,
+  figure(1);
+  cs = ginput(1);
+
+  ws = [40,40];
+
+  J = get_win(Iw,cs(1,:),ws);
+  Jbar = get_win5(Cresult,cs(1,:),ws);
+  Jmag = get_win(mag,cs(1,:),ws);
+
+  figure(3);imagesc(J);
+
+  figure(3);
+  for j=1:6,
+    subplot(2,3,j);imagesc(abs(Jbar(:,:,j)));axis('image');colorbar;
+  end
+
+  f= abs(Jbar(40,38,:));
+  g= abs(Jbar(40,47,:));
+
+  dot(f,g)/max(dot(f,f),dot(g,g))
+
+  ff = myinterp(f,10);  gg = myinterp(g,10);
+  dot(ff,gg)/max(dot(ff,ff),dot(gg,gg))
+
+  cum = mc_corr(ff,gg,[-6,6]);
+  max(cum)/max(dot(f,f),dot(g,g))
+
+
+  center = [40,35];
+
+  f = squeeze(abs(Jbar(center(1),center(2),:)));
+  ff = myinterp(f,10);
+  mag_ff = dot(ff,ff);
+  mag_c = Jmag(center(1),center(2));
+  dy = 0;
+
+  cor_cofs = [];
+  mags = [];
+  for dx = -15:15,
+     g = squeeze(abs(Jbar(center(1)+dy,center(2)+dx,:)));
+     gg = myinterp(g,10);
+
+     cum = mc_corr(ff,gg,[-6,6]);
+     cor_cofs = [cor_cofs,max(cum)/max(mag_ff,dot(gg,gg))];
+
+     mags =[mags,max(mag_c,Jmag(center(1)+dy,center(2)+dx,:))];
+
+  end
+
+  simulation_on =0;
+
+  if simulation_on,
+
+     sz = [161,161]
+     SI = zeros(sz);
+
+     for i=2:18:sz(1),
+       SI(i:i+2,:) = 1+SI(i:i+2,:);
+     end
+
+     imagesc(SI);axis('image');
+
+     tmp1 = mimrotate(SI,90,'nearest','crop');
+     tmp2 = mimrotate(SI,45,'nearest','crop');
+
+     ly = round(0.7*sz(1));
+     lx = round(0.7*sz(1));
+     sy = round(0.16*sz(1));
+     sx = round(0.2*sz(2));
+     TI = [tmp1(1:ly,1:lx),tmp2(sy+1:sy+ly,sy+1:sy+round(0.4*lx))];
+
+     TI = TI+0.04*randn(size(TI));
+
+     %sig = 1/sqrt(2);r = 3;sz = round(r*3*sig);
+
+     sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);
+     [text_des,TIw] = compute_filter(TI,sigs,r,szs);
+     figure(2);imagesc(TIw);axis('image');
+
+     figure(3);
+     im5(abs(text_des),2,3);
+
+     text_des = abs(text_des);
+
+     text_des = T1;
+
+     numband = size(text_des,3); r = 10;
+     sig_x = 90; sig_inten = 0.15; sig_tex = 0.01;w_inten = 0.03;
+     para = [numband,r,sig_x,sig_inten,sig_tex,w_inten];
+
+     [A,D,Ipara] = compute_A_text(TIw,text_des,para);
+     nr = Ipara(1);nc = Ipara(2);
+     B = A+A'; clear A;
+
+     figure(2);
+     cs = ginput(1);
+     cs = round(cs);id = cs(2)*nc+cs(1);
+
+     figure(4);
+     imagesc(reshape(B(id,:),nc,nr)');axis('image');colorbar
+
+     [v,d] = eigs(B);
+     figure(4);imagesc(reshape(D.*v(:,1),nc,nr)');axis('image');
+
+  end
+
+end
+
+if test_real == 1,
+    sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);
+    text_des = compute_filter(I,sigs,r,szs);
+
+    text_des = abs(text_des);
+%save filter_3.mat
+
+    %%%% cutoff margins,
+    margin = 6+10;
+
+    Iw = cutoff(I,margin);
+
+    T1= reshape(text_des,size(text_des,1),size(text_des,2),size(text_des,3)*size(text_des,4));
+    T1 = cutoff(T1,margin);
+
+    %%%%% reduce resolution
+
+    Iwp = compact(Iw,4);
+
+    T1 = reduce_all(T1);
+    T1 = reduce_all(T1);
+
+%    T1 = T1/70;
+
+   % writepnm5('test6_image.pnm',Iwp);writepnm5('test6_filter.pnm',T1);
+
+    numband = size(T1,3); r = 2;
+    sig_x = 20; sig_inten = 0.15; sig_tex = 0.01;w_inten = 0.01;
+    para = [numband,r,sig_x,sig_inten,sig_tex,w_inten];
+
+    [A,D,Ipara] = compute_A_text(Iwp,T1,para);
+    nr = Ipara(1);nc = Ipara(2);
+    figure(4);imagesc(reshape(D,nc,nr)');axis('image');
+   drawnow;
+
+
+    numband = 6;
+    r = 5;  sig_x = 20.0;
+    sig_tex = 0.01; w_inten = 0.01; w = 2;
+    para = [numband,r,sig_x,sig_tex,w_inten,w,size(T1,2)];
+
+    [A,D,Ipara] = compute_A_text2(Iw,T1(:,:,1:numband)/70,para);
+    nr = Ipara(1);nc = Ipara(2);
+
+
+
+    B = A+A'; clear A;
+
+
+     figure(2);
+     cs = ginput(1);
+     cs = floor(cs/4)+1;id = cs(2)*nc+cs(1);
+
+     figure(4);
+     imagesc(reshape(B(id,:),nc,nr)');axis('image');colorbar
+
+    
+end
+
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp.m
new file mode 100755
index 0000000..b932912
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp.m
@@ -0,0 +1,68 @@
+
+sw = 3;
+gap = 2*sw+1;
+
+nw = 6;
+
+for j=1:20,
+   l = max(0,j-1-nw);
+%    l = max(0,j-1-2*nw);
+   rs(j) = ceil((l-sw)/gap) + 1;
+   l = min(20,j-1+nw);
+%    l = min(20,j-1);
+   re(j) = floor((l-sw)/gap) +1;
+end
+
+plot([1:20],rs,'p-',[1:20],re,'rp-')
+
+
+%%%%%%%%
+
+bin_max = 1.0;
+bin_min = -1.0;
+num_bin = 30;
+sig = 0.2;
+
+data = 0.482;
+
+inc = (bin_max-bin_min)/num_bin;
+
+bs = -100;
+be = bin_min+inc;
+b = [];
+
+for j=1:num_bin,
+  
+  b(j) = tmp1(bs,be,data,sig);
+  bs = be;
+  be= be+inc;
+end
+plot(b,'p-');
+
+
+
+bmin = -1;
+
+inc = 0.2;
+a = 0.1;
+b = -1250;
+ovs = 625;
+
+bs = bmin;
+be = bs+inc;
+
+data = -0.482;
+
+for j=1:10,
+  tmp = bs-data;
+  fs = exp(-(tmp*tmp*ovs));
+  ks = b*tmp;
+
+  tmp = be-data;
+  fe = exp(-(tmp*tmp*ovs));
+  ke = b*tmp;
+
+  bin(j) = fs*(2+a*ks) + fe*(2-a*ke);
+  bs = be;
+  be = be+inc;
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp1.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp1.m
new file mode 100755
index 0000000..db5dbc1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp1.m
@@ -0,0 +1,25 @@
+function d = tmp1(bs,be,data,sig)
+
+sig = sig^2;
+
+if 1,
+a = (bs+be)*0.5;
+d = (a-bs)*(exp(-(bs-data)^2/sig) + exp(-(a-data)^2/sig)) + ...
+    (be-a)*(exp(-(a-data)^2/sig) + exp(-(be-data)^2/sig));
+d = d*2/sqrt(pi);
+else
+
+a = (be-bs)/2;
+
+h1 = exp(-(bs-data)^2/sig);
+h2 = exp(-(be-data)^2/sig);
+
+k1 = -2*(bs-data)/sig;
+k2 = -2*(be-data)/sig;
+
+d = a*(h1*(2+2*a*k1) + h2*(2-2*a*k2));
+d = d*2/sqrt(pi);
+
+end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp2.m
new file mode 100755
index 0000000..b361cdc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp2.m
@@ -0,0 +1,17 @@
+function d = tmp2(bs,be,data,sig)
+
+sig = sig^2;
+
+
+a = (be-bs)/2;
+
+h1 = exp(-(bs-data)^2/sig);
+h2 = exp(-(be-data)^2/sig);
+
+k1 = -2*(bs-data)/sig;
+k2 = -2*(be-data)/sig;
+
+d = (h1*(2+2*a*k1) + h2*(2-2*a*k2));
+%d = a*d*2/sqrt(pi);
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp3.m
new file mode 100755
index 0000000..c1bffd9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/tmp3.m
@@ -0,0 +1,126 @@
+function result = erfcore(x,jint)
+%ERFCORE Core algorithm for error functions.
+%   erf(x) = erfcore(x,0)
+%   erfc(x) = erfcore(x,1)
+%   erfcx(x) = exp(x^2)*erfc(x) = erfcore(x,2)
+
+%   C. Moler, 2-1-91.
+%   Copyright (c) 1984-96 by The MathWorks, Inc.
+%   $Revision: 5.7 $  $Date: 1996/10/28 20:57:59 $
+
+%   This is a translation of a FORTRAN program by W. J. Cody,
+%   Argonne National Laboratory, NETLIB/SPECFUN, March 19, 1990.
+%   The main computation evaluates near-minimax approximations
+%   from "Rational Chebyshev approximations for the error function"
+%   by W. J. Cody, Math. Comp., 1969, PP. 631-638.
+
+    if ~isreal(x),
+       error('Input argument must be real.')
+    end
+    result = repmat(NaN,size(x));
+%
+%   evaluate  erf  for  |x| <= 0.46875
+%
+    xbreak = 0.46875;
+    k = find(abs(x) <= xbreak);
+    if ~isempty(k)
+        a = [3.16112374387056560e00; 1.13864154151050156e02;
+             3.77485237685302021e02; 3.20937758913846947e03;
+             1.85777706184603153e-1];
+        b = [2.36012909523441209e01; 2.44024637934444173e02;
+             1.28261652607737228e03; 2.84423683343917062e03];
+
+            y = abs(x(k));
+            z = y .* y;
+            xnum = a(5)*z;
+            xden = z;
+            for i = 1:3
+               xnum = (xnum + a(i)) .* z;
+               xden = (xden + b(i)) .* z;
+            end
+            result(k) = x(k) .* (xnum + a(4)) ./ (xden + b(4));
+            if jint ~= 0, result(k) = 1 - result(k); end
+            if jint == 2, result(k) = exp(z) .* result(k); end
+    end
+%
+%   evaluate  erfc  for 0.46875 <= |x| <= 4.0
+%
+    k = find((abs(x) > xbreak) &  (abs(x) <= 2.));
+    if ~isempty(k)
+        c = [5.64188496988670089e-1; 8.88314979438837594e00;
+             6.61191906371416295e01; 2.98635138197400131e02;
+             8.81952221241769090e02; 1.71204761263407058e03;
+             2.05107837782607147e03; 1.23033935479799725e03;
+             2.15311535474403846e-8];
+        d = [1.57449261107098347e01; 1.17693950891312499e02;
+             5.37181101862009858e02; 1.62138957456669019e03;
+             3.29079923573345963e03; 4.36261909014324716e03;
+             3.43936767414372164e03; 1.23033935480374942e03];
+
+            y = abs(x(k));
+            xnum = c(9)*y;
+            xden = y;
+            for i = 1:7
+               xnum = (xnum + c(i)) .* y;
+               xden = (xden + d(i)) .* y;
+            end
+            result(k) = (xnum + c(8)) ./ (xden + d(8));
+            if jint ~= 2
+               z = fix(y*16)/16;
+               del = (y-z).*(y+z);
+               result(k) = exp(-z.*z) .* exp(-del) .* result(k);
+            end
+    end
+%
+%   evaluate  erfc  for |x| > 4.0
+%
+    k = find(abs(x) > 2.0);
+    if ~isempty(k)
+if 0,
+        p = [3.05326634961232344e-1; 3.60344899949804439e-1;
+             1.25781726111229246e-1; 1.60837851487422766e-2;
+             6.58749161529837803e-4; 1.63153871373020978e-2];
+        q = [2.56852019228982242e00; 1.87295284992346047e00;
+             5.27905102951428412e-1; 6.05183413124413191e-2;
+             2.33520497626869185e-3];
+
+            y = abs(x(k));
+            z = 1 ./ (y .* y);
+            xnum = p(6).*z;
+            xden = z;
+            for i = 1:4
+               xnum = (xnum + p(i)) .* z;
+               xden = (xden + q(i)) .* z;
+            end
+            result(k) = z .* (xnum + p(5)) ./ (xden + q(5));
+            result(k) = (1/sqrt(pi) -  result(k)) ./ y;
+            if jint ~= 2
+               z = fix(y*16)/16;
+               del = (y-z).*(y+z);
+               result(k) = exp(-z.*z) .* exp(-del) .* result(k);
+               k = find(~isfinite(result));
+               result(k) = 0*k;
+            end
+end
+    result(k) = 0;
+    end
+%
+%   fix up for negative argument, erf, etc.
+%
+    if jint == 0
+            k = find(x > xbreak);
+            result(k) = (0.5 - result(k)) + 0.5;
+            k = find(x < -xbreak);
+            result(k) = (-0.5 + result(k)) - 0.5;
+    elseif jint == 1
+            k = find(x < -xbreak);
+            result(k) = 2. - result(k);
+    else  % jint must = 2 
+            k = find(x < -xbreak);
+            z = fix(x(k)*16)/16;
+            del = (x(k)-z).*(x(k)+z);
+            y = exp(z.*z) .* exp(del);
+            result(k) = (y+y) - result(k);
+    end
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/true_loc.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/true_loc.m
new file mode 100755
index 0000000..7bf060f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/true_loc.m
@@ -0,0 +1,23 @@
+function a = true_loc(loca,g,scale);
+
+if ~exist('scale'),
+  scale = 50;
+end
+
+y = loca(1,:);
+x = loca(2,:);
+
+min_x = min(x);
+min_y = min(y);
+
+x = x - min_x;
+y = y - min_y;
+
+max_x = max(x);max_y = max(y);
+min_scale = min(max_x,max_y);
+
+a(1) = (g(1)-1)*min_scale/(scale);
+a(2) = (g(2)-1)*min_scale/(scale);
+
+a(1) = a(1) + min_x;
+a(2) = a(2) + min_y;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/vmquant.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/vmquant.m
new file mode 100755
index 0000000..ab4eb28
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/vmquant.m
@@ -0,0 +1,112 @@
+function [im, map] = vmquant(arg1,arg2,arg3,arg4,arg5,arg6,arg7)
+%VMQUANT Variance Minimization Color Quantization.
+%       [X, MAP] = VMQUANT(R,G,B,K,[Qr Qg Qb],DITHER,Qe) or
+%       VMQUANT(RGB,K,[Qr Qg Qb],DITHER,Qe), where RGB is a 3-D array, 
+%       converts an arbitrary image comprised of RGB triples into an 
+%       indexed image X with color map MAP.  K specifies the number 
+%       of desired entries in the target color map, and [Qr Qg Qb] 
+%       specifies the number of quantization bits to assign each color 
+%       axis during color interpolation.  DITHER is a string ('dither' or
+%       'nodither') that indicates whether or not to perform error propagation 
+%       dither on the output image.  Qe specifies the number of bits of
+%       quantization used in the error calculations.
+%
+%       K is optional and defaults to 256.
+%       [Qr Qg Qb] is optional and defaults to [5 5 5].
+%       DITHER is optional and defaults to 'nodither'.
+%       Qe is optional and defaults to 8.
+%
+%       See also: RGB2IND, RGB2GRAY, DITHER, IND2RGB, CMUNIQUE, IMAPPROX.
+
+%       This is the wrapper function for the MEX file VMQUANTC.C
+
+%       Joseph M. Winograd 6-93
+%       Copyright (c) 1993 by The MathWorks, Inc.
+%       $Revision: 5.3 $  $Date: 1996/08/22 22:09:03 $
+
+%       Reference: Xiaolin Wu, "Efficient Statistical Computation for
+%       Optimal Color Quantization," Graphics Gems II, (ed. James
+%       Arvo).  Academic Press: Boston. 1991.
+
+if nargin < 1,
+  error('Not enough input arguments.');
+end
+
+threeD = (ndims(arg1)==3); % Determine if input includes a 3-D array
+
+if threeD,
+  error( nargchk( 1, 5, nargin ) );
+
+                        % NOTE: If you change defaults, change them also
+                        %       in VMQUANTC.C and recompile the MEX function.
+  if nargin < 5
+      arg5 = 8;         % DEFAULT_QE = 8
+  end
+
+  if nargin < 4
+      arg4 = 'n';               % DEFAULT_DITHER = 0
+  end
+
+  if nargin < 3
+      arg3 = [5 5 5];   % DEFAULT_Q = [5 5 5]
+  end
+
+  if nargin < 2
+      arg2 = 256;               % DEFAULT_K = 256
+  end
+
+  rout = arg1(:,:,1);
+  g = arg1(:,:,2);
+  b = arg1(:,:,3); 
+
+  if strcmp(lower(arg4(1)),'d')
+    dith = 1;
+  else
+    dith = 0;
+  end
+  
+  arg7 = arg5;
+  arg5 = arg3;
+  arg4 = arg2;
+
+else
+  error( nargchk( 3, 7, nargin ) );
+
+  if nargin < 7
+      arg7 = 8;         % DEFAULT_QE = 8
+  end
+
+  if nargin < 6
+      arg6 = 'n';               % DEFAULT_DITHER = 0
+  end
+
+  if nargin < 5
+      arg5 = [5 5 5];   % DEFAULT_Q = [5 5 5]
+  end
+
+  if nargin < 4
+      arg4 = 256;               % DEFAULT_K = 256
+  end
+
+  rout = arg1;
+  g = arg2;
+  b = arg3;
+
+  if strcmp(lower(arg6(1)),'d')
+    dith = 1;
+  else
+    dith = 0;
+  end
+
+end
+
+if (~isa(rout,'uint8'))
+  rout = uint8(round(255*rout));
+end
+if (~isa(g,'uint8'))
+  g = uint8(round(255*g));
+end
+if (~isa(b,'uint8'))
+  b = uint8(round(255*b));
+end
+[im,map] = vmquantc( rout, g, b, arg4, arg5, dith, arg7 );
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm.m
new file mode 100755
index 0000000..915e07d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm.m
@@ -0,0 +1,26 @@
+function [L1,L2,phi,Txx,Txy,Tyy]=wismm(X,N);
+% [L1,L2,phi,T11,T12,T22]=wismm(X,N);
+% Calculate windowed image second moment matrices for image X and return
+% the following values:
+%
+% L1 is the larger eigenvalue (lambda_1)
+% L2 is the smaller eigenvalue (lambda_2)
+% phi is the angle of the 1st eigenvector (phi)
+
+[G1,G2]=gradient(X);
+
+GGTxx=G1.^2;
+GGTxy=G1.*G2;
+GGTyy=G2.^2;
+
+Txx=gaussN(GGTxx,N);
+Txy=gaussN(GGTxy,N);
+Tyy=gaussN(GGTyy,N);
+
+tr=Txx+Tyy;
+V1=0.5*sqrt(tr.^2-4*(Txx.*Tyy-Txy.^2));
+
+L1=real(0.5*tr+V1);
+L2=real(0.5*tr-V1);
+phi=0.5*atan2(2*Txy,Txx-Tyy);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm2.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm2.m
new file mode 100755
index 0000000..ae62ce9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm2.m
@@ -0,0 +1,66 @@
+function [L1,L2,phi,aniso,pol,con,window_sizes]=wismm2(V);
+% [L1,L2,phi,aniso,pol,con,window_sizes]=wismm2(V);
+% Calculate windowed image second moment matrices for image V and return
+% the following values:
+%
+% L1 is the larger eigenvalue (lambda_1)
+% L2 is the smaller eigenvalue (lambda_2)
+% phi is the angle of the 1st eigenvector (phi)
+%
+
+[Gx,Gy]=gradient(V);
+
+GGTxx=Gx.^2;
+GGTxy=Gx.*Gy;
+GGTyy=Gy.^2;
+
+[r,c]=size(V);
+
+min_window_size=3;
+max_window_size=3*round(min(r,c)/16);
+if (-1)^max_window_size==1
+   max_window_size=max_window_size+1;
+end
+window_step_size=2;
+
+window_sizes=min_window_size:2:max_window_size;
+max_count=length(window_sizes);
+
+L1=zeros(r,c,max_count);
+L2=zeros(r,c,max_count);
+phi=zeros(r,c,max_count);
+pol=zeros(r,c,max_count);
+con=zeros(r,c,max_count);
+
+fprintf(1,'Integration window size: ');
+counter=1;
+for n=window_sizes
+   fprintf(1,'%d ',n);
+   Txx=gaussN(GGTxx,n);
+   Txy=gaussN(GGTxy,n);
+   Tyy=gaussN(GGTyy,n);
+   
+   tr=Txx+Tyy;
+   V1=0.5*sqrt(tr.^2-4*(Txx.*Tyy-Txy.^2));
+   V1=real(V1);
+   
+   L1(:,:,counter)=0.5*tr+V1;
+   L2(:,:,counter)=0.5*tr-V1;
+   phi(:,:,counter)=0.5*atan2(2*Txy,Txx-Tyy);
+   
+   % do polarity stuff here
+   grad_smooth_x=gaussN(Gx,n);
+   grad_smooth_y=gaussN(Gy,n);
+   grad_smooth_mag=sqrt(grad_smooth_x.^2+grad_smooth_y.^2);
+   grad_mag=sqrt(Gx.^2+Gy.^2);
+   grad_mag_smooth=gaussN(grad_mag,n);
+   pol(:,:,counter)=grad_smooth_mag./(grad_mag_smooth+eps);
+   
+   % contrast calculation
+   con(:,:,counter)=tr;
+   counter=counter+1;
+end
+fprintf(1,'\n')
+
+aniso=1-L2./(L1+eps);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm3.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm3.m
new file mode 100755
index 0000000..89c56ef
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/wismm3.m
@@ -0,0 +1,71 @@
+function [L1,L2,phi,aniso,pol,con,window_sizes]=wismm3(V);
+% [L1,L2,phi,aniso,pol,con,window_sizes]=wismm3(V);
+% Calculate windowed image second moment matrices for image V and return
+% the following values:
+%
+% L1 is the larger eigenvalue (lambda_1)
+% L2 is the smaller eigenvalue (lambda_2)
+% phi is the angle of the 1st eigenvector (phi)
+%
+
+[Gx,Gy]=gradient(V);
+
+GGTxx=Gx.^2;
+GGTxy=Gx.*Gy;
+GGTyy=Gy.^2;
+
+[r,c]=size(V);
+
+min_window_size=3;
+max_window_size=3*round(min(r,c)/16);
+if (-1)^max_window_size==1
+   max_window_size=max_window_size+1;
+end
+window_step_size=2;
+
+window_sizes=min_window_size:2:max_window_size;
+max_count=length(window_sizes);
+
+L1=zeros(r,c,max_count);
+L2=zeros(r,c,max_count);
+phi=zeros(r,c,max_count);
+pol=zeros(r,c,max_count);
+con=zeros(r,c,max_count);
+
+fprintf(1,'Integration window size: ');
+counter=1;
+for n=window_sizes
+   fprintf(1,'%d ',n);
+   Txx=gaussN(GGTxx,n);
+   Txy=gaussN(GGTxy,n);
+   Tyy=gaussN(GGTyy,n);
+   
+   tr=Txx+Tyy;
+   V1=0.5*sqrt(tr.^2-4*(Txx.*Tyy-Txy.^2));
+   V1=real(V1);
+   
+   L1(:,:,counter)=0.5*tr+V1;
+   L2(:,:,counter)=0.5*tr-V1;
+   phi(:,:,counter)=0.5*atan2(2*Txy,Txx-Tyy);
+
+   % do polarity stuff here
+   [P,angle_vector]=polarity(Gx,Gy,n);
+   quant_bound=angle_vector(2)/2;
+   % (quantize angle and pull corresponding polarity out of P)
+   % (perhaps use set-theoretic functions for masking out P???)
+   for m=1:length(angle_vector);
+      a=angle_vector(end-m+1);
+      old_pol=pol(:,:,counter);
+      Pmask=abs(cos(phi(:,:,counter)-a))>=cos(quant_bound);
+      Pmask=Pmask&(old_pol==0); % prevent pileup on quant. boundaries
+      pol(:,:,counter)=old_pol+(Pmask.*P(:,:,m));
+   end
+   
+   % contrast calculation
+   con(:,:,counter)=tr;
+   counter=counter+1;
+end
+fprintf(1,'\n')
+
+aniso=1-L2./(L1+eps);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_command.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_command.m
new file mode 100755
index 0000000..954a39e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_command.m
@@ -0,0 +1,8 @@
+function write_command(fname,fn_base,para)
+
+fid = fopen(fname,'w');
+
+fprintf(fid,'%s ',fn_base);
+fprintf(fid,'%d ',para);
+fprintf(fid,'\nrun\n');
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_test.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_test.m
new file mode 100755
index 0000000..e444c27
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/write_test.m
@@ -0,0 +1,38 @@
+
+I_max = 250;
+tex_max = 30;
+
+%fnames = [130038,130039,130042,130056,130057];
+%fnames = [334074 334031 334044 334003 334065 334000 334039 334018 334002 334029]
+
+fnames = 130057;
+
+for j=1:length(fnames),
+fname = sprintf('images/%d.pgm',fnames(j));
+
+sigs = [1/sqrt(2),1,sqrt(2),2,2*sqrt(2)];r = 3;szs = round(r*3*sigs);  
+szs = szs(length(szs))*ones(1,length(szs));
+num_ori = 6;
+
+I = readpgm(fname);
+[text_des,filters] = compute_filter_fft(I,sigs,r,szs,num_ori); 
+
+outname = sprintf('plaatje_data/%d',fnames(j));
+
+cutsz =20;
+I = cutoff(I,cutsz);%figure(1);im(I);
+text_des = cutoff(text_des,cutsz);
+
+writeout_feature(I,text_des(:,:,:),outname,I_max,tex_max);
+
+
+if 0,
+for j=0:30,
+  cm = sprintf('!mv plaatje_data/134013.pfm_%d.pfm plaatje_data/134013_%d.pfm',j,j);
+  disp(cm);eval(cm);
+end
+end
+
+end
+
+exit
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writeout_feature.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writeout_feature.m
new file mode 100755
index 0000000..5376d5f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writeout_feature.m
@@ -0,0 +1,40 @@
+function writeout_feature(I,tex,fname,I_max,tex_max)
+%
+%   writeout_feature(I,tex,fname,I_max,tex_max)
+%
+%
+
+
+%%%%% print out image 
+I_min = min(I(:));
+
+I = I-I_min;
+I = min(1,I/(I_max-I_min));
+
+I = 2*I-1;
+
+j = 0;
+fn = sprintf('%s_%d.pfm',fname,j);
+cm = sprintf('writepfm: I->%s',fn);
+disp(cm);
+writepfm(fn,I);
+
+
+%%% print out texture
+nf = size(tex,3)
+
+for j=1:nf,
+  
+  fn = sprintf('%s_%d.pfm',fname,j);
+  cm = sprintf('writepfm:tex_%d->%s',j,fn);
+  disp(cm);
+
+tex(:,:,j) = tex(:,:,j)/tex_max;fprintf('.');
+tex(:,:,j) = tex(:,:,j).*(tex(:,:,j)<=1) + 1*(tex(:,:,j)>1);fprintf('.')
+tex(:,:,j) = tex(:,:,j).*(tex(:,:,j)>=-1) + (-1)*(tex(:,:,j)<-1);fprintf('.');
+
+  writepfm(fn,tex(:,:,j));
+
+
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepfm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepfm.m
new file mode 100755
index 0000000..a831970
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepfm.m
@@ -0,0 +1,11 @@
+function writepfm(name,I)
+%
+%  writepfm(name,I)
+%
+  [nr,nc] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, '%d %d\n', nr,nc);
+  fprintf(fid,'%f ',I);
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepgm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepgm.m
new file mode 100755
index 0000000..113cb18
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepgm.m
@@ -0,0 +1,8 @@
+function I = writepgm(name,I)
+
+  [y,x] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, 'P5\n%d %d\n255\n', x,y);
+  fwrite(fid, I', 'uint8');
+  fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepmm.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepmm.m
new file mode 100755
index 0000000..675df93
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepmm.m
@@ -0,0 +1,14 @@
+function writepmm(name,I)
+%
+%   writepmm(name,I)
+%
+
+  [nr,nc,nb] = size(I);
+
+  fid = fopen(name,'w');
+
+  fprintf(fid, 'P5\n%d %d %d\n255\n', nc,nr,nb);
+
+  fprintf(fid,'%f ',I);
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepnm5.m b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepnm5.m
new file mode 100755
index 0000000..633fba9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filter_hist/writepnm5.m
@@ -0,0 +1,26 @@
+function writepnm5(name,I)
+%
+%   writepnm5(name,I)
+%
+%     I is a mul-band image
+%
+
+  [nr,nc,nb] = size(I);
+
+  fid = fopen(name,'w');
+
+  fprintf(fid, 'P5\n%d %d %d\n255\n', nc,nr,nb);
+
+  n = nr*nc;
+
+  J = [];
+
+  for j=1:nb,
+      J = [J,reshape(I(:,:,j)',n,1)];
+  end
+
+  J = reshape(J',nb*n,1);
+  
+  fprintf(fid,'%f ',J);
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog1.m b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog1.m
new file mode 100755
index 0000000..dd8e87b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog1.m
@@ -0,0 +1,32 @@
+function H=doog1(sig,r,th,N);
+% H=doog1(sig,r,th,N);
+
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+phi=pi*th/180;
+sigy=sig;
+sigx=r*sig;
+R=[cos(phi) -sin(phi); sin(phi) cos(phi)];
+C=R*diag([sigx,sigy])*R';
+
+X=[x(:) y(:)];
+
+Gb=gaussian(X,[0 0]',C);
+Gb=reshape(Gb,N,N);
+
+m=R*[0 sig]';
+
+a=1;
+b=-1;
+
+% make odd-symmetric filter
+Ga=gaussian(X,m/2,C);
+Ga=reshape(Ga,N,N);
+Gb=rot90(Ga,2);
+H=a*Ga+b*Gb; 
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog2.m b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog2.m
new file mode 100755
index 0000000..a0511cb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/doog2.m
@@ -0,0 +1,38 @@
+function G=doog2(sig,r,th,N);
+% G=doog2(sig,r,th,N);
+% Make difference of offset gaussians kernel
+% theta is in degrees
+% (see Malik & Perona, J. Opt. Soc. Amer., 1990)
+%
+% Example:
+% >> imagesc(doog2(1,12,0,64,1))
+% >> colormap(gray)
+
+% by Serge Belongie
+
+no_pts=N;  % no. of points in x,y grid
+
+[x,y]=meshgrid(-(N/2)+1/2:(N/2)-1/2,-(N/2)+1/2:(N/2)-1/2);
+
+phi=pi*th/180;
+sigy=sig;
+sigx=r*sig;
+R=[cos(phi) -sin(phi); sin(phi) cos(phi)];
+C=R*diag([sigx,sigy])*R';
+
+X=[x(:) y(:)];
+
+Gb=gaussian(X,[0 0]',C);
+Gb=reshape(Gb,N,N);
+
+m=R*[0 sig]';
+Ga=gaussian(X,m,C);
+Ga=reshape(Ga,N,N);
+Gc=rot90(Ga,2);
+
+a=-1;
+b=2;
+c=-1;
+
+G = a*Ga + b*Gb + c*Gc;
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_even2.m b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_even2.m
new file mode 100755
index 0000000..f7f4527
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_even2.m
@@ -0,0 +1,45 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz,enlong)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+if nargin<4,
+    enlong = 3;
+end
+
+enlong = 2*enlong;
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog2(filter_scales(m),enlong,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_odd2.m b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_odd2.m
new file mode 100755
index 0000000..0059dca
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/make_filterbank_odd2.m
@@ -0,0 +1,46 @@
+function FB = make_filterbank(num_ori,filter_scales,wsz,enlong)
+%
+%  F = make_filterbank(num_ori,num_scale,wsz)
+%
+
+if nargin<4,
+    enlong = 3;
+end
+
+enlong = enlong*2;
+
+% definine filterbank
+%num_ori=6;
+%num_scale=3;
+
+num_scale = length(filter_scales);
+
+M1=wsz; % size in pixels
+M2=M1;
+
+ori_incr=180/num_ori;
+ori_offset=ori_incr/2; % helps with equalizing quantiz. error across filter set
+
+FB=zeros(M1,M2,num_ori,num_scale);
+
+
+% elongated filter set
+counter = 1;
+
+for m=1:num_scale  
+   for n=1:num_ori
+      % r=12 here is equivalent to Malik's r=3;
+      f=doog1(filter_scales(m),enlong,ori_offset+(n-1)*ori_incr,M1);
+      FB(:,:,n,m)=f;
+   end
+end
+
+FB=reshape(FB,M1,M2,num_scale*num_ori);
+total_num_filt=size(FB,3);
+
+for j=1:total_num_filt,
+  F = FB(:,:,j);
+  a = sum(sum(abs(F)));
+  FB(:,:,j) = FB(:,:,j)/a;
+end
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/quadedgep2.m b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/quadedgep2.m
new file mode 100755
index 0000000..5041377
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/filtersQuad/quadedgep2.m
@@ -0,0 +1,188 @@
+% function [xs,ys,gx,gy,par,threshold,mag,mage,g,FIe,FIo,mago] = quadedgep(I,par,threshold);

+% Input:

+%    I = image

+%    par = vector for 4 parameters

+%      [number of filter orientations, number of scale, filter size, elongation]

+%      To use default values, put 0.

+%    threshold = threshold on edge strength

+% Output:

+%    [x,y,gx,gy] = locations and gradients of an ordered list of edgels

+%       x,y could be horizontal or vertical or 45 between pixel sites

+%       but it is guaranteed that there [floor(y) + (floor(x)-1)*nr] 

+%       is ordered and unique.  In other words, each edgel has a unique pixel id.

+%    par = actual par used

+%    threshold = actual threshold used

+%    mag = edge magnitude

+%    mage = phase map

+%    g = gradient map at each pixel

+%    [FIe,FIo] = odd and even filter outputs

+%    mago = odd filter output of optimum orientation

+

+% Stella X. Yu, 2001

+

+% This is the multi scale version of the filtering

+% For the moment the parameters are defined by default. See line 34

+

+

+function [x,y,gx,gy,par,threshold,mag_s,mage,g,FIe,FIo,mago] = quadedgep2(I,par,data,threshold);

+

+

+if nargin<4 | isempty(threshold),

+    threshold = 0.1;

+end

+

+[r,c] = size(I);

+def_par = [4,30,3];

+

+display_on = 1;

+

+% take care of parameters, any missing value is substituted by a default value

+if nargin<2 | isempty(par),

+   par = def_par;

+end

+% par(end+1:4)=0;

+% par = par(:);

+% j = (par>0);

+% have_value = [ j, 1-j ];

+% j = 1; n_filter = have_value(j,:) * [par(j); def_par(j)];

+% j = 2; n_scale  = have_value(j,:) * [par(j); def_par(j)];

+% j = 3; winsz    = have_value(j,:) * [par(j); def_par(j)];

+% j = 4; enlong   = have_value(j,:) * [par(j); def_par(j)];

+

+n_ori = par(1);      %if it doesn't work, par<-def_par

+

+winsz = par(2);

+enlong = par(3);

+

+% always make filter size an odd number so that the results will not be skewed

+j = winsz/2;

+if not(j > fix(j) + 0.1),

+    winsz = winsz + 1;

+end

+

+% filter the image with quadrature filters 

+if (isempty(data.W.scales))

+        error ('no scales entered');

+end

+

+n_scale=length(data.W.scales);

+filter_scales=data.W.scales;

+%     

+% if strcmp(data.dataWpp.mode,'multiscale')

+%     n_scale=size((data.dataWpp.scales),2);

+%     filter_scales=data.dataWpp.scales;

+% else

+%     filter_scales=data.dataWpp.scales(1);

+%     n_scale=1;

+% end

+% if n_scale>0&&strcmp(data.dataWpp.mode,'multiscale')

+%     if (~isempty(data.dataWpp.scales))

+%         filter_scales=data.dataWpp.scales;

+%     else

+%         filter_scales=zeros(1,n_scale);

+%         

+%         for i=1:n_scale,

+%         filter_scales(i)=(sqrt(2))^(i-1);

+%         end

+%         data.dataWpp.scales=filter_scales;

+%     end

+% else filter_scale=1;

+%     data.dataWpp.scales=filter_scales;

+% end

+% 

+% %%%%%%% juste pour que ca tourne

+% if ~strcmp(data.dataWpp.mode,'multiscale')

+%     filter_scales=data.dataWpp.scales(1);

+%     n_scale=4;

+% end

+% %%%%%%%%%%%%

+

+FBo = make_filterbank_odd2(n_ori,filter_scales,winsz,enlong);

+FBe = make_filterbank_even2(n_ori,filter_scales,winsz,enlong);

+n = ceil(winsz/2);

+f = [fliplr(I(:,2:n+1)), I, fliplr(I(:,c-n:c-1))];

+f = [flipud(f(2:n+1,:)); f; flipud(f(r-n:r-1,:))];

+FIo = fft_filt_2(f,FBo,1); 

+FIo = FIo(n+[1:r],n+[1:c],:);

+FIe = fft_filt_2(f,FBe,1);

+FIe = FIe(n+[1:r],n+[1:c],:);

+

+% compute the orientation energy and recover a smooth edge map

+% pick up the maximum energy across scale and orientation

+% even filter's output: as it is the second derivative, zero cross localize the edge

+% odd filter's output: orientation

+

+[nr,nc,nb] = size(FIe);

+

+FIe = reshape(FIe, nr,nc,n_ori,length(filter_scales));

+FIo = reshape(FIo, nr,nc,n_ori,length(filter_scales));

+

+mag_s = zeros(nr,nc,n_scale);

+mag_a = zeros(nr,nc,n_ori);

+

+mage = zeros(nr,nc,n_scale);

+mago = zeros(nr,nc,n_scale);

+mage = zeros(nr,nc,n_scale);

+mago = zeros(nr,nc,n_scale);

+

+

+

+for i = 1:n_scale,

+    mag_s(:,:,i) = sqrt(sum(FIo(:,:,:,i).^2,3)+sum(FIe(:,:,:,i).^2,3));

+    mag_a = sqrt(FIo(:,:,:,i).^2+FIe(:,:,:,i).^2);

+    [tmp,max_id] = max(mag_a,[],3);

+ 

+    base_size = nr * nc;

+    id = [1:base_size]';

+    mage(:,:,i) = reshape(FIe(id+(max_id(:)-1)*base_size+(i-1)*base_size*n_ori),[nr,nc]);

+    mago(:,:,i) = reshape(FIo(id+(max_id(:)-1)*base_size+(i-1)*base_size*n_ori),[nr,nc]);

+    

+    mage(:,:,i) = (mage(:,:,i)>0) - (mage(:,:,i)<0);

+

+    if display_on, 

+    ori_incr=pi/n_ori; % to convert jshi's coords to conventional image xy

+    ori_offset=ori_incr/2;

+    theta = ori_offset+([1:n_ori]-1)*ori_incr; % orientation detectors

+    % [gx,gy] are image gradient in image xy coords, winner take all

+    

+    ori = theta(max_id);

+    ori = ori .* (mago(:,:,i)>0) + (ori + pi).*(mago(:,:,i)<0);

+    gy{i} = mag_s(:,:,i) .* cos(ori);

+    gx{i} = -mag_s(:,:,i) .* sin(ori);

+    g = cat(3,gx{i},gy{i});

+

+    % phase map: edges are where the phase changes

+    mag_th = max(max(mag_s(:,:,i))) * threshold;

+    eg = (mag_s(:,:,i)>mag_th);

+    h = eg & [(mage(2:r,:,i) ~= mage(1:r-1,:,i)); zeros(1,nc)];

+    v = eg & [(mage(:,2:c,i) ~= mage(:,1:c-1,i)), zeros(nr,1)];

+    [y{i},x{i}] = find(h | v);

+    k = y{i} + (x{i}-1) * nr;

+    h = h(k);

+    v = v(k);

+    y{i} = y{i} + h * 0.5; % i

+    x{i} = x{i} + v * 0.5; % j

+    t = h + v * nr;

+    gx{i} = g(k) + g(k+t);

+    k = k + (nr * nc);

+    gy{i} = g(k) + g(k+t);

+    

+%     figure(1);

+%     clf;

+%     imagesc(I);colormap(gray);

+%     hold on;

+%     quiver(x,y,gx,gy); hold off;

+%     title(sprintf('scale = %d, press return',i));

+    

+   % pause;

+    0;

+else

+    x = [];

+    y = [];

+    gx = [];

+    gy =[];

+     g= [];

+ end

+end

+

+

diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/convert422.m b/SD-VBS/common/toolbox/toolbox_basic/io/convert422.m
new file mode 100755
index 0000000..919e82e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/convert422.m
@@ -0,0 +1,27 @@
+image_current = '/hid/jshi';
+
+image_dir = 'vr05_5 ';
+pg_path = '/hid/jshi/422toppm/422toppm';
+
+cm = sprintf('cd %s',image_dir);
+disp(cm);
+eval(cm);
+
+d = dir('seq*');
+filename = char(sort({d.name}));
+
+for j=1:size(filename),
+ cm = sprintf('!%s %s',pg_path,deblank(filename(j,:)));
+disp(cm); 
+eval(cm);
+end
+
+
+%%% change back
+cm = sprintf('cd %s',image_current);
+disp(cm);eval(cm);
+
+
+if 0,
+  deblank(filename(f,:));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/im_vd.m b/SD-VBS/common/toolbox/toolbox_basic/io/im_vd.m
new file mode 100755
index 0000000..590cd9b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/im_vd.m
@@ -0,0 +1,6 @@
+function J = im_vd(I);
+
+J(:,:,1) = I(1:2:end,1:2:end);
+J(:,:,2) = I(2:2:end,1:2:end);
+
+montage2(J);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/imread2.m b/SD-VBS/common/toolbox/toolbox_basic/io/imread2.m
new file mode 100755
index 0000000..27a5e4b
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/imread2.m
@@ -0,0 +1,45 @@
+function I = imread2(fname,im_dir);
+%
+%  I = imread2(fname,im_dir);
+%
+
+cur_dir = pwd;
+
+if nargin>1,
+  cd(im_dir);
+end
+
+%%% put on the necessary extension
+d = dir(fname);
+
+if isempty(d),
+  d = dir([fname,'*']);
+end
+
+if isempty(d),
+  I = [];
+else
+
+  fname = d.name;
+  
+  %%% find extension
+  k = findstr(fname,'.');
+  ext = fname(k(end)+1:end);
+
+  if (ext == 'bz2'),
+    cm = sprintf('!bzip2 -d %s',fname);
+    disp(cm);eval(cm);
+    I = imread2(fname(1:k(end-1)-1));
+    cm = sprintf('!bzip2 %s',fname(1:k(end)-1));
+                 disp(cm);eval(cm);
+  elseif (ext == 'ppm');
+    I = readppm(fname);
+  elseif (ext == 'pgm');
+    I = readpgm(fname);
+  else
+    I = imread(fname);
+I = double(I)/255;
+  end
+end
+
+cd(cur_dir);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/peek_pgm_size.m b/SD-VBS/common/toolbox/toolbox_basic/io/peek_pgm_size.m
new file mode 100755
index 0000000..13e54cd
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/peek_pgm_size.m
@@ -0,0 +1,19 @@
+function [nr,nc] = peek_pgm_size(filename)
+% function [nr,nc] = peek_pgm_size(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');
+      nc = YX(1); nr = YX(2);
+   end
+end
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/pgmread.m b/SD-VBS/common/toolbox/toolbox_basic/io/pgmread.m
new file mode 100755
index 0000000..49a35a8
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/pgmread.m
@@ -0,0 +1,24 @@
+function [img,header] = pgmread(filename)
+%
+%  [img,header] = pgmread(filename)
+
+[fid, msg] = fopen(filename, 'r');
+if fid == -1,
+  error(msg)
+end
+
+head = [];
+good = 0;
+while (good == 0) ,
+  l = fgetl(fid);
+  if (length(l) == 3),
+   if (l == '255'),
+    good = 1;
+    sze = sscanf(header,'%d');
+   end
+  end
+  header= l;
+end
+
+img = fread(fid, sze', 'uchar')';
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/ppmtojpg.m b/SD-VBS/common/toolbox/toolbox_basic/io/ppmtojpg.m
new file mode 100755
index 0000000..ce47e45
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/ppmtojpg.m
@@ -0,0 +1,25 @@
+function []= ppm2jpg(fname,dlm,ori)
+%
+%  ppm2jpg(fname,dlm,ori)
+%      
+%     dlm =1, remove the file extension from fname
+%             before convert
+%     ori =1, transpose the image
+%
+
+if dlm,
+ dlm = findstr(fname,'.');
+ fname = fname(1:dlm(end)-1);
+end
+
+fname_1 = sprintf('%s.ppm',fname);
+I = readppm(fname_1);
+
+if ori == 1,
+  I = permute(I,[2 1 3]);
+end
+
+
+fname_2 = sprintf('%s.jpg',fname);
+imwrite(I,fname_2,'jpeg','Quality',90);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read422.m b/SD-VBS/common/toolbox/toolbox_basic/io/read422.m
new file mode 100755
index 0000000..31a27f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read422.m
@@ -0,0 +1,45 @@
+function I = read422(fname,nc);
+%
+%   I = read422(fname,width);
+%
+%      read in a .422 file, need to pass image width, default = 640
+%
+
+% assume image width = 640
+if nargin<2,
+  nc = 640;
+end
+
+%% find the image size
+fid = fopen(fname);
+fseek(fid,0,1);
+fsize = ftell(fid);
+
+nr = fsize/nc/2;
+fseek(fid,0,-1);
+
+%% read in Ybr data
+data = fread(fid,fsize,'uchar');
+
+%%% extract Y, Cb, Cr
+Y1 = data(1:2:end);  Y1 = reshape(Y1,nc,nr)';
+Cb1 = data(2:4:end); Cb1 = reshape(Cb1,nc/2,nr)';
+Cr1 = data(4:4:end); Cr1 = reshape(Cr1,nc/2,nr)';
+
+Cb = zeros(size(Y1)); 
+Cr = zeros(size(Y1));
+
+Cb(:,1:2:end) = Cb1;  Cb(:,2:2:end) = Cb1;
+%Cb(:,2:2:end) = 0.5*(Cb1+[Cb1(:,2:end),Cb1(:,end)]);
+
+Cr(:,1:2:end) = Cr1; Cr(:,2:2:end) = Cr1;
+%Cr(:,2:2:end) = 0.5*(Cr1+[Cr1(:,2:end),Cr1(:,end)]);
+
+%%% convert to r,g,b
+r = 1.164*(Y1-16.0) + 1.596*(Cr-128.0);
+g = 1.164*(Y1-16.0) - 0.813*(Cr-128.0) - 0.391*(Cb-128.0);
+b = 1.164*(Y1-16.0) + 2.018*(Cb-128.0);
+
+I = cat(3,r,g,b);
+I = max(0,min(I,255));
+I = I/255;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read422f.m b/SD-VBS/common/toolbox/toolbox_basic/io/read422f.m
new file mode 100755
index 0000000..0063000
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read422f.m
@@ -0,0 +1,50 @@
+function I = read422(fname,nc);
+%
+%   I = read422(fname,width);
+%
+%      read in a .422 file, need to pass image width, default = 640
+%
+
+% assume image width = 640
+if nargin<2,
+  nc = 640;
+end
+
+%% find the image size
+fid = fopen(fname);
+fseek(fid,0,1);
+fsize = ftell(fid);
+
+nr = fsize/nc/2;
+
+fseek(fid,0,-1);
+
+%% read in Ybr data
+data = fread(fid,fsize,'uchar');
+
+%%% extract Y, Cb, Cr
+Y1 = data(1:2:end);  Y1 = reshape(Y1,nc,nr)';
+Cb1 = data(2:4:end); Cb1 = reshape(Cb1,nc/2,nr)';
+Cr1 = data(4:4:end); Cr1 = reshape(Cr1,nc/2,nr)';
+
+Cb = zeros(size(Y1)); 
+Cr = zeros(size(Y1));
+
+Cb(:,1:2:end) = Cb1;  Cb(:,2:2:end) = Cb1;
+%Cb(:,2:2:end) = 0.5*(Cb1+[Cb1(:,2:end),Cb1(:,end)]);
+
+Cr(:,1:2:end) = Cr1; Cr(:,2:2:end) = Cr1;
+%Cr(:,2:2:end) = 0.5*(Cr1+[Cr1(:,2:end),Cr1(:,end)]);
+
+%%% convert to r,g,b
+r = 1.164*(Y1-16.0) + 1.596*(Cr-128.0);
+g = 1.164*(Y1-16.0) - 0.813*(Cr-128.0) - 0.391*(Cb-128.0);
+b = 1.164*(Y1-16.0) + 2.018*(Cb-128.0);
+
+r = flipud(max(0,min(r,255)));
+g = flipud(max(0,min(g,255)));
+b = flipud(max(0,min(b,255)));
+
+I = cat(3,r,g,b);
+
+I = permute(I/255,[2,1,3]);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_cimgs.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_cimgs.m
new file mode 100755
index 0000000..d5df7f5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_cimgs.m
@@ -0,0 +1,40 @@
+function Is = read_imgs(homedir,imgdir,prename,postname,digits,startid,endid,step_img)
+%
+% Is = read_imgs(homedir,imgdir,prename,postname,digits,startid,endid,step_img)
+%
+
+
+
+command = ['%s%s%s%.',num2str(digits),'d%s'];
+
+fname = sprintf(command,homedir,imgdir,prename,startid,postname);
+disp(fname);
+if (strcmp('.ppm',postname)),
+ I1 = readppm(fname);
+else
+   I1 = imread(fname); 
+end
+
+
+Is = zeros(size(I1,1),size(I1,2),size(I1,3),1+floor((endid-startid)/step_img));
+Is(:,:,:,1) = I1;
+im_id = 1;
+for j = startid+step_img:step_img:endid,
+    command = ['%s%s%s%.',num2str(digits),'d%s'];
+    fname = sprintf(command,homedir,imgdir,prename,j,postname);
+    disp(fname);
+    im_id = im_id+1;
+    
+    if (strcmp('.ppm',postname)),
+      Is(:,:,:,im_id) = readppm(fname);
+    else
+      a = imread(fname); 
+      Is(:,:,:,im_id) = a;
+    end
+end
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm.m
new file mode 100755
index 0000000..3f7b69d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm.m
@@ -0,0 +1,26 @@
+function [evs,ev_info] = read_ev_pgm(basename,start_id,end_id,neigs)
+%
+%  evs = read_ev_pgm(basename,start_id,end_id,neigs)
+%
+%
+
+fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,start_id,1)
+[nr,nc] = peek_pgm_size(fname);
+
+evs = zeros(nr,nc,neigs-1,start_id-end_id+1);
+ev_info = zeros(4,neigs-1,start_id-end_id+1);
+
+for j=start_id:end_id,
+    for k=1:neigs-1,
+
+        fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,j,k);
+        [I,info] = readpgm_evinfo(fname);
+
+        if (length(info)<4)
+          info = [0;0;0;0];
+        end
+
+        evs(:,:,k,j-start_id+1) = I;
+        ev_info(:,k,j-start_id+1) = info';
+    end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm2.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm2.m
new file mode 100755
index 0000000..b0cc3f9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm2.m
@@ -0,0 +1,29 @@
+function [evs,ev_info] = read_ev_pgm2(basename,start_id,end_id,neigs)

+%
+%  evs = read_ev_pgm(basename,start_id,end_id,neigs)
+%
+%  read_ev_pgm.m modified by SXY in Feb. 2001.
+%  The first eigenvector is also included
+
+fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,start_id,1)
+[nr,nc] = peek_pgm_size(fname);
+
+evs = zeros(nr,nc,neigs,start_id-end_id+1);
+ev_info = zeros(4,neigs,start_id-end_id+1);
+
+for j=start_id:end_id,
+   
+    for k=1:neigs,
+
+        fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,j,k-1);
+
+        [I,info] = readpgm_evinfo(fname);
+
+        if (length(info)<4)
+                 info = [0;0;0;0];
+        end
+
+        evs(:,:,k,j-start_id+1) = I;
+        ev_info(:,k,j-start_id+1) = info';
+    end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm_real.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm_real.m
new file mode 100755
index 0000000..d985679
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_ev_pgm_real.m
@@ -0,0 +1,30 @@
+function [evs,ev_info] = read_ev_pgm(basename,start_id,end_id,neigs)
+%
+%  evs = read_ev_pgm(basename,start_id,end_id,neigs)
+%
+%
+
+fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,start_id,1);
+[nr,nc] = peek_pgm_size(fname);
+
+evs = zeros(nr,nc,neigs-1,start_id-end_id+1);
+ev_info = zeros(4,neigs-1,start_id-end_id+1);
+
+for j=start_id:end_id,
+    for k=1:neigs,
+
+        fname = sprintf('%s_ev_%.2d.%.2d.pgm',basename,j,k-1);
+        [I,info] = readpgm_evinfo(fname);
+
+        evs(:,:,k,j-start_id+1) = I;
+        ev_info(:,k,j-start_id+1) = info';
+    end
+end
+
+evs = squeeze(evs);
+
+for j=1:neigs,
+      evs(:,:,j) = (evs(:,:,j)/ev_info(3,j)) +ev_info(1,j);
+      %evs(:,:,j) = evs(:,:,j)/norm(reshape(evs(:,:,j),nr*nc,1));
+end
+    
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_imgs.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_imgs.m
new file mode 100755
index 0000000..f84486c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_imgs.m
@@ -0,0 +1,47 @@
+function Is = read_imgs(homedir,imgdir,prename,postname,digits,startid,endid,step_img)
+%
+% Is = read_imgs(homedir,imgdir,prename,postname,digits,startid,endid,step_img)
+%
+
+
+
+command = ['%s%s%s%.',num2str(digits),'d%s'];
+
+fname = sprintf(command,homedir,imgdir,prename,startid,postname);
+disp(fname);
+if (strcmp('.pgm',postname)),
+ I1 = readpgm(fname);
+elseif (strcmp('.ppm',postname))
+ a = readppm(fname);
+ I1 = sum(a,3);
+else
+   a = imread(fname); a = sum(double(a),3);
+   I1 = a;
+end
+
+
+Is = zeros(size(I1,1),size(I1,2),1+floor((endid-startid)/step_img));
+Is(:,:,1) = I1;
+im_id = 1;
+for j = startid+step_img:step_img:endid,
+    command = ['%s%s%s%.',num2str(digits),'d%s'];
+    fname = sprintf(command,homedir,imgdir,prename,j,postname);
+    disp(fname);
+    im_id = im_id+1;
+    
+    if (strcmp('.pgm',postname)),
+      Is(:,:,im_id) = readpgm(fname);
+    elseif (strcmp('.ppm',postname))
+      a = readppm(fname);
+      Is(:,:,im_id) = sum(a,3);
+    else
+      a = imread(fname); a = sum(double(a),3);
+      Is(:,:,im_id) = a;
+    end
+end
+
+
+
+
+
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_pmm.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_pmm.m
new file mode 100755
index 0000000..9e2eed1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_pmm.m
@@ -0,0 +1,12 @@
+function I = read_pmm(fname)
+
+fid = fopen(fname,'r');
+
+[A] = fscanf(fid,'%d\n',3);
+
+I = fscanf(fid,'%d',prod(A));
+
+
+I = reshape(I,A(2),A(1))';
+
+I = squeeze(I);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_scan.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_scan.m
new file mode 100755
index 0000000..6ad818a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_scan.m
@@ -0,0 +1,42 @@
+function [img,sizeinfo] = 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.
+
+
+fname_header = sprintf('%s.h01',filename);
+fname_data = sprintf('%s.i01',filename);
+
+fid = fopen(fname_header,'r');
+
+
+done = 0;
+while done~=3,
+  cmt = fgets(fid)
+  if (findstr(cmt,'!matrix size[1]')),
+    nc = sscanf(cmt,'!matrix size[1] :=%d');
+    done = done+1;
+  elseif (findstr(cmt,'!matrix size[2]')),
+    nr = sscanf(cmt,'!matrix size[2] :=%d');
+    done = done+1;
+  elseif (findstr(cmt,'!matrix size[3]')),
+    ns = sscanf(cmt,'!matrix size[3] :=%d');
+    done = done+1;
+  end
+end
+fclose(fid);
+
+fid = fopen(fname_data,'r');
+
+%img = fscanf(fid,'%d',size);
+%img = img';
+
+img = fread(fid,nc*nr*ns,'uint8');
+img = reshape(img,nc,nr,ns);
+
+sizeinfo(1) = nr;
+sizeinfo(2) = nc;
+sizeinfo(3) = ns;
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/read_seg_file.m b/SD-VBS/common/toolbox/toolbox_basic/io/read_seg_file.m
new file mode 100755
index 0000000..a056ebc
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/read_seg_file.m
@@ -0,0 +1,36 @@
+function [seg_map,seg] = read_seg(filename)
+%
+% function seg = read_seg(filename)
+%
+
+fid = fopen(filename,'r');
+if (fid < 0),
+   error(sprintf('can not find file: %s',filename));
+end
+
+header_done =0;
+while ~header_done,
+
+   cmt = fgets(fid);
+   if length(findstr(cmt,'#')) ~=1,
+    header_done = 1;
+    cmt = fgets(fid);
+    nc = sscanf(cmt,'width %d\n');
+    cmt = fgets(fid);
+    nr = sscanf(cmt,'height %d\n');
+    cmt = fgets(fid);
+    mseg = sscanf(cmt,'segments %d\n');
+    cmt = fgets(fid);
+   end
+end
+
+seg = fscanf(fid,'%d',100*nr);
+tmp = length(seg(:))/4;
+seg = reshape(seg,4,tmp)';
+
+seg_map = zeros(nr,nc);
+
+for j=1:tmp,
+  seg_map(seg(j,2)+1,1+seg(j,3):1+seg(j,4)) = seg(j,1);
+end
+ 
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readlines.m b/SD-VBS/common/toolbox/toolbox_basic/io/readlines.m
new file mode 100755
index 0000000..90bc944
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readlines.m
@@ -0,0 +1,30 @@
+function [lines,indexes] = readlines(fname)
+%
+%  [lines,indexes] = readlines(fname)
+%   Read Edges points from .Ins file produced by "getlines"
+%   lines: a num_pointsx2 matrix of the edge points
+%   indexes: the braking point the lines
+%
+
+fid = fopen(fname,'r');
+
+done = 0;
+lines = [];
+indexes = [];
+
+first_line = fscanf(fid,'%s',1);
+
+while (~done),
+  num_lines = sscanf(first_line(3:length(first_line)),'%d');
+  disp(num_lines);
+  indexes = [indexes,num_lines];
+  a = fscanf(fid,'%f',[2,num_lines]);
+  lines = [lines;a'];
+
+  first_line = fscanf(fid,'%s',1);
+  if (first_line == []),
+   done = 1;
+  end
+end
+
+  
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpdm3.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpdm3.m
new file mode 100755
index 0000000..c21fc48
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpdm3.m
@@ -0,0 +1,16 @@
+function I = readpdm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',3)
+A(3) = max(1,A(3));
+
+I = fscanf(fid,'%d',[A(1)*A(2)*A(3)]);
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+I = reshape(I,A(2),A(1),A(3));
+
+I = permute(I,[2,1,3]);
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpdmc.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpdmc.m
new file mode 100755
index 0000000..37910b9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpdmc.m
@@ -0,0 +1,12 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%d',[A(2),A(1)]);
+%I = fscanf(fid,'%d',[300,1000]);
+I = I';
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpfm.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpfm.m
new file mode 100755
index 0000000..48ecd78
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpfm.m
@@ -0,0 +1,10 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%f',[A(1),A(2)]);
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpfm3.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpfm3.m
new file mode 100755
index 0000000..15ba959
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpfm3.m
@@ -0,0 +1,17 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',3);
+A(3) = max(1,A(3));
+
+I = fscanf(fid,'%f',[A(1)*A(2)*A(3)]);
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+I = reshape(I,A(2),A(1),A(3));
+I = permute(I,[2,1,3]);
+
+I = squeeze(I);
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpfmc.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpfmc.m
new file mode 100755
index 0000000..2039002
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpfmc.m
@@ -0,0 +1,11 @@
+function I = readpfm(filename)
+
+fid = fopen(filename,'r');
+
+A = fscanf(fid,'%d',2);
+I = fscanf(fid,'%f',[A(2),A(1)]);
+I = I';
+
+%I = fscanf(fid,'%f',A(2)*A(1));I = reshape(I,A(1),A(2));
+
+fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readpgm.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpgm.m
new file mode 100755
index 0000000..7aaf998
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpgm.m
@@ -0,0 +1,30 @@
+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');
+if (fid < 0),
+   error(sprintf('can not find file: %s',filename));
+end
+
+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/io/readpgm_evinfo.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpgm_evinfo.m
new file mode 100755
index 0000000..69f80ba
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpgm_evinfo.m
@@ -0,0 +1,35 @@
+function [img,ev_info] = pgmread_evinfo(filename)
+% function img = pgmread(filename)
+%   this is my version of pgmread for the pgm file created by XV.
+%
+%   return the information in line # 
+
+
+fid = fopen(filename,'r');
+
+if (fid <0),
+  error(sprintf('can not find file %s',filename));
+end
+
+fscanf(fid, 'P5\n');
+cmt = '#';
+while findstr(cmt, '#'),
+  cmt = fgets(fid);
+  if findstr(cmt,'#'),
+      ev_info = sscanf(cmt,'# minv: %f, maxv: %f, scale: %f, eigval: %f');
+  end   
+  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/io/readpmm.m b/SD-VBS/common/toolbox/toolbox_basic/io/readpmm.m
new file mode 100755
index 0000000..88fe907
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readpmm.m
@@ -0,0 +1,22 @@
+function I=readpmm(name)
+%
+%   I=writepmm(name)
+%
+%     I is a mul-band image
+%
+  fid = fopen(name,'r');
+
+  if (fid <0),
+    error(sprintf('can not find file %s',name));
+  end
+
+  a = fscanf(fid,'%d',3);
+  nr = a(1);nc = a(2);nb = a(3);
+
+
+  I = fscanf(fid, '%f\n', nr*nc*nb);
+
+  I = reshape(I,nc,nr,nb)';
+  I = squeeze(I);
+
+  fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/readppm.m b/SD-VBS/common/toolbox/toolbox_basic/io/readppm.m
new file mode 100755
index 0000000..b9dd566
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/readppm.m
@@ -0,0 +1,23 @@
+function [I,r, g, b] = readppm(name)
+
+  fid = fopen(name, 'r');
+  fscanf(fid, 'P6\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); 
+  packed = fread(fid,[3*y,x],'uint8')';
+  r = packed(:,1:3:3*y);
+  g = packed(:,2:3:3*y);
+  b = packed(:,3:3:3*y);
+  fclose(fid);
+
+ I(:,:,1) = r;
+ I(:,:,2) = g;
+ I(:,:,3) = b;
+ I  =  I/255;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/writepgm.m b/SD-VBS/common/toolbox/toolbox_basic/io/writepgm.m
new file mode 100755
index 0000000..113cb18
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/writepgm.m
@@ -0,0 +1,8 @@
+function I = writepgm(name,I)
+
+  [y,x] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, 'P5\n%d %d\n255\n', x,y);
+  fwrite(fid, I', 'uint8');
+  fclose(fid);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/io/writeppm.m b/SD-VBS/common/toolbox/toolbox_basic/io/writeppm.m
new file mode 100755
index 0000000..3d2fed1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/io/writeppm.m
@@ -0,0 +1,14 @@
+function writeppm(name,I)
+
+  [y,x,nb] = size(I);
+
+  fid = fopen(name, 'w');
+  fprintf(fid, 'P6\n%d %d\n255\n', x,y);
+
+  I1 = reshape(I(:,:,1)',1,x*y);
+  I2 = reshape(I(:,:,2)',1,x*y);
+  I3 = reshape(I(:,:,3)',1,x*y);
+  
+  fwrite(fid, [I1;I2;I3], 'uint8');
+  fclose(fid);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m
new file mode 100755
index 0000000..3ddb232
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/Contents.m
@@ -0,0 +1,26 @@
+%Functions related to the assignment problem.
+%Version 1.0, 25-May-1999.
+%
+%Copyright (c) 1995-1999 Niclas Borlin, Dept. of Computing Science, 
+%                        Umea University, SE-901 87 UMEA, Sweden.
+%                        Niclas.Borlin@cs.umu.se. 
+%                        www.cs.umu.se/~niclas.
+%
+%All standard disclaimers apply.
+%
+%You are free to use this code as you wish.  If you use it for a
+%publication or in a commercial package, please include an
+%acknowledgement and/or at least send me an email. (Looks good in my CV :-).
+%
+%Main functions:
+%   hungarian - calculate a solution of the square assignment
+%               problem. See HELP for a reference.
+%   condass   - calculate a condition number of the solution to the
+%               assignment problem. See HELP for a reference.
+%   allcosts  - calculate the costs of all possible assignments.
+%   allperms  - calculate all possible permutations/assignments of a
+%               given problem size.
+%
+%Test/demo functions.
+%   demo      - short demonstration of the main functions.
+%   test      - test/verification of the main functions.
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m
new file mode 100755
index 0000000..ffdb8b9
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allcosts.m
@@ -0,0 +1,17 @@
+function [c,p]=allcosts(C)
+%ALLCOSTS Calculate all costs for an assignment problem.
+%
+%[c,p]=allcosts(C)
+%c returns the costs, p the corresponding permutations.
+
+% v1.0  95-07-18. Niclas Borlin, niclas@cs.umu.se.
+
+p=allperm(size(C,1));
+
+c=zeros(size(p,1),1);
+
+I=eye(size(C,1));
+
+for i=1:size(p,1)
+        c(i)=sum(C(logical(sparse(p(i,:),1:size(C,1),1))));
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m
new file mode 100755
index 0000000..b8d419e
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/allperm.m
@@ -0,0 +1,17 @@
+function p=allperm(n)
+%ALLPERM All permutation matrix.
+%
+%p=allperm(n)
+%Returns a matrix with all permutations of 1:n stored row-wise.
+
+% v1.0  95-07-18. Niclas Borlin, niclas@cs.umu.se.
+
+if (n<=1)
+        p=1;
+else
+        q=allperm(n-1);
+        p=[];
+        for i=1:n
+                p=[p;i*ones(size(q,1),1) q+(q>=i)];
+        end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m
new file mode 100755
index 0000000..82552e7
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/condass.m
@@ -0,0 +1,54 @@
+function [k,C1,T1,C2,T2]=condass(A)
+%CONDASS Calculate condition number of the assigment problem.
+%
+%[k,C1,T1,C2,T2]=condass(A)
+%A  - A square cost matrix.
+%k  - The condition number of the assigment problem.
+%C1 - The best assigment.
+%T1 - The lowest cost.
+%C2 - The second best assignment.
+%T2 - The second lowest cost.
+%
+%The condition number is calculated as the relative difference between
+%the best and second best solutions, as described in Nystrom, Soderkvist,
+%and Wedin, "A Note on some Identification Problems Arising in Roentgen
+%Stereo Photogrammetric Analysis", J of Biomechanics, 27(10):1291-1294,
+%1994.
+
+% v1.0  96-09-14. Niclas Borlin, niclas@cs.umu.se.
+
+% A substantial effort was put into this code. If you use it for a
+% publication or otherwise, please include an acknowledgement and notify
+% me by email. /Niclas
+
+% Create a large number used to block selected assignments.
+big=sum(sum(A))+1;
+
+% Get best assigment.
+[C1,T1]=hungarian(A);
+
+% Initialize second best solution.
+T2=inf;
+C2=zeros(size(C1));
+
+% Create a work matrix.
+B=A;
+for i=1:length(C1)
+    % Block assigment in column i.
+    B(C1(i),i)=big;
+    % Get best assigment with this one blocked.
+    [C,T]=hungarian(B);
+    if (T<T2)
+                % Remember it if it's the best so far.
+                T2=T;
+                C2=C;
+    end
+    % Remove blocking in column i.
+    B(C1(i),i)=A(C1(i),i);
+end
+
+% Calculate difference...
+mu=T2-T1;
+
+% ...and condition number.
+k=T1/mu;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m
new file mode 100755
index 0000000..2d34e37
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/demo.m
@@ -0,0 +1,38 @@
+A=magic(10);
+B=A(4:7,4:7);
+disp('Cost matrix:')
+disp(B)
+disp('Calculating best assignment...');
+[c,t]=hungarian(B);
+disp('Best assignment (as row indices):')
+disp(c)
+disp('Best assignment (as logical matrix):')
+disp(logical(full(sparse(c,1:4,1))))
+disp('Lowest cost:')
+disp(t)
+
+disp(sprintf('\nCalculating condition number for solution...'));
+[k,c1,t1,c2,t2]=condass(B);
+disp('Lowest cost (should be same as above): ')
+disp(t1)
+disp('corresponding assignment (should be same as above):')
+disp(c1)
+disp('Second lowest cost: ')
+disp(t2)
+disp('corresponding assignment:')
+disp(c2)
+disp('Condition number for solution:')
+disp(k)
+
+disp(sprintf('\nCalculating all possible costs...'));
+[c,p]=allcosts(B);
+% Sort by cost.
+[y,i]=sort(c);
+disp('The three lowest costs:')
+disp(c(i(1:3)))
+disp('Corresponding assignments:')
+disp(p(i(1:3),:))
+disp('The three highest costs:')
+disp(c(i(end+[-2:0])))
+disp('Corresponding assignments:')
+disp(p(i(end+[-2:0]),:))
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m
new file mode 100755
index 0000000..0b493f0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/hungarian.m
@@ -0,0 +1,464 @@
+function [C,T]=hungarian(A)
+%HUNGARIAN Solve the Assignment problem using the Hungarian method.
+%
+%[C,T]=hungarian(A)
+%A - a square cost matrix.
+%C - the optimal assignment.
+%T - the cost of the optimal assignment.
+
+% Adapted from the FORTRAN IV code in Carpaneto and Toth, "Algorithm 548:
+% Solution of the assignment problem [H]", ACM Transactions on
+% Mathematical Software, 6(1):104-111, 1980.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+%                 Department of Computing Science, Umeå University,
+%                 Sweden. 
+%                 All standard disclaimers apply.
+
+% A substantial effort was put into this code. If you use it for a
+% publication or otherwise, please include an acknowledgement or at least
+% notify me by email. /Niclas
+
+[m,n]=size(A);
+
+if (m~=n)
+    error('HUNGARIAN: Cost matrix must be square!');
+end
+
+% Save original cost matrix.
+orig=A;
+
+% Reduce matrix.
+A=hminired(A);
+
+% Do an initial assignment.
+[A,C,U]=hminiass(A);
+
+% Repeat while we have unassigned rows.
+while (U(n+1))
+    % Start with no path, no unchecked zeros, and no unexplored rows.
+    LR=zeros(1,n);
+    LC=zeros(1,n);
+    CH=zeros(1,n);
+    RH=[zeros(1,n) -1];
+    
+    % No labelled columns.
+    SLC=[];
+    
+    % Start path in first unassigned row.
+    r=U(n+1);
+    % Mark row with end-of-path label.
+    LR(r)=-1;
+    % Insert row first in labelled row set.
+    SLR=r;
+    
+    % Repeat until we manage to find an assignable zero.
+    while (1)
+        % If there are free zeros in row r
+        if (A(r,n+1)~=0)
+            % ...get column of first free zero.
+            l=-A(r,n+1);
+            
+            % If there are more free zeros in row r and row r in not
+            % yet marked as unexplored..
+            if (A(r,l)~=0 & RH(r)==0)
+                % Insert row r first in unexplored list.
+                RH(r)=RH(n+1);
+                RH(n+1)=r;
+                
+                % Mark in which column the next unexplored zero in this row
+                % is.
+                CH(r)=-A(r,l);
+            end
+        else
+            % If all rows are explored..
+            if (RH(n+1)<=0)
+                % Reduce matrix.
+                [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
+            end
+            
+            % Re-start with first unexplored row.
+            r=RH(n+1);
+            % Get column of next free zero in row r.
+            l=CH(r);
+            % Advance "column of next free zero".
+            CH(r)=-A(r,l);
+            % If this zero is last in the list..
+            if (A(r,l)==0)
+                % ...remove row r from unexplored list.
+                RH(n+1)=RH(r);
+                RH(r)=0;
+            end
+        end
+        
+        % While the column l is labelled, i.e. in path.
+        while (LC(l)~=0)
+            % If row r is explored..
+            if (RH(r)==0)
+                % If all rows are explored..
+                if (RH(n+1)<=0)
+                    % Reduce cost matrix.
+                    [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR);
+                end
+                
+                % Re-start with first unexplored row.
+                r=RH(n+1);
+            end
+            
+            % Get column of next free zero in row r.
+            l=CH(r);
+            
+            % Advance "column of next free zero".
+            CH(r)=-A(r,l);
+            
+            % If this zero is last in list..
+            if(A(r,l)==0)
+                % ...remove row r from unexplored list.
+                RH(n+1)=RH(r);
+                RH(r)=0;
+            end
+        end
+        
+        % If the column found is unassigned..
+        if (C(l)==0)
+            % Flip all zeros along the path in LR,LC.
+            [A,C,U]=hmflip(A,C,LC,LR,U,l,r);
+            % ...and exit to continue with next unassigned row.
+            break;
+        else
+            % ...else add zero to path.
+            
+            % Label column l with row r.
+            LC(l)=r;
+            
+            % Add l to the set of labelled columns.
+            SLC=[SLC l];
+            
+            % Continue with the row assigned to column l.
+            r=C(l);
+            
+            % Label row r with column l.
+            LR(r)=l;
+            
+            % Add r to the set of labelled rows.
+            SLR=[SLR r];
+        end
+    end
+end
+
+% Calculate the total cost.
+T=sum(orig(logical(sparse(C,1:size(orig,2),1))));
+
+
+function A=hminired(A)
+%HMINIRED Initial reduction of cost matrix for the Hungarian method.
+%
+%B=assredin(A)
+%A - the unreduced cost matris.
+%B - the reduced cost matrix with linked zeros in each row.
+
+% v1.0  96-06-13. Niclas Borlin, niclas@cs.umu.se.
+
+[m,n]=size(A);
+
+% Subtract column-minimum values from each column.
+colMin=min(A);
+A=A-colMin(ones(n,1),:);
+
+% Subtract row-minimum values from each row.
+rowMin=min(A')';
+A=A-rowMin(:,ones(1,n));
+
+% Get positions of all zeros.
+[i,j]=find(A==0);
+
+% Extend A to give room for row zero list header column.
+A(1,n+1)=0;
+for k=1:n
+    % Get all column in this row. 
+    cols=j(k==i)';
+    % Insert pointers in matrix.
+    A(k,[n+1 cols])=[-cols 0];
+end
+
+
+function [A,C,U]=hminiass(A)
+%HMINIASS Initial assignment of the Hungarian method.
+%
+%[B,C,U]=hminiass(A)
+%A - the reduced cost matrix.
+%B - the reduced cost matrix, with assigned zeros removed from lists.
+%C - a vector. C(J)=I means row I is assigned to column J,
+%              i.e. there is an assigned zero in position I,J.
+%U - a vector with a linked list of unassigned rows.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+[n,np1]=size(A);
+
+% Initalize return vectors.
+C=zeros(1,n);
+U=zeros(1,n+1);
+
+% Initialize last/next zero "pointers".
+LZ=zeros(1,n);
+NZ=zeros(1,n);
+
+for i=1:n
+    % Set j to first unassigned zero in row i.
+        lj=n+1;
+        j=-A(i,lj);
+
+    % Repeat until we have no more zeros (j==0) or we find a zero
+        % in an unassigned column (c(j)==0).
+    
+        while (C(j)~=0)
+                % Advance lj and j in zero list.
+                lj=j;
+                j=-A(i,lj);
+        
+                % Stop if we hit end of list.
+                if (j==0)
+                        break;
+                end
+        end
+
+        if (j~=0)
+                % We found a zero in an unassigned column.
+                
+                % Assign row i to column j.
+                C(j)=i;
+                
+                % Remove A(i,j) from unassigned zero list.
+                A(i,lj)=A(i,j);
+
+                % Update next/last unassigned zero pointers.
+                NZ(i)=-A(i,j);
+                LZ(i)=lj;
+
+                % Indicate A(i,j) is an assigned zero.
+                A(i,j)=0;
+        else
+                % We found no zero in an unassigned column.
+
+                % Check all zeros in this row.
+
+                lj=n+1;
+                j=-A(i,lj);
+                
+                % Check all zeros in this row for a suitable zero in another row.
+                while (j~=0)
+                        % Check the in the row assigned to this column.
+                        r=C(j);
+                        
+                        % Pick up last/next pointers.
+                        lm=LZ(r);
+                        m=NZ(r);
+                        
+                        % Check all unchecked zeros in free list of this row.
+                        while (m~=0)
+                                % Stop if we find an unassigned column.
+                                if (C(m)==0)
+                                        break;
+                                end
+                                
+                                % Advance one step in list.
+                                lm=m;
+                                m=-A(r,lm);
+                        end
+                        
+                        if (m==0)
+                                % We failed on row r. Continue with next zero on row i.
+                                lj=j;
+                                j=-A(i,lj);
+                        else
+                                % We found a zero in an unassigned column.
+                        
+                                % Replace zero at (r,m) in unassigned list with zero at (r,j)
+                                A(r,lm)=-j;
+                                A(r,j)=A(r,m);
+                        
+                                % Update last/next pointers in row r.
+                                NZ(r)=-A(r,m);
+                                LZ(r)=j;
+                        
+                                % Mark A(r,m) as an assigned zero in the matrix . . .
+                                A(r,m)=0;
+                        
+                                % ...and in the assignment vector.
+                                C(m)=r;
+                        
+                                % Remove A(i,j) from unassigned list.
+                                A(i,lj)=A(i,j);
+                        
+                                % Update last/next pointers in row r.
+                                NZ(i)=-A(i,j);
+                                LZ(i)=lj;
+                        
+                                % Mark A(r,m) as an assigned zero in the matrix . . .
+                                A(i,j)=0;
+                        
+                                % ...and in the assignment vector.
+                                C(j)=i;
+                                
+                                % Stop search.
+                                break;
+                        end
+                end
+        end
+end
+
+% Create vector with list of unassigned rows.
+
+% Mark all rows have assignment.
+r=zeros(1,n);
+rows=C(C~=0);
+r(rows)=rows;
+empty=find(r==0);
+
+% Create vector with linked list of unassigned rows.
+U=zeros(1,n+1);
+U([n+1 empty])=[empty 0];
+
+
+function [A,C,U]=hmflip(A,C,LC,LR,U,l,r)
+%HMFLIP Flip assignment state of all zeros along a path.
+%
+%[A,C,U]=hmflip(A,C,LC,LR,U,l,r)
+%Input:
+%A   - the cost matrix.
+%C   - the assignment vector.
+%LC  - the column label vector.
+%LR  - the row label vector.
+%U   - the 
+%r,l - position of last zero in path.
+%Output:
+%A   - updated cost matrix.
+%C   - updated assignment vector.
+%U   - updated unassigned row list vector.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+n=size(A,1);
+
+while (1)
+    % Move assignment in column l to row r.
+    C(l)=r;
+    
+    % Find zero to be removed from zero list..
+    
+    % Find zero before this.
+    m=find(A(r,:)==-l);
+    
+    % Link past this zero.
+    A(r,m)=A(r,l);
+    
+    A(r,l)=0;
+    
+    % If this was the first zero of the path..
+    if (LR(r)<0)
+        ...remove row from unassigned row list and return.
+        U(n+1)=U(r);
+        U(r)=0;
+        return;
+    else
+        
+        % Move back in this row along the path and get column of next zero.
+        l=LR(r);
+        
+        % Insert zero at (r,l) first in zero list.
+        A(r,l)=A(r,n+1);
+        A(r,n+1)=-l;
+        
+        % Continue back along the column to get row of next zero in path.
+        r=LC(l);
+    end
+end
+
+
+function [A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
+%HMREDUCE Reduce parts of cost matrix in the Hungerian method.
+%
+%[A,CH,RH]=hmreduce(A,CH,RH,LC,LR,SLC,SLR)
+%Input:
+%A   - Cost matrix.
+%CH  - vector of column of 'next zeros' in each row.
+%RH  - vector with list of unexplored rows.
+%LC  - column labels.
+%RC  - row labels.
+%SLC - set of column labels.
+%SLR - set of row labels.
+%
+%Output:
+%A   - Reduced cost matrix.
+%CH  - Updated vector of 'next zeros' in each row.
+%RH  - Updated vector of unexplored rows.
+
+% v1.0  96-06-14. Niclas Borlin, niclas@cs.umu.se.
+
+n=size(A,1);
+
+% Find which rows are covered, i.e. unlabelled.
+coveredRows=LR==0;
+
+% Find which columns are covered, i.e. labelled.
+coveredCols=LC~=0;
+
+r=find(~coveredRows);
+c=find(~coveredCols);
+
+% Get minimum of uncovered elements.
+m=min(min(A(r,c)));
+
+% Subtract minimum from all uncovered elements.
+A(r,c)=A(r,c)-m;
+
+% Check all uncovered columns..
+for j=c
+    % ...and uncovered rows in path order..
+    for i=SLR
+        % If this is a (new) zero..
+        if (A(i,j)==0)
+            % If the row is not in unexplored list..
+            if (RH(i)==0)
+                % ...insert it first in unexplored list.
+                RH(i)=RH(n+1);
+                RH(n+1)=i;
+                % Mark this zero as "next free" in this row.
+                CH(i)=j;
+            end
+            % Find last unassigned zero on row I.
+            row=A(i,:);
+            colsInList=-row(row<0);
+            if (length(colsInList)==0)
+                % No zeros in the list.
+                l=n+1;
+            else
+                l=colsInList(row(colsInList)==0);
+            end
+            % Append this zero to end of list.
+            A(i,l)=-j;
+        end
+    end
+end
+
+% Add minimum to all doubly covered elements.
+r=find(coveredRows);
+c=find(coveredCols);
+
+% Take care of the zeros we will remove.
+[i,j]=find(A(r,c)<=0);
+
+i=r(i);
+j=c(j);
+
+for k=1:length(i)
+    % Find zero before this in this row.
+    lj=find(A(i(k),:)==-j(k));
+    % Link past it.
+    A(i(k),lj)=A(i(k),j(k));
+    % Mark it as assigned.
+    A(i(k),j(k))=0;
+end
+
+A(r,c)=A(r,c)+m;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m
new file mode 100755
index 0000000..4c238f2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/matching/pub/contrib/v5/optim/assignprob/test.m
@@ -0,0 +1,87 @@
+disp('Testing hungarian...');
+A=magic(10);
+B=A(4:7,4:7);
+[c,t]=hungarian(B);
+if (any(c~=[2 1 3 4]))
+        disp('Wrong coupling!');
+elseif (t~=77)
+        disp('Wrong solution!');
+else
+        disp('Hungarian appears OK.');
+end
+
+disp('Testing condass...');
+[k,c1,t1,c2,t2]=condass(B);
+if (any(c1~=[2 1 3 4]))
+        disp('Wrong best coupling!');
+elseif (t1~=77)
+        disp('Wrong lowest cost!');
+elseif (any(c2~=[1 2 3 4]))
+        disp('Wrong second best coupling!');
+elseif (t2~=102)
+        disp('Wrong second lowest cost!');
+elseif (k~=t1/(t2-t1))
+        disp('Wrong condition number!');
+else
+        disp('condass appears OK.');
+end
+
+disp('Testing allcosts...');
+[c,p]=allcosts(B);
+cTrue=[ 102
+                127
+                202
+                227
+                222
+                222
+                77
+                102
+                182
+                202
+                202
+                197
+                177
+                202
+                182
+                202
+                302
+                297
+                202
+                202
+                207
+                202
+                307
+                302
+];
+pTrue=[ 1     2     3     4
+                1     2     4     3
+                1     3     2     4
+                1     3     4     2
+                1     4     2     3
+                1     4     3     2
+                2     1     3     4
+                2     1     4     3
+                2     3     1     4
+                2     3     4     1
+                2     4     1     3
+                2     4     3     1
+                3     1     2     4
+                3     1     4     2
+                3     2     1     4
+                3     2     4     1
+                3     4     1     2
+                3     4     2     1
+                4     1     2     3
+                4     1     3     2
+                4     2     1     3
+                4     2     3     1
+                4     3     1     2
+                4     3     2     1
+          ];
+if (any(c~=cTrue))
+        disp('Wrong costs!');
+elseif (any(p~=pTrue))
+        disp('Wrong permutations!');
+else    
+        disp('allcosts appears OK.');
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/091399fbn-jets.3.jpg b/SD-VBS/common/toolbox/toolbox_basic/pyramid/091399fbn-jets.3.jpg
new file mode 100755
index 0000000..b91732f
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/091399fbn-jets.3.jpg
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/expand.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/expand.m
new file mode 100755
index 0000000..64e9fda
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/expand.m
@@ -0,0 +1,8 @@
+function J = expand(I)
+%
+%
+
+[sy,sx] = size(I);
+[x,y] = meshgrid(1:2*sx+1,1:2*sy+1);
+
+nx = 
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/gauss_lowpass.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/gauss_lowpass.m
new file mode 100755
index 0000000..87ad4f1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/gauss_lowpass.m
@@ -0,0 +1,9 @@
+function J = gauss_lowpass(I,a)
+
+if (nargin < 2),
+  a = 0.4;
+end
+
+w = gen_w(a);
+
+J = conv2(conv2(I,w,'same'),w','same');
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/gen_w.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/gen_w.m
new file mode 100755
index 0000000..b255751
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/gen_w.m
@@ -0,0 +1,12 @@
+function w = gen_w(a)
+%
+
+if (nargin == 0),
+  a = 0.4;
+end
+
+w(3) = a;
+w(1) = 1/4 - a/2;
+w(5) = 1/4 - a/2;
+w(2) = 1/4;
+w(4) = 1/4;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/reduce.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/reduce.m
new file mode 100755
index 0000000..6837e8a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/reduce.m
@@ -0,0 +1,7 @@
+function J = reduce(I)
+
+[nr,nc,nb] = size(I);
+for j=1:nb,
+    tmp = gauss_lowpass(I(:,:,j));
+    J(:,:,j) = tmp(1:2:nr,1:2:nc);
+end
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/session.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/session.m
new file mode 100755
index 0000000..9d0aa6c
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/session.m
@@ -0,0 +1,26 @@
+%image_dir = '/home/barad-dur/d/malik/jshi/';
+%I = gifread([image_dir,'tape9/t9a1_L.40.gif']);
+I = pgmread('car100x100_0001');
+Io = I;
+
+B = [];
+
+done = 0;
+st = 2;
+sz = size(I);
+while (~done),
+   w = max(1,round(0.05*size(I,1)));
+   b = zeros(round(0.5*size(Io)),round(0.5*size(I)));
+   %J = smooth(I,w);
+   %I = J(1:st:size(J,1),1:st:size(J,2));
+   I = reduce(I);
+   sz = [sz;size(I)];
+   b(1:size(I,1),1:size(I,2)) = I;
+   disp(int2str(size(I,1)));
+
+   B = [B,b];
+
+   if (size(I,1) < 8),
+    done = 1;
+   end
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/pyramid/startup.m b/SD-VBS/common/toolbox/toolbox_basic/pyramid/startup.m
new file mode 100755
index 0000000..f86f9f4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/pyramid/startup.m
@@ -0,0 +1,5 @@
+home_dir = '/home/nef0/malik/jshi/jshi/matlab/';
+path([home_dir,'toolbox/io'], path)
+path([home_dir,'toolbox/filter'],path)
+path(path,[home_dir,'vision/vision94/tracking/'])
+clear home_dir
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/remap_angle.m b/SD-VBS/common/toolbox/toolbox_basic/remap_angle.m
new file mode 100755
index 0000000..09f8cff
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/remap_angle.m
@@ -0,0 +1,4 @@
+function a = remap_angle(theta,min,max)
+
+a = (theta<=min).*(theta+pi) + (theta>=max).*(theta-pi) +...
+           ((theta>min)&(theta<max)).*theta;
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/spmtimesd.c b/SD-VBS/common/toolbox/toolbox_basic/spmtimesd.c
new file mode 100755
index 0000000..a98dc0a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/spmtimesd.c
@@ -0,0 +1,141 @@
+/*================================================================
+* spmtimesd.c
+* This routine computes a sparse matrix times a diagonal matrix
+* whose diagonal entries are stored in a full vector.
+*
+* Examples: 
+*     spmtimesd(m,d,[]) = diag(d) * m,
+*     spmtimesd(m,[],d) = m * diag(d)
+*     spmtimesd(m,d1,d2) = diag(d1) * m * diag(d2)
+*     m could be complex, but d is assumed real
+*
+* Stella X. Yu's first MEX function, Nov 9, 2001.
+
+% test sequence:
+
+m = 1000;
+n = 2000;
+a=sparse(rand(m,n));
+d1 = rand(m,1);
+d2 = rand(n,1);
+tic; b=spmtimesd(a,d1,d2); toc
+tic; bb = spdiags(d1,0,m,m) * a * spdiags(d2,0,n,n); toc
+e = (bb-b);
+max(abs(e(:)))
+
+*=================================================================*/
+
+# include "mex.h"
+
+void mexFunction(
+    int nargout,
+    mxArray *out[],
+    int nargin,
+    const mxArray *in[]
+)
+{
+    /* declare variables */
+    int i, j, k, m, n, nzmax, cmplx, xm, yn;
+    int *pir, *pjc, *qir, *qjc;
+    double *x, *y, *pr, *pi, *qr, *qi;
+    
+    /* check argument */
+    if (nargin != 3) {
+        mexErrMsgTxt("Three input arguments required");
+    }
+    if (nargout>1) {
+        mexErrMsgTxt("Too many output arguments.");
+    }
+    if (!(mxIsSparse(in[0]))) {
+        mexErrMsgTxt("Input argument #1 must be of type sparse");
+    }
+    if ( mxIsSparse(in[1]) || mxIsSparse(in[2]) ) {
+        mexErrMsgTxt("Input argument #2 & #3 must be of type full");
+    }
+  
+    /* computation starts */
+    m = mxGetM(in[0]);
+    n = mxGetN(in[0]);
+    pr = mxGetPr(in[0]);
+    pi = mxGetPi(in[0]);
+    pir = mxGetIr(in[0]);
+    pjc = mxGetJc(in[0]);
+ 
+    i = mxGetM(in[1]); 
+    j = mxGetN(in[1]);
+    xm = ((i>j)? i: j);
+
+    i = mxGetM(in[2]); 
+    j = mxGetN(in[2]);
+    yn = ((i>j)? i: j);
+   
+    if ( xm>0 && xm != m) {
+        mexErrMsgTxt("Row multiplication dimension mismatch.");
+    }
+    if ( yn>0 && yn != n) {
+        mexErrMsgTxt("Column multiplication dimension mismatch.");
+    }
+    
+ 
+    nzmax = mxGetNzmax(in[0]);
+    cmplx = (pi==NULL ? 0 : 1);    
+    out[0] = mxCreateSparse(m,n,nzmax,cmplx);
+    if (out[0]==NULL) {
+        mexErrMsgTxt("Not enough space for the output matrix.");
+    }    
+   
+    qr = mxGetPr(out[0]);
+    qi = mxGetPi(out[0]);
+    qir = mxGetIr(out[0]);
+    qjc = mxGetJc(out[0]);
+    
+    /* left multiplication */
+    x = mxGetPr(in[1]);
+    if (yn==0) {
+        for (j=0; j<n; j++) {
+            qjc[j] = pjc[j];
+            for (k=pjc[j]; k<pjc[j+1]; k++) {
+                i = pir[k];   
+                qir[k] = i;
+                 qr[k] = x[i] * pr[k];
+                 if (cmplx) {
+                     qi[k] = x[i] * pi[k];
+                 }
+            }
+        }
+        qjc[n] = k;
+        return;
+    }
+    
+    /* right multiplication */
+    y = mxGetPr(in[2]);
+    if (xm==0) {
+        for (j=0; j<n; j++) {
+            qjc[j] = pjc[j];
+            for (k=pjc[j]; k<pjc[j+1]; k++) {
+                qir[k] = pir[k];
+                qr[k]  = pr[k] * y[j];
+                if (cmplx) {
+                    qi[k] = qi[k] * y[j];
+                }
+            }
+       }
+       qjc[n] = k;
+       return;
+   }
+    
+   /* both sides */
+   for (j=0; j<n; j++) {
+       qjc[j] = pjc[j];
+       for (k=pjc[j]; k<pjc[j+1]; k++) {
+           i = pir[k];
+           qir[k]= i;
+           qr[k] = x[i] * pr[k] * y[j];
+           if (cmplx) {
+               qi[k] = x[i] * qi[k] * y[j];      
+           }
+       }
+       qjc[n] = k;
+   }
+   
+}   
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/afromncut.m b/SD-VBS/common/toolbox/toolbox_basic/stella/afromncut.m
new file mode 100755
index 0000000..ec014d0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/afromncut.m
@@ -0,0 +1,73 @@
+% function a = afromncut(v,s,d,visimg,no_rep,pixel_loc)
+% Input:
+%    v = eigenvectors of d*a*d, starting from the second.
+%      (the first is all one over some constant determined by d)
+%    s = eigenvalues
+%    d = normalization matrix 1/sqrt(rowsum(abs(a)))
+%    visimg = 1/0 if each eigenvector is/not 2D (so v is 3D)
+%    no_rep = 1 (default), affinity has attraction only
+%        if 1, the first column of v is the second eigenvector
+%        if 0, the first column of v is the first eigenvector.
+%    pixel_loc = nx1 matrix, each is a pixel location
+% Output:
+%    a = diag(1/d) * na * diag(1/d);
+%    If pixel_loc = []; a is returned, if not out of memory
+%    otherwise, only rows of a at pixel_loc are returned.
+%
+% This routine is used to estimate the original affinity matrix
+% through the first few eigenvectors and its normalization matrix.
+
+% A test sequence includes:
+% a = randsym(5);
+% [na,d] = normalize(a);
+% [v,s] = ncut(a,5);
+% v = v(:,2:end); s = s(2:end);
+% aa = afromncut(v,s,d);
+% max(abs(aa(:) - a(:)))
+
+% Stella X. Yu, 2000.
+
+function a = afromncut(v,s,d,visimg,no_rep,pixel_loc)
+
+[nr,nc,nv] = size(v);
+if nargin<4 | isempty(visimg),
+   visimg = (nv>1);
+end
+
+if nargin<5 | isempty(no_rep),
+   no_rep = 1;
+end
+
+if visimg,
+   nr = nr * nc;
+else
+   nv = nc;
+end
+
+if nargin<6 | isempty(pixel_loc),
+   pixel_loc = 1:nr;
+end
+
+% D^(1/2)
+d = 1./(d(:)+eps);
+
+% first recover the first eigenvector
+if no_rep,
+    u = (1/norm(d)) + zeros(nr,1);
+    s = [1;s(:)];
+    nv = nv + 1;
+else
+    u = [];
+end
+
+% the full set of generalized eigenvectors
+v = [u, reshape(v,[nr,nv-no_rep])];
+
+% This is the real D, row sum
+d = d.^2;
+
+% an equivalent way to compute v = diag(d) * v;
+v = v .* d(:,ones(nv,1)); % to avoid using a big matrix diag(d)
+
+% synthesis
+a = v(pixel_loc,:)*diag(s)*v';
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/dispimg.m b/SD-VBS/common/toolbox/toolbox_basic/stella/dispimg.m
new file mode 100755
index 0000000..4e419a0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/dispimg.m
@@ -0,0 +1,65 @@
+% function dispimg(g,fmt,lgd,cmap) display multiple images in one figure. 
+% Input:
+%    g = a cell and fmt is a 1x2 vector specifying the layout.
+%    lgd = a string cell for the title of each image. 
+%    cmap = the colormap (default is the gray, -1 for the inverted gray).
+%    ishori = a vector of 1/0 to display real and imag parts horizontally / vertically
+
+% Stella X. Yu, 2000.
+
+function dispimg(g,fmt,lgd,cmap,ishori);
+
+cellg = iscell(g);
+if cellg,
+    num_fig = length(g);
+else
+    num_fig = size(g,3);
+end;
+
+if nargin<2 | isempty(fmt),
+    m = ceil(sqrt(num_fig));
+    n = ceil(num_fig / m);
+else
+    m = fmt(1);
+    n = fmt(2);
+end
+
+if nargin<3 | isempty(lgd),
+    lgd = 1:num_fig;
+end 
+if isnumeric(lgd),
+   lgd = cellstr(num2str(lgd(:),3)); 
+end
+i = size(lgd);
+if i(1)==1,
+   lgd = [lgd, cell(1,num_fig-i(2))];
+else
+   lgd = [lgd; cell(num_fig-i(1),1)];
+end
+
+if nargin<5 | isempty(ishori),
+   ishori = ones(num_fig,1);
+end
+ishori(end+1:num_fig) = ishori(end);
+
+for k=1:num_fig,
+    subplot(m,n,k);
+    if cellg,
+        showim(g{k},[],ishori(k));
+    else
+        showim(g(:,:,k),[],ishori(k));
+    end
+    title(lgd{k});
+end
+
+if nargin<4 | isempty(cmap),
+   cmap = gray;
+end
+if length(cmap)==1,
+   if cmap==1,
+      cmap = gray;
+   else
+      cmap = flipud(gray);
+   end
+end
+colormap(cmap);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/firstncut.m b/SD-VBS/common/toolbox/toolbox_basic/stella/firstncut.m
new file mode 100755
index 0000000..a22077d
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/firstncut.m
@@ -0,0 +1,67 @@
+% function [v,s,d] = firstncut(base_name,rec_num)
+% Input:
+%    base_name = image name
+%    rec_num = parameter record number 
+% Output:
+%    v = eigenvectors
+%    s = eigenvalues
+%    d = normalization matrix d = 1/sqrt(rowsum(abs(a)))
+% Convert Jianbo Shi's Ncut Ccode results from images to matlab matrices.
+
+% Stella X. Yu, 2000.
+
+function [v,s,d] = firstncut(base_name,rec_num);
+
+if nargin<2 | isempty(rec_num),
+   rec_num = 1;
+end
+
+cur_dir = pwd;
+globalenvar;
+cd(IMAGE_DIR);
+cd(base_name);
+   feval([base_name,'_par']);
+   j = length(p);
+   if rec_num>j,
+      disp(sprintf('parameter record number %d out of range %d, check %s!',rec_num,j,[base_name,'_par.m']));
+      Qlabel = [];
+      v = [];
+      s = [];
+      ev_info = [];
+      return;
+   end
+   nv = p(rec_num).num_eigvecs;
+   no_rep = (p(rec_num).offset<1e-6);
+   
+   % read the image
+   cm=sprintf('I = readppm(''%s.ppm'');',base_name);
+   eval(cm);
+
+   % read eigenvectors   
+   base_name_hist = sprintf('%s_%d_IC',base_name,rec_num);
+   if no_rep,
+      [v,ev_info] = read_ev_pgm(base_name_hist,1,1,nv);
+   else
+      [v,ev_info] = read_ev_pgm2(base_name_hist,1,1,nv);
+   end
+   s = ev_info(4,:)';
+   
+   % read the normalization matrix
+   d = readpfmc(sprintf('%s_%d_D_IC.pfm',base_name,rec_num));   
+cd(cur_dir);
+
+% D^(1/2)
+dd = (1./(d(:)+eps));
+
+% recover real eigenvectors
+for j = 1:nv-no_rep,
+   vmin = ev_info(1,j);
+   vmax = ev_info(2,j);
+   y = v(:,:,j).*((vmax - vmin)/256) + vmin;
+   %validity check: x = D^(1/2)y should be normalized
+   x = norm(y(:).*dd);
+   v(:,:,j) = y./x;
+end
+
+dispimg(cat(3,mean(I,3),v),[],[{'image'};cellstr(num2str(s,3))]);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/getfnames.m b/SD-VBS/common/toolbox/toolbox_basic/stella/getfnames.m
new file mode 100755
index 0000000..4990451
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/getfnames.m
@@ -0,0 +1,47 @@
+% function [fn,dn] = getfnames(direc,opt) 
+% Input:
+%    direc = directory
+%    opt = wildcat
+% Output:
+%    fn = a cell with all filenames under direc and with opt
+%    dn = a cell with all directory names under direc and with opt
+% For example, getfnames('19990910','*.jpg');
+% Set IS_PC according to your platform in globalenvar.m
+
+% Stella X. Yu, 2000.
+
+function [fn,dn] = getfnames(direc,opt)
+
+if (nargin<1 | isempty(direc)),
+    direc = '.';
+end
+
+if nargin<2 | isempty(opt),
+    opt = [];
+end 
+
+fn = {};
+dn = {};
+
+cur_dir = pwd;
+cd(direc);
+s = dir(opt);
+if isempty(s),
+   disp('getfnames: no data');
+   return;
+end
+ 
+n = length(s);
+i = 1; 
+j = 1; 
+for k=1:n,
+   if s(k).isdir,
+      dn{j,1} = s(k).name;
+      j = j + 1;
+   else
+      fn{i,1} = s(k).name;
+      i = i + 1;
+   end
+end
+      cd(cur_dir)
+%[fn{1:n,1}]=deal(s.name);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/getimage2.m b/SD-VBS/common/toolbox/toolbox_basic/stella/getimage2.m
new file mode 100755
index 0000000..945ddd2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/getimage2.m
@@ -0,0 +1,46 @@
+% function f = getimage2(imagefile) returns a normalized intensity image.

+% If the file postfix is not given, then I will search any possible image file

+% under the IMAGE_DIR.

+

+% Stella X. Yu, March 1999

+

+function f = getimage2(imagefile)

+

+if exist(imagefile)==2,

+   g = {imagefile};

+else

+   g = {};

+end

+globalenvar;

+g = [g; getfnames(IMAGE_DIR,[imagefile,'.*'])];

+

+j = 1;

+for i=1:length(g),

+   k = findstr(g{i},'.');

+   gp = g{i}(k(end)+1:end);

+   if strcmp(gp,'ppm'),

+      f = double(readppm(g{i}));

+      j = 0;

+   elseif sum(strcmp(gp,{'jpg','tif','jpeg','tiff','bmp','png','hdf','pcx','xwd'}))>0,

+      f = double(imread(g{i}));

+      j = 0;

+   end

+   if j==0,

+      disp(sprintf('This is an image read from %s under %s',g{i},IMAGE_DIR));

+      break;

+   end

+end

+if j,

+   f = [];

+   disp('Image not found');

+   return;

+end

+         

+if size(f,3)>1,

+    %f = sum(f,3)./3;

+   f = rgb2ntsc(f);

+   f = f(:,:,1);

+end

+minf = min(f(:));

+maxf = max(f(:));

+f = (f - minf) ./ (maxf - minf);

diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/globalenvar.m b/SD-VBS/common/toolbox/toolbox_basic/stella/globalenvar.m
new file mode 100755
index 0000000..1e61b61
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/globalenvar.m
@@ -0,0 +1,6 @@
+% globalenvar
+
+HOME_DIR = '/hid/jshi/Research/walking/stella';
+IMAGE_DIR = '/hid/jshi/test_images';
+C_DIR = '/hid/jshi/Research/Ncut_code_C';
+IS_PC = 0;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/jshincut.m b/SD-VBS/common/toolbox/toolbox_basic/stella/jshincut.m
new file mode 100755
index 0000000..d0f11cb
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/jshincut.m
@@ -0,0 +1,94 @@
+% function [par, rec_num] = jshincut(par,image_dir)
+% Input:
+%   par = a structure with parameters for command_ncut.tex
+%   image_dir = the directory where the imagefile is stored
+% Output:
+%   par = parameters used
+%   rec_num = record number in the NCut database
+% Jianbo Shi's ncut_IC is applied to the image
+%   (If there is no .ppm format for the named image,
+%   conversion from related files would be attempted.)
+%   Results are organized according to the parameters.
+% Example: jshincut('240018s');
+% See also: jshincutdefpar; ncutcheckin
+% Set IS_PC according to your platform in globalenvar.m
+
+% Stella X. Yu, 2000.
+
+function [par,rec_num] = jshincut(par,image_dir)
+
+rec = jshincutdefpar;
+
+fields = fieldnames(rec);
+nf = length(fields);
+
+if ischar(par),
+   imagename = par; 
+   par = rec;
+   par.fname_base = imagename;
+end
+
+globalenvar;
+   
+if nargin<2 | isempty(image_dir),
+   image_dir = IMAGE_DIR;
+end
+
+imagename = getfield(par,fields{1});
+for i=2:nf,
+   t = getfield(par,fields{i});
+   if isempty(t),
+      par = setfield(par,fields{i},getfield(rec,fields{i}));
+   end
+end
+
+% dir and filename 
+catchar = {'/','\'};
+catchar = catchar{IS_PC+1};
+
+% first check if there is a ppm file for this image
+if not(exist([image_dir,catchar,imagename,'.ppm'])),
+   j = getfnames(image_dir,[imagename,'.*']);
+   if isempty(j),
+      disp('Image not found.');
+      return;
+   end
+   k = 0;
+   for i=1:length(j),
+      k = k + not(isempty(im2ppm(j{i},image_dir)));
+      if k==1,
+         disp(sprintf('%s -> %s.ppm succeeded.',j{i},imagename));
+         break;
+      end
+   end
+   if k==0,
+      disp('Sorry.  Attempt to convert your named image into ppm format failed.');
+      return;
+   end
+end
+
+cd(C_DIR);
+
+% generate command_ncut.tex file
+fn = 'command_ncut.tex';
+fid = fopen(fn,'w');
+fprintf(fid,'%21s\t%s%c%s\n',fields{1},image_dir,catchar,imagename);
+for i=2:nf,
+   t = getfield(par,fields{i});
+   if isnumeric(t),
+      t = num2str(t);
+   end
+   fprintf(fid,['%21s\t%s\n'],fields{i},t);
+end
+fclose(fid);
+%disp('You can check and modify command_ncut.tex before I run ncut_IC on it.  Good?');pause(1);
+
+% run ncut_IC
+unix(['.',catchar,'ncut_IC']);
+cd(HOME_DIR);
+
+% check in
+copyfile([C_DIR,catchar,fn],[image_dir,catchar,fn]);
+rec_num = ncutcheckin(fn,image_dir,image_dir);
+%delete([image_dir,catchar,imagename,'.ppm']);
+%delete([image_dir,catchar,fn]);
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/jshincutdefpar.m b/SD-VBS/common/toolbox/toolbox_basic/stella/jshincutdefpar.m
new file mode 100755
index 0000000..4f07192
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/jshincutdefpar.m
@@ -0,0 +1,20 @@
+% function rec = jshincutdefpar;
+% default parameter setting for Jianbo Shi's NCut C codes
+
+% Stella X. Yu, 2000.
+
+function rec = jshincutdefpar;
+
+rec.fname_base =     '240018s'; 
+rec.fname_ext =          'ppm'; 
+rec.num_eigvecs =          15;  
+rec.spatial_neighborhood_x=20;  
+rec.sigma_x=               10;  
+rec.sig_I=                 -0.16;
+rec.sig_IC=                0.01;
+rec.hr=                    2;
+rec.eig_blk_sze=           3;
+rec.power_D=               1;
+rec.offset =               0.0;
+rec.sig_filter =           1.0;
+rec.elong_filter =         3.0;
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/ncutcheckin.m b/SD-VBS/common/toolbox/toolbox_basic/stella/ncutcheckin.m
new file mode 100755
index 0000000..cd82ee5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/ncutcheckin.m
@@ -0,0 +1,136 @@
+% function rec_num = ncutcheckin(fn,sdir,tdir)
+% Input:
+%    fn = parameter file name, default = 'command_ncut.tex'
+%    sdir = source dir for fn as well as data files
+%    tdir = target dir to check in, both default = IMAGE_DIR
+% Output:
+%    rec_num = the number of current parameter records
+%    The imagefile_par.m is updated if fn contains a new 
+%    parameter set.  Data files are tagged and copied over to 
+%    a subdir under tdir.
+% Example: ncutcheckin;
+% Set IS_PC, IMAGE_DIR according to your platform in globalenvar.m
+
+% Stella X. Yu, 2000.
+
+function rec_num = ncutcheckin(fn,sdir,tdir)
+
+globalenvar;
+
+cur_dir = pwd;
+
+if nargin<1 | isempty(fn),
+   fn = 'command_ncut.tex';
+end
+
+if nargin<2 | isempty(sdir),
+   sdir = IMAGE_DIR;
+end
+
+if nargin<3 | isempty(tdir),
+   tdir = IMAGE_DIR;
+end
+
+rec = jshincutdefpar;
+
+% first, generate a parameter record from fn
+cd(sdir);
+[names,values] = textread(fn,'%s %s','commentstyle','shell');
+n = length(names);
+s = rec;
+for i=1:n,
+   j = str2num(values{i});
+   if isempty(j),
+      s = setfield(s,names{i},values{i});
+   else
+      s = setfield(s,names{i},j);
+   end
+end
+cd(cur_dir);
+
+% special care to extract the image file name
+imagename = getfield(s,names{1});
+catchar = {'/','\'};
+catchar = catchar{IS_PC + 1};
+k = max([0,findstr(imagename,catchar)]);
+imagename = imagename(k+1:end);
+s = setfield(s,names{1},imagename);
+
+% second, check if the target dir contains a profile for the image
+cd(tdir);
+if not(exist(imagename,'dir')),
+   mkdir(imagename);
+   cd(cur_dir);
+   j = [catchar,imagename,'.',getfield(s,names{2})];
+   copyfile([sdir,j],[tdir,catchar,imagename,j]);
+   cd(tdir);
+end
+cd(imagename);
+j = [imagename,'_par'];
+if not(exist(j)),
+   rec_num = 1;
+   p = s;
+else
+   % load par file
+   feval(j);    
+   rec_num = length(p);
+   i = 1;
+   while (i<=rec_num),
+      k = 0;
+      for j=1:n,
+         k = k + sum(getfield(s,names{j})-getfield(p(i),names{j}));
+      end
+      if k==0,
+         if not(isempty(input(['Data already existed as record # ',num2str(i),...
+                  '\nPress any non-return key to Overwrite'],'s'))),
+            break;
+         else
+            rec_num = i; % have checked in already, no update
+            cd(cur_dir);
+            return;
+         end
+      else
+         i = i + 1;
+      end
+   end            
+   rec_num = i; % new parameter setting
+   p(rec_num)=s;
+end
+tdir = [tdir,catchar,imagename];
+cd(cur_dir);
+
+% third, check in data files 
+% leave .ppm and _edgecon, _phase files 
+% if not(exist([tdir,catchar,imagename,'.ppm'])),
+%   copyfile([sdir,catchar,imagename,'.ppm'],[tdir,catchar,imagename,'.ppm']);
+% end
+
+% IC files only
+dn = getfnames(sdir,[imagename,'*_IC*.*']);
+header = sprintf('%s%c%s_%d_',tdir,catchar,imagename,rec_num);
+j = length(imagename)+2;
+k = length(dn);
+for i=1:k,
+   copyfile([sdir,catchar,dn{i}],[header,dn{i}(j:end)]);
+   delete([sdir,catchar,dn{i}]);
+end
+disp(sprintf('%d files checked in as record #%d',k,rec_num));
+
+
+% finally, update parameter file
+cd(tdir);
+fid = fopen([imagename,'_par.m'],'w');
+fprintf(fid,'%% Last checked in at %s\n\n',datestr(now));
+for i=1:rec_num,
+   for j=1:n,
+      k = getfield(p(i),names{j});
+      if ischar(k),
+         fprintf(fid,'p(%d).%s=\''%s\'';\n',i,names{j},k);
+      else
+         fprintf(fid,'p(%d).%s=%s;\n',i,names{j},num2str(k));
+      end
+   end
+   fprintf(fid,'\n');
+end
+fclose(fid);      
+cd(cur_dir);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/openfigure.m b/SD-VBS/common/toolbox/toolbox_basic/stella/openfigure.m
new file mode 100755
index 0000000..e677014
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/openfigure.m
@@ -0,0 +1,52 @@
+% function openfigure(m,n,caption,isnew)

+function h = openfigure(m,n,caption,isnew)

+

+if nargin<3,

+    caption = ' ';

+end

+

+if nargin<4,

+    isnew = 1;

+end

+

+if (m<=0 | n<=0)

+    return;

+end

+

+if isnew,

+    h = figure; colormap(gray);

+else

+    h = gcf;

+end

+clf

+

+subplot('position',[0,0,0.1,0.1]); axis off;

+text(0.1,0.15,sprintf('S. X. Yu, %s',date),'FontSize',8);

+

+subplot('position',[0,0.96,0.1,0.1]); axis off;

+text(0.1,0.15,caption,'FontSize',8);

+

+subplot(m,n,1); 

+%return

+

+if (m==1 & n==1),

+    return;

+end

+

+%set(gcf,'PaperPosition',[0.25, 8, 8,2.5*m]);

+%     set(gcf,'PaperPosition',[0.25,0.25,8,10.5]);

+%return

+

+if (m<=n),

+     set(gcf,'PaperOrientation','landscape','PaperPosition',[0.25,0.25,10.5,8]);

+else

+     set(gcf,'PaperPosition',[0.25,0.25,8,10.5]);

+end

+

+% comment on PaperPosition

+% [a,b,c,d]

+% (a,b) is the coordinate of the lower-left corner of the figure

+% (a,b) = (0,0) is the lower-left corner of the paper

+% (c,d) is the coordinate of the upper-right corner of the figure relative to the lower-left corner of the figure

+% Therefore, c>=a, d>=b

+% Full paper position would be [0,0,8.5,11] in inches

diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/showim.m b/SD-VBS/common/toolbox/toolbox_basic/stella/showim.m
new file mode 100755
index 0000000..10db297
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/showim.m
@@ -0,0 +1,36 @@
+% function showim(f,cmap) display real or complex image.

+% When it is complex, the real part and imaginary part

+% are displayed as [real,imag] in one image.

+% cmap is the colormap. default = gray, -1 = inverted gray.

+

+% Stella X. Yu, 2000. 

+

+function showim(f,cmap,ishori)

+

+if not(isreal(f)),

+   i = [real(f(:)); imag(f(:))];

+   j = [min(i), max(i)];

+   [nr,nc] = size(f);

+   if nargin<3 | isempty(ishori),

+      ishori =  nr>nc;

+   end

+   if ishori,

+      i = zeros(nr,1);

+      f = [real(f), [i+j(1),i+j(2)], imag(f)];

+   else

+      i = zeros(1,nc);

+      f = [real(f); [i+j(1);i+j(2)]; imag(f)];

+   end

+end

+imagesc(f); axis off; axis image; 

+

+if nargin<2 | isempty(cmap),

+   return;

+end

+

+if cmap==1,

+   cmap = gray;

+elseif cmap==-1,

+   cmap = flipud(gray);

+end

+colormap(cmap);
\ No newline at end of file
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/showncut.m b/SD-VBS/common/toolbox/toolbox_basic/stella/showncut.m
new file mode 100755
index 0000000..b1fe1f4
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/showncut.m
@@ -0,0 +1,92 @@
+% function [g,lgd,v,s,dd] = showncut(fn,rec_num)
+% Input:
+%    fn = file / image name
+%    rec_num = Ncut record number
+% Output:
+%    g = a cell contains 1D, 2D and 3D embeddings
+%    lgd = legend for g
+%    v = eigenvectors
+%    s = eigenvalues
+%    dd = normalization matrix  = 1/sqrt(rowsum(abs(a))) 
+%    an image is displayed
+
+function [g,lgd,v,s,dd] = showncut(fn,rec_num)
+
+globalenvar; cd(IMAGE_DIR);cd(fn); feval([fn,'_par']);cd(HOME_DIR);
+par = p(rec_num);
+no_rep = (par.offset<1e-6);
+                   
+[v,s,dd] = firstncut(fn,rec_num);
+[m,n,nc] = size(v);
+
+% generate images for display
+nr = 5;
+num_plots = nc * nr;
+g = cell(num_plots,1);
+lgd = g;
+names = {'r','\theta','\phi'};
+x = cell(3,1);
+for j=1:nc,
+    g{j} = v(:,:,j);
+    lgd{j} = sprintf('%s_{%d} = %1.2f','\lambda', j+no_rep, s(j));
+
+    if j<nc,
+       [x{2},x{1}] = cart2pol(v(:,:,j),v(:,:,j+1));
+       k = j;
+       for t=1:2,
+           k = k + nc;
+           g{k} = x{t};
+           lgd{k} = sprintf('%s_{%d,%d}',names{t},j+[0:1]+no_rep);
+       end
+       
+       if j<nc-1,
+          [x{2},x{3},x{1}] = cart2sph(v(:,:,j),v(:,:,j+1),v(:,:,j+2));
+          for t=[1,3], % theta must be the same as 2D embedding, so ignore it
+              k = k + nc;
+              g{k} = x{t};
+              lgd{k} = sprintf('%s_{%d,%d,%d}',names{t},j+[0:2]+no_rep);
+          end
+       end
+    end
+end
+
+% fill in slots by image f and affinity pattern
+j = nc + nc; g{j} = getimage2(fn); lgd{j} = sprintf('%d x %d image',m,n);
+j = nr * nc; g{j} = readpcm([fn,'_phase.pfm']); lgd{j} = 'phase';
+j = j - 1;  g{j} = exp(-(readpfmc([fn,'_edgecon.pfm'])/(255*par.sig_IC)).^2); lgd{j} = 'IC';
+
+i = round(m*[1;3]./4);
+%i = i([1,1,2,2]);
+j = round(n*[1;3]./4);
+%j = j([1,2,1,2]);
+k = m * (j-1) + i;
+
+a = afromncut(v,s,dd,1,no_rep,k);
+
+y = [4*nc-1, 4*nc, 5*nc-1, 5*nc, 6*nc-1, 6*nc]; 
+for t=1:length(k),
+    g{y(t)} = reshape(a(t,:),[m,n]);
+    lgd{y(t)} = sprintf('a at (%d,%d)',i(t),j(t));    
+end
+
+% find parameters
+fg_title = sprintf('%s:  %s=%d, %s=%d, %s=%3.2f, %s=%3.2f',...
+par.fname_base,...
+'r_x', par.spatial_neighborhood_x,...
+'\sigma_x',par.sigma_x,...
+'\sigma_{IC}',par.sig_IC,...
+'repulsion',par.offset);
+
+openfigure(nr,nc,fg_title,0); 
+dispimg(g,[nr,nc],lgd);
+
+% fix
+subplot(nr,nc,nc*3);
+plot(s,'ro'); title('\lambda');
+axis square; axis tight; set(gca,'XTick',[]);
+for t=1:length(k),
+    subplot(nr,nc,y(t));
+    hold on;
+    text(j(t),i(t),'+');
+end
+hold off
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/startup.m b/SD-VBS/common/toolbox/toolbox_basic/stella/startup.m
new file mode 100755
index 0000000..262429a
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/startup.m
@@ -0,0 +1,18 @@
+globalenvar;
+
+addpath(IMAGE_DIR);
+
+addpath(HOME_DIR);
+
+path(path,['/hid/jshi/Research/walking']);
+
+tb_dir = ['/hid/jshi/matlab/toolbox/'];
+
+path(path,[tb_dir,'io']);
+path(path,[tb_dir,'filter']);
+path(path,[tb_dir,'filter_hist']);
+path(path,[tb_dir,'disp']);
+path(path,[tb_dir,'common']);
+path(path,[tb_dir,'textons']);
+path(path,[tb_dir,'pyramid']);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/stella/test_ncutm.m b/SD-VBS/common/toolbox/toolbox_basic/stella/test_ncutm.m
new file mode 100755
index 0000000..c9b46ab
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/stella/test_ncutm.m
@@ -0,0 +1,38 @@
+fn = 'walk1';
+
+repulsion_test = 1;
+
+if 1,
+   f = getimage2(fn);
+   par = jshincutdefpar;
+   par.fname_base = fn;
+   par.spatial_neighborhood_x = 10;
+   par.sigma_x = 3 * par.spatial_neighborhood_x;
+   par.sig_IC = 0.03;
+   par.num_eigvecs = 10;
+   par.offset = 0.00;
+   par.sig_filter = 1.0;
+   par.elong_filter = 3.0;
+   [par,rec_num] = jshincut(par);
+   [g,lgd,v,s,dd] = showncut(fn,rec_num);
+
+if repulsion_test,
+   par.offset = 0.1;
+   [par,rec_num] = jshincut(par);
+   figure;
+   [g,lgd,v,s,dd] = showncut(fn,rec_num);
+end
+end
+
+if 0,
+   x = v(:,:,1);
+   y = v(:,:,2);
+   figure;
+   subplot(2,1,1); plot(x(:),y(:),'ro');
+   r = sqrt(x.^2+y.^2);
+   x = x./r;
+   y = y./r;
+   subplot(2,1,2); im([x,y]*[x,y]');
+ %  mask = (x>0) & y>0;
+ %  showmask(f,mask);
+end
diff --git a/SD-VBS/common/toolbox/toolbox_basic/tars/TOOLBOX_calib.tar b/SD-VBS/common/toolbox/toolbox_basic/tars/TOOLBOX_calib.tar
new file mode 100755
index 0000000..bb418a5
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/tars/TOOLBOX_calib.tar
diff --git a/SD-VBS/common/toolbox/toolbox_basic/textons/dist2.m b/SD-VBS/common/toolbox/toolbox_basic/textons/dist2.m
new file mode 100755
index 0000000..f2d93e1
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/textons/dist2.m
@@ -0,0 +1,27 @@
+function n2 = dist2(x, c)
+%DIST2  Calculates squared distance between two sets of points.
+%
+%       Description
+%       D = DIST2(X, C) takes two matrices of vectors and calculates the
+%       squared Euclidean distance between them.  Both matrices must be of
+%       the same column dimension.  If X has M rows and N columns, and C has
+%       L rows and N columns, then the result has M rows and L columns.  The
+%       I, Jth entry is the  squared distance from the Ith row of X to the
+%       Jth row of C.
+%
+%       See also
+%       GMMACTIV, KMEANS, RBFFWD
+%
+
+%       Copyright (c) Christopher M Bishop, Ian T Nabney (1996, 1997)
+
+[ndata, dimx] = size(x);
+[ncentres, dimc] = size(c);
+if dimx ~= dimc
+        error('Data dimension does not match dimension of centres')
+end
+
+n2 = (ones(ncentres, 1) * sum((x.^2)', 1))' + ...
+                ones(ndata, 1) * sum((c.^2)',1) - ...
+                2.*(x*(c'));
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons.m b/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons.m
new file mode 100755
index 0000000..e7fa4b0
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons.m
@@ -0,0 +1,46 @@
+function [centers,label,post,d2]=find_textons(FIw,ncenters,centers_in,n_iter);
+% [centers,label,post,d2]=find_textons(FIw,ncenters,centers_in,n_iter);
+% 
+% find textons using kmeans for windowed portion FIw of filtered image 
+%
+% to start with centers pulled randomly from image, set centers_in=[]
+
+% define number of textons
+%ncenters=25;
+
+[N1,N2,N3]=size(FIw);
+% reshape filtered image stack into a long array of feature vectors
+fv=reshape(FIw,N1*N2,N3);
+% (each row is a feature vector)
+
+%centers=.01^2*randn(ncenters,N3);
+% take centers randomly from within image
+if isempty(centers_in)
+   rndnum=1+floor(N1*N2*rand(1,ncenters));
+   centers_in=fv(rndnum,:);
+end
+
+options = foptions;
+options(1)=1;           % Prints out error values.
+options(5) = 0;
+if nargin<4
+   n_iter=15;
+end
+options(14) = n_iter;           % Number of iterations.
+
+[centers,options,d2,post]=kmeans2(centers_in,fv,options);
+
+% reshuffle the centers so that the one closest to the origin
+% (featureless) comes last
+norms=sum(centers.^2,2);
+[sortval,sortind]=sort(-norms);
+centers=centers(sortind,:);
+d2=reshape(d2,N1,N2,ncenters);
+post=reshape(post,N1,N2,ncenters);
+d2=d2(:,:,sortind);
+post=post(:,:,sortind);
+
+
+% retrieve cluster number assigned to each feature vector
+[minval,label]=min(d2,[],3);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons1.m b/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons1.m
new file mode 100755
index 0000000..b192015
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/textons/find_textons1.m
@@ -0,0 +1,37 @@
+function [centers,label,post,d2]=find_textons(fv,ncenters,centers_in,n_iter);
+% [centers,label,post,d2]=find_textons(FIw,ncenters,centers_in,n_iter);
+% 
+% find textons using kmeans for windowed portion FIw of filtered image 
+%
+% to start with centers pulled randomly from image, set centers_in=[]
+
+[N1,N2] =size(fv);
+
+% take centers randomly from within image
+if isempty(centers_in)
+   rndnum=1+floor(N1*rand(1,ncenters));
+   centers_in=fv(rndnum,:);
+end
+
+options = foptions;
+options(1)=1;           % Prints out error values.
+options(5) = 0;
+if nargin<4
+   n_iter=15;
+end
+options(14) = n_iter;           % Number of iterations.
+
+[centers,options,d2,post]=kmeans2(centers_in,fv,options);
+
+
+% retrieve cluster number assigned to each feature vector
+[minval,label]=min(d2,[],2);
+
+
+h = hist(label(:),[1:max(label(:))]);
+a = h>0;
+a = cumsum(a);
+
+[nr,nc] = size(label);
+label = reshape(a(label(:)),nr,nc);
+
diff --git a/SD-VBS/common/toolbox/toolbox_basic/textons/kmeans2.m b/SD-VBS/common/toolbox/toolbox_basic/textons/kmeans2.m
new file mode 100755
index 0000000..0bd87b2
--- /dev/null
+++ b/SD-VBS/common/toolbox/toolbox_basic/textons/kmeans2.m
@@ -0,0 +1,127 @@
+function [centres, options, d2, post, errlog] = kmeans2(centres, data, options)
+%KMEANS Trains a k means cluster model.
+%
+%       Description
+%        CENTRES = KMEANS(CENTRES, DATA, OPTIONS) uses the batch K-means
+%       algorithm to set the centres of a cluster model. The matrix DATA
+%       represents the data which is being clustered, with each row
+%       corresponding to a vector. The sum of squares error function is used.
+%       The point at which a local minimum is achieved is returned as
+%       CENTRES.  The error value at that point is returned in OPTIONS(8).
+%
+%       [CENTRES, OPTIONS, POST, ERRLOG] = KMEANS(CENTRES, DATA, OPTIONS)
+%       also returns the cluster number (in a one-of-N encoding) for each
+%       data point in POST and a log of the error values after each cycle in
+%       ERRLOG.    The optional parameters have the following
+%       interpretations.
+%
+%       OPTIONS(1) is set to 1 to display error values; also logs error
+%       values in the return argument ERRLOG. If OPTIONS(1) is set to 0, then
+%       only warning messages are displayed.  If OPTIONS(1) is -1, then
+%       nothing is displayed.
+%
+%       OPTIONS(2) is a measure of the absolute precision required for the
+%       value of CENTRES at the solution.  If the absolute difference between
+%       the values of CENTRES between two successive steps is less than
+%       OPTIONS(2), then this condition is satisfied.
+%
+%       OPTIONS(3) is a measure of the precision required of the error
+%       function at the solution.  If the absolute difference between the
+%       error functions between two successive steps is less than OPTIONS(3),
+%       then this condition is satisfied. Both this and the previous
+%       condition must be satisfied for termination.
+%
+%       OPTIONS(14) is the maximum number of iterations; default 100.
+%
+%       See also
+%       GMMINIT, GMMEM
+%
+
+%       Copyright (c) Christopher M Bishop, Ian T Nabney (1996, 1997)
+
+[ndata, data_dim] = size(data);
+[ncentres, dim] = size(centres);
+
+if dim ~= data_dim
+  error('Data dimension does not match dimension of centres')
+end
+
+if (ncentres > ndata)
+  error('More centres than data')
+end
+
+% Sort out the options
+if (options(14))
+  niters = options(14);
+else
+  niters = 100;
+end
+
+store = 0;
+if (nargout > 3)
+  store = 1;
+  errlog = zeros(1, niters);
+end
+
+% Check if centres and posteriors need to be initialised from data
+if (options(5) == 1)
+  % Do the initialisation
+  perm = randperm(ndata);
+  perm = perm(1:ncentres);
+
+  % Assign first ncentres (permuted) data points as centres
+  centres = data(perm, :);
+end
+% Matrix to make unit vectors easy to construct
+id = eye(ncentres);
+
+% Main loop of algorithm
+for n = 1:niters
+
+  % Save old centres to check for termination
+  old_centres = centres;
+  
+  % Calculate posteriors based on existing centres
+  d2 = dist2(data, centres);
+  % Assign each point to nearest centre
+  [minvals, index] = min(d2', [], 1);
+  post = id(index,:);
+
+  num_points = sum(post, 1);
+  % Adjust the centres based on new posteriors
+  for j = 1:ncentres
+    if (num_points(j) > 0)
+      centres(j,:) = sum(data(find(post(:,j)),:), 1)/num_points(j);
+    end
+  end
+
+  % Error value is total squared distance from cluster centres
+  e = sum(minvals);
+  tmp = sort(minvals);
+  e95 = sqrt(tmp(round(length(tmp) * 0.95)));
+  erms = sqrt(e/ndata);
+  if store
+    errlog(n) = e;
+  end
+  if options(1) > 0
+    fprintf(1, '   Cycle %4d  RMS Error %11.6f  95-tier Error %11.6f\n', n, erms,e95);
+  end
+
+  if n > 1
+    % Test for termination
+    if max(max(abs(centres - old_centres))) < options(2) & ...
+        abs(old_e - e) < options(3)
+      options(8) = e;
+      return;
+    end
+  end
+  old_e = e;
+end
+
+% If we get here, then we haven't terminated in the given number of 
+% iterations.
+options(8) = e;
+%if (options(1) >= 0)
+%  disp('Warning: Maximum number of iterations has been exceeded');
+%end
+