merge second order code into A2B_matrix

calibration/src/A2B_matrix.m

@@ -2,7 +2,9 @@
 %%
 %% Copyright 2007, Aaron Heller, All Rights Reserved
 %% heller@ai.sri.com
-%% FOA Octathingy: Fernando Lopez-Lezcano 2017
+%%
+%% Octathingy first and second order code:
+%% Copyright 2017, Fernando Lopez-Lezcano
 %%
 %%  This program is free software: you can redistribute it and/or modify
 %%  it under the terms of the GNU General Public License as published by
@@ -37,7 +39,7 @@ function [A2B pv max_err] = A2B_matrix(Y, X, v_z_factor)
     %% overdetermined
     pv = fill_v_z( pv )';
     ipv = pinv(pv);
-    A2B = ([1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] * ipv)';
+    A2B = ([1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1] * ipv)';
   else
     pv = fill_v_z( pv );
     A2B = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1] / pv;
@@ -49,7 +51,13 @@ function X = measurement_dirs_z0(n)
     n = 8;
   end
   x_range = 0 : 2*pi/n : 2*pi*(n-1)/n;
-  X = [ cos(x_range)' sin(x_range)' zeros(n,1) ];
+  if (n > 4)
+    %% add second order horizontal components
+    u_range = [(0 : 4*pi/n : 2*pi*(n-1)/n) (0 : 4*pi/n : 2*pi*(n-1)/n)];
+    X = [ cos(x_range)' sin(x_range)' zeros(n,1) cos(u_range)' sin(u_range)' ];
+  else
+    X = [ cos(x_range)' sin(x_range)' zeros(n,1) ];
+  end
 end
 
 function pv = fill_v_z (pv, signs, elevation)