diff --git a/misc/matlab/orientation/ld_compute_orientation.m b/misc/matlab/orientation/ld_compute_orientation.m index adc683375f8ba46c495038d80dddcc2309d7a69e..dba76289cd59d9f3326ecf901d286d30a6974828 100644 --- a/misc/matlab/orientation/ld_compute_orientation.m +++ b/misc/matlab/orientation/ld_compute_orientation.m @@ -22,7 +22,12 @@ ld.points = [ cos(ld.theta') .* ld.readings'; sin(ld.theta').* ld.readings']; n = size(ld.points,2); for i=1:n - + if ld.valid(i) == 0 + ld.alpha(i) = nan; + ld.alpha_valid(i) = 0; + ld.alpha_error(i) = nan; + continue; + end if i==1 min_dist = norm(ld.points(:,i)-ld.points(:,i+1)); @@ -31,7 +36,7 @@ for i=1:n min_dist = norm(ld.points(:,i)-ld.points(:,i-1)); else min_dist = min( norm(ld.points(:,i)-ld.points(:,i-1)), ... - norm(ld.points(:,i)-ld.points(:,i+1))); + norm(ld.points(:,i)-ld.points(:,i+1))); end end @@ -49,6 +54,7 @@ for i=1:n imin=i; for j=max(1,i-1):-1:max(1,i-MAX) + d = norm( ld.points(:,j)- ld.points(:,j+1) ); if d > THRESHOLD * min_dist break; @@ -56,8 +62,6 @@ for i=1:n imin=j; end -% imin = max(1, i-3); -% imax = min(n, i+3); ni=imax-imin+1; @@ -97,6 +101,11 @@ for i=1:n alpha_var = fd0_var * (( f0 / (f0.^2 + fd0.^2) ).^2) ... + var * (( fd0 / (f0.^2 + fd0.^2) ).^2) ; + if isnan(alpha) + fprintf('bug here... \n') + pause + end + ld.alpha(i) = alpha-pi; % rivolta verso dentro ld.alpha_valid(i) = 1; ld.alpha_error(i) = alpha_var; diff --git a/misc/matlab/utils/params_set_default.m b/misc/matlab/utils/params_set_default.m index 8eb362ff53bfb64706283d359254ee3f6d1b8007..d1f3a62939ec129dfd823257ea279f3c01516f10 100644 --- a/misc/matlab/utils/params_set_default.m +++ b/misc/matlab/utils/params_set_default.m @@ -2,9 +2,9 @@ function res = params_set_default(p, field, default_value) % Checks whether the field is contained in p; if not, it adds the default_value. if not(isfield(p, field)) p = setfield(p, field, default_value); - fprintf('Setting default for %s = ', field); - fprintf('%f ', default_value); - fprintf('\n'); + % fprintf('Setting default for %s = ', field); + % fprintf('%f ', default_value); + % fprintf('\n'); end res = p; diff --git a/misc/matlab/vis/ld_plot.m b/misc/matlab/vis/ld_plot.m index 52b7ffd561318bbcd397e5ab797d9456e373eda5..3a6a424dd44d7036feb72bfa2a980216637d6f6e 100644 --- a/misc/matlab/vis/ld_plot.m +++ b/misc/matlab/vis/ld_plot.m @@ -8,6 +8,8 @@ function res = ld_plot(ld, params) % params.rototranslated (= true); if true, the scan is drawn % rototranslated at ld.estimate, else is drawn at 0; % params.rototranstated_odometry = false; +% +% ld.dr -> tangent vector ld.points = [ cos(ld.theta') .* ld.readings'; sin(ld.theta').* ld.readings']; @@ -108,3 +110,26 @@ function res = ld_plot(ld, params) plotVectors( reference, [from to] , color); end end + + if isfield(ld,'dr') + % disegno normali + maxLength = 0.3; + + dr = ld.dr / max(abs(ld.dr)) * maxLength; + + for i=1:ld.nrays + theta = ld.theta(i); + readings = ld.readings(i); + alpha = ld.alpha(i); + + %len = [cos(theta);sin(theta)]' * [cos(alpha);sin(alpha)] * dr(i); + %dir = alpha; + len = dr(i); + dir = theta; + + from = readings*[cos(theta);sin(theta)]; + to = from + [cos(dir);sin(dir)] * len; + plotVectors( reference, [from to] , 'g-'); + end + end + diff --git a/scripts/fig2pics.rb b/scripts/fig2pics.rb index 0fd99102e5174a4aaa75a84aa173856a71ccb925..14b51ded0f5e52d9a9d470507ce70b5bfa4517cf 100755 --- a/scripts/fig2pics.rb +++ b/scripts/fig2pics.rb @@ -206,6 +206,7 @@ if opt_bounds then change_bounds(slideps, opt_bounds) end if opt_output then outpdf = opt_output end + execute_cmd("epstopdf #{slideps} --outfile=#{outpdf} --debug", exit_on_error=true) execute_cmd "rm -f #{slide} #{slidedvi} #{slideps} #{temp}.* #{slideprefix}.*" diff --git a/sm/apps/json_decimate.c b/sm/apps/json_decimate.c index 1e96b433b72619e104204a08126a367907ff5bd6..5a39946a1e90d0b213dfb3fd6091768b6ab07b2c 100644 --- a/sm/apps/json_decimate.c +++ b/sm/apps/json_decimate.c @@ -34,9 +34,9 @@ int main(int argc, const char * argv[]) { int count = 0; while(1) { - JO jo = json_read_stream(stdin); + JO jo = json_read_stream(input_stream); if(!jo) { - if(feof(stdin)) break; + if(feof(input_stream)) break; sm_error("Malformed JSON\n"); return -1; }