For 2 bad markers use ICP not rot3dfit

When there are two adjacent bad markers removed, rot3dfit does not work
well (see #7). Therefore an alternative point to point ICP method has
been implemented to give an alternative transformation matrix. This
seems to give an identical solution compared with rot3dfit (with 5
markers) and much better solution for 3 markers. Workaround for #7.
Thoughts @paulsowman?

MEMES3.m

@@ -193,13 +193,22 @@ function MEMES3(dir_name,elpfile,hspfile,confile,mrkfile,path_to_MRI_library,bad
     fids_2_use = shape.fid.pnt(4:end,:); 
     % Now take out the bad marker(s) when you realign
     fids_2_use(badcoilpos,:) = [];
-
-    [R,T,Yf,Err]                = rot3dfit(markers,fids_2_use);%calc rotation transform
-    meg2head_transm             = [[R;T]'; 0 0 0 1];%reorganise and make 4*4 transformation matrix
 
+    % If there are two bad coils use the ICP method, if only one use
+    % rot3dfit as usual
     disp('Performing re-alignment');
-    grad_trans                  = ft_transform_geometry_PFS_hacked(meg2head_transm,grad_con); %Use my hacked version of the ft function - accuracy checking removed not sure if this is good or not
-    grad_trans.fid              = shape; %add in the head information
+
+    if length(bad_coil) == 2
+        [R, T, err, dummy, info]    = icp(fids_2_use', markers','Minimize', 'point');
+        meg2head_transm             = [[R T]; 0 0 0 1];%reorganise and make 4*4 transformation matrix
+        grad_trans                  = ft_transform_geometry_PFS_hacked(meg2head_transm,grad_con); %Use my hacked version of the ft function - accuracy checking removed not sure if this is good or not
+        grad_trans.fid              = shape; %add in the head information
+    else
+        [R,T,Yf,Err]                = rot3dfit(markers,fids_2_use);%calc rotation transform
+        meg2head_transm             = [[R;T]'; 0 0 0 1];%reorganise and make 4*4 transformation matrix
+        grad_trans                  = ft_transform_geometry_PFS_hacked(meg2head_transm,grad_con); %Use my hacked version of the ft function - accuracy checking removed not sure if this is good or not
+        grad_trans.fid              = shape; %add in the head information
+    end
 end
 
 % Create figure to view relignment