No commit message

No commit message

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;

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;
 

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
+	

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}.*"

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;
 		}