diff --git a/sm/apps/json_extract.c b/sm/apps/json_extract.c
index dce118885e53ac19a49684ebaf584a3a04709dd6..e3c012e28c5922f55ec252e9dabe2dae8835a03a 100644
--- a/sm/apps/json_extract.c
+++ b/sm/apps/json_extract.c
@@ -27,7 +27,7 @@ int main(int argc, const char * argv[]) {
 	
 	int i; for(i=0;i<nth;i++) {
 		if(!json_stream_skip(input_stream)) {
-			fprintf(stderr, "Could not skip %d-th object\n", i);
+			sm_error("Could not skip %d-th object\n", i);
 			return -2;
 		}
 	}
diff --git a/sm/apps/json_extract_field.c b/sm/apps/json_extract_field.c
index 97b0f3b3be6eb1f75ccd5c2b770a87e3a2de246d..8ebda07ddbc42a7701934b946b7da3bc52de7eea 100644
--- a/sm/apps/json_extract_field.c
+++ b/sm/apps/json_extract_field.c
@@ -9,10 +9,12 @@ int main(int argc, const char * argv[]) {
 	const char*input_filename;
 	const char*output_filename;
 	const char*field;
+	int exit_on_error;
 	
 	struct option* ops = options_allocate(3);
 	options_string(ops, "in", &input_filename, "stdin", "input file (JSON)");
 	options_string(ops, "out", &output_filename, "stdout", "output file (JSON)");
+	options_int(ops, "exit_on_error", &exit_on_error, 0, "if true, exit if object has no field");
 	options_string(ops, "field", &field, "field_name", "field to extract from structure");
 	
 	if(!options_parse_args(ops, argc, argv)) {
@@ -27,8 +29,10 @@ int main(int argc, const char * argv[]) {
 	
 	if(!input_stream || !output_stream) return -1;
 	
-	
+	int n=0;
 	while(1) {
+		n++;
+		
 		JO jo = json_read_stream(input_stream);
 		if(!jo) {
 			if(feof(input_stream)) break;
@@ -38,8 +42,13 @@ int main(int argc, const char * argv[]) {
 		
 		JO jo_field = jo_get(jo, field);
 		if(!jo_field) {
-			sm_error("Field '%s' not found in structure.\n", field);
-			return -1;
+			if(exit_on_error) {
+				sm_error("object #%d: field '%s' not found in structure.\n", n, field);
+				return -1;
+			} else {
+				sm_error("object #%d: field '%s' not found in structure.\n", n, field);
+				continue;
+			}
 		}
 		
 		jo_write_plain(jo_field, output_stream);
@@ -64,7 +73,7 @@ void jo_write_plain(JO jo, FILE* out) {
 		}
 		case json_type_double: {
 			double v = json_object_get_double(jo);
-			fprintf(out, "%f", v);
+			fprintf(out, "%g", v);
 			break;
 		}
 		case json_type_string: {
diff --git a/sm/apps/sm1.c b/sm/apps/sm1.c
index f0ee1ae230651da24756efaf61447ef4b994eb95..f89ef64e152dfda51bc1d9770649ed251de5eb21 100644
--- a/sm/apps/sm1.c
+++ b/sm/apps/sm1.c
@@ -13,12 +13,14 @@ struct sm1_params {
 	int debug;
 	
 	int algo;
+	int write_post_mortem;
 } p;
 
 
 extern int distance_counter;
 extern void sm_options(struct sm_params*p, struct option*ops);
 
+
 const char *sm1_banner = 
 
 "There are TWO ways to define the input to this program.\n"
@@ -67,9 +69,8 @@ int main(int argc, const char*argv[]) {
 	options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) ");
 
 	options_int(ops, "debug", &p.debug, 0, "Shows debug information");
+	options_int(ops, "write_post_mortem", &p.write_post_mortem, 1, "In case of failure, writes a post mortem.");
 
-	
-	
 	sm_options(&params, ops);
 	if(!options_parse_args(ops, argc, argv)) {
 		fprintf(stderr, "\n\nUsage:\n");
@@ -84,7 +85,6 @@ int main(int argc, const char*argv[]) {
 		: open_file_for_reading(p.file2);
 	if(!file1 || !file2) return -1;
 	
-	
 	FILE * out = open_file_for_writing(p.file_output);
 	if(!out) return -2;
 	
@@ -95,7 +95,10 @@ int main(int argc, const char*argv[]) {
 	}
 	
 	LDP ld1, ld2;
+	int count = 0;
 	while(1) {
+		count++;
+		
 		ld1 = ld_from_json_stream(file1);
 		if(!ld1) {
 			if(feof(file1)) break;
@@ -112,6 +115,15 @@ int main(int argc, const char*argv[]) {
 		params.laser_ref = ld1;
 		params.laser_sens = ld2;
 
+		if(	any_nan(params.laser_ref->odometry,3) ||  
+			any_nan(params.laser_sens->odometry,3) ) {
+				sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n");
+				sm_error(" (file %s) laser_ref->odometry = %s \n", p.file1, friendly_pose(params.laser_ref->odometry) );
+				sm_error(" (file %s) laser_sens->odometry = %s \n", p.file2, friendly_pose(params.laser_sens->odometry) );
+				sm_error(" I will quit it here. \n");
+				return 3;
+		}
+		
 		pose_diff_d( params.laser_sens->odometry,  
 		/* o minus */ params.laser_ref->odometry,
 			/* = */ params.first_guess);
@@ -129,6 +141,40 @@ int main(int argc, const char*argv[]) {
 				return -1;
 		}
 		
+		if(p.write_post_mortem && !result.valid) {
+			char casename[256];
+			sprintf(casename, "sm1_failure_matching%d", count);
+			sm_error("sm1: matching #%d failed. Writing a special case %s.\n", count, casename );
+//			save_testcase(&params)
+			
+			char file_config[256],file1[256],file2[256],file_jj[256],script[256];
+			
+			sprintf(file_config, "%s.config", casename);
+			sprintf(file1, "%s_laser_ref.json", casename);
+			sprintf(file2, "%s_laser_sens.json", casename);
+			sprintf(file_jj, "%s.journal", casename);
+			sprintf(script, "%s.sh", casename);
+			
+			FILE * f = fopen(file_config, "w");
+			options_dump(ops,f,0);
+			fclose(f);
+
+			f = fopen(file1, "w");
+			ld_write_as_json(params.laser_ref,f);
+			fclose(f);
+			
+			f = fopen(file2, "w");
+			ld_write_as_json(params.laser_sens,f);
+			fclose(f);
+			
+			f = fopen(script, "w");
+			fprintf(f, "#!/bin/bash\n");
+			fprintf(f, "%s -config %s -file1 %s -file2 %s -debug 1 -file_jj %s -write_post_mortem 0 \n", 
+				argv[0], file_config, file1, file2, file_jj);
+			fprintf(f, "sm_animate -in %s -out %s_anim.pdf \n", file_jj, casename);
+			fclose(f);
+		}
+		
 		
 		JO jo = result_to_json(&params, &result);
 
@@ -137,7 +183,6 @@ int main(int argc, const char*argv[]) {
 				params.laser_ref->true_pose, true_x);
 			double true_e[3];
 			
-			
 			pose_diff_d(result.x, true_x, true_e);
 		/*	int i=0;for(;i<3;i++) true_e[i] = result.x[i] - true_x[i];*/
 			
diff --git a/sm/apps/sm2.c b/sm/apps/sm2.c
index ec926405af93401d4fa42368d527800d080ca0ba..b8f282a627e15c136f5247f0740dc0a061d7cfd9 100644
--- a/sm/apps/sm2.c
+++ b/sm/apps/sm2.c
@@ -104,6 +104,16 @@ int main(int argc, const char*argv[]) {
 		params.laser_sens = laser_sens;
 
 		/* Set first guess as the difference in odometry */
+		
+		if(	any_nan(params.laser_ref->odometry,3) ||  
+			any_nan(params.laser_sens->odometry,3) ) {
+				sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n");
+				sm_error("  laser_ref->odometry = %s \n",  friendly_pose(params.laser_ref->odometry) );
+				sm_error("  laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) );
+				sm_error(" I will quit it here. \n");
+				return -3;
+		}
+		
 		double odometry[3];
 		pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry);
 		double ominus_laser[3], temp[3];
diff --git a/sm/apps/sm_animate.c b/sm/apps/sm_animate.c
index d5b045cd859c5a221d9192aac89d58e1011a0b33..7b0ad3b821d4c606e54737cc1297e6b0792cba62 100644
--- a/sm/apps/sm_animate.c
+++ b/sm/apps/sm_animate.c
@@ -76,6 +76,7 @@ int main(int argc, const char** argv)
 	while( (jo = json_read_stream(input)) ) {
 		char filename[100];
 		sprintf(filename, p.file_output, count);
+		sm_info("Writing frame %s \n", p.file_output);
 		if(!draw_animation(&p, jo, filename))
 			return -2;
 		count++;
@@ -151,31 +152,32 @@ int draw_animation(anim_params* p, JO jo, const char*filename) {
 		jo_read_double_array(iteration, "x_old", x_old, 3, NAN);
 		jo_read_double_array(iteration, "x_new", x_new, 3, NAN);
 
-		JO corr0 = jo_get(iteration, "corr0");
-		JO corr1 = jo_get(iteration, "corr1");
-		JO corr2 = jo_get(iteration, "corr2");
-		if(!corr1 || !corr2 || !corr0) {
-			sm_error("Iteration %d: could not read correspondences (field 'corr<i>'). Probably ICP failed here?\n", it);
-			break;
-		}
 
 		cairo_save(cr);
 			cr_ld_draw(cr, laser_ref, &(p->laser_ref_s));
 
 			ld_compute_world_coords(laser_sens, x_old);
 
-			cr_set_style(cr, &(p->corr));
-			cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
-			json_to_corr(corr0, laser_sens->corr, laser_sens->nrays);
-			cr_ld_draw_corr(cr, laser_ref, laser_sens);
-
-			cairo_set_source_rgb (cr, 1.0, 0.0, 1.0);
-			json_to_corr(corr1, laser_sens->corr, laser_sens->nrays);
-			cr_ld_draw_corr(cr, laser_ref, laser_sens);
 
-			cairo_set_source_rgb (cr, 0.0, 1.0, 0.0);
-			json_to_corr(corr2, laser_sens->corr, laser_sens->nrays);
-			cr_ld_draw_corr(cr, laser_ref, laser_sens);
+			JO corr0 = jo_get(iteration, "corr0");
+			JO corr1 = jo_get(iteration, "corr1");
+			JO corr2 = jo_get(iteration, "corr2");
+			if(!corr1 || !corr2 || !corr0) {
+				sm_error("Iteration %d: could not read correspondences (field 'corr<i>'). Probably ICP failed here?\n", it);
+			} else {
+				cr_set_style(cr, &(p->corr));
+				cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
+				json_to_corr(corr0, laser_sens->corr, laser_sens->nrays);
+				cr_ld_draw_corr(cr, laser_ref, laser_sens);
+
+				cairo_set_source_rgb (cr, 1.0, 0.0, 1.0);
+				json_to_corr(corr1, laser_sens->corr, laser_sens->nrays);
+				cr_ld_draw_corr(cr, laser_ref, laser_sens);
+
+				cairo_set_source_rgb (cr, 0.0, 1.0, 0.0);
+				json_to_corr(corr2, laser_sens->corr, laser_sens->nrays);
+				cr_ld_draw_corr(cr, laser_ref, laser_sens);
+			}
 
 			cr_set_reference(cr, x_old);
 			cr_ld_draw(cr, laser_sens, &(p->laser_sens_s));
diff --git a/sm/csm/icp/icp.c b/sm/csm/icp/icp.c
index 7ef7d22e9b577162161b059b751fc4c087ad36b4..a96838ca90973ae0e9e7c1d2abaef55f3d71d653 100644
--- a/sm/csm/icp/icp.c
+++ b/sm/csm/icp/icp.c
@@ -90,7 +90,7 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 	int iterations;
 	int nvalid;
 	if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
-		sm_error("ICP failed for some reason. \n");
+		sm_error("icp: ICP failed for some reason. \n");
 		res->valid = 0;
 		res->iterations = iterations;
 		res->nvalid = 0;
@@ -171,7 +171,6 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 			}
 		}
 	
-	
 		res->error = best_error;
 		res->iterations = iterations;
 		res->nvalid = nvalid;
diff --git a/sm/csm/icp/icp_loop.c b/sm/csm/icp/icp_loop.c
index 58ec34a8c0f370b1a905752813b71a3e9a5a5abe..f64d17a9a321c76e90c47f6494f68967bd10161a 100644
--- a/sm/csm/icp/icp_loop.c
+++ b/sm/csm/icp/icp_loop.c
@@ -12,6 +12,12 @@
 
 int icp_loop(struct sm_params*params, const double*q0, double*x_new, 
 	double*total_error, int*valid, int*iterations) {
+	if(any_nan(q0,3)) {
+		sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
+		return 0;
+	}
+		
+		
 	LDP laser_sens = params->laser_sens;
 	double x_old[3], delta[3], delta_old[3] = {0,0,0};
 	copy_d(q0, 3, x_old);
@@ -29,6 +35,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		if(JJ) jj_add_double_array("x_old", x_old, 3);
 
 		egsl_push_named("icp_loop iteration");
+		sm_debug("== icp_loop: starting iteration. %d  \n", iteration);
 
 		/** Compute laser_sens's points in laser_ref's coordinates
 		    by roto-translating by x_old */
@@ -80,11 +87,11 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		*total_error = error; 
 		*valid = num_corr_after;
 
-		sm_debug("Total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);
+		sm_debug("  icp_loop: total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);
 		
 		/* If not many correspondences, bail out */
 		if(num_corr_after < fail_perc * laser_sens->nrays){
-			sm_error("Failed: after trimming, only %d correspondences.\n",num_corr_after);
+			sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration"); /* loop context */
 			break;
@@ -92,7 +99,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 
 		/* Compute next estimate based on the correspondences */
 		if(!compute_next_estimate(params, x_old, x_new)) {
-			sm_error("Cannot compute next estimate.\n");
+			sm_error("  icp_loop: Cannot compute next estimate.\n");
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration");
 			break;			
@@ -101,7 +108,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		pose_diff_d(x_new, x_old, delta);
 		
 		{
-			sm_debug("killing. laser_sens has %d/%d rays valid,  %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
+			sm_debug("  icp_loop: killing. laser_sens has %d/%d rays valid,  %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
 			if(JJ) {
 				jj_add_double_array("x_new", x_new, 3);
 				jj_add_double_array("delta", delta, 3);
@@ -111,7 +118,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		hashes[iteration] = ld_corr_hash(laser_sens);
 		
 		{
-			sm_debug("icp_loop: it. %d  hash=%d nvalid=%d mean error = %f, x_new= %s\n", 
+			sm_debug("  icp_loop: it. %d  hash=%d nvalid=%d mean error = %f, x_new= %s\n", 
 				iteration, hashes[iteration], *valid, *total_error/ *valid, 
 				friendly_pose(x_new));
 		}
@@ -171,6 +178,9 @@ int compute_next_estimate(struct sm_params*params,
 
 	int i; int k=0;
 	for(i=0;i<laser_sens->nrays;i++) {
+		if(!laser_sens->valid[i])
+			continue;
+			
 		if(!ld_valid_corr(laser_sens,i))
 			continue;
 		
@@ -203,6 +213,9 @@ int compute_next_estimate(struct sm_params*params,
 			c[k].C[0][1] = cos_alpha*sin_alpha;
 			c[k].C[1][1] = sin_alpha*sin_alpha;
 			
+/*			sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d,  alpha_seg=%f, cos=%f sin=%f \n", k,i,
+				rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
+			
 #if 0
 			/* Note: it seems that because of numerical errors this matrix might be
 			   not semidef positive. */
@@ -309,12 +322,13 @@ int compute_next_estimate(struct sm_params*params,
 	double old_error = gpc_total_error(c, k, x_old);
 	double new_error = gpc_total_error(c, k, x_new);
 
-	sm_debug("Old error: %f  x_old= %s \n", old_error, friendly_pose(x_old));
-	sm_debug("New error: %f  x_new= %s \n", old_error, friendly_pose(x_old));
+	sm_debug("\tcompute_next_estimate: old error: %f  x_old= %s \n", old_error, friendly_pose(x_old));
+	sm_debug("\tcompute_next_estimate: new error: %f  x_new= %s \n", new_error, friendly_pose(x_new));
+	sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
 
 	double epsilon = 0.000001;
 	if(new_error > old_error + epsilon) {
-		sm_error("Something's fishy here! Old error: %lf  new error: %lf  x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
+		sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf  new error: %lf  x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
 	}
 	
 	return 1;
diff --git a/sm/csm/icp/icp_outliers.c b/sm/csm/icp/icp_outliers.c
index d595babe5baf9d1062895ecaee86a9a1040448dc..b4202e3bf606c23b713abf41a364650690874212 100644
--- a/sm/csm/icp/icp_outliers.c
+++ b/sm/csm/icp/icp_outliers.c
@@ -18,7 +18,7 @@ void visibilityTest(LDP laser_ref, const gsl_vector*u) {
 			      gvg(u,0)-laser_ref->points[j].p[0]);
 	}
 	
-	sm_debug("visibility: Found outliers: ");
+	sm_debug("\tvisibility: Found outliers: ");
 	int invalid = 0;
 	for(j=1;j<laser_ref->nrays;j++) {
 		if(!ld_valid_ray(laser_ref,j)||!ld_valid_ray(laser_ref,j-1)) continue;
@@ -67,7 +67,7 @@ void kill_outliers_double(struct sm_params*params) {
 			nkilled ++;
 		}
 	}
-	sm_debug("kill_outliers_double: killed %d correspondences\n",nkilled);
+	sm_debug("\tkill_outliers_double: killed %d correspondences\n",nkilled);
 }
 	
 /** 
@@ -150,7 +150,7 @@ void kill_outliers_trim(struct sm_params*params,  double*total_error) {
 	if(JJ) jj_add_double("error_limit_adaptive", error_limit2);
 	if(JJ) jj_add_double("error_limit", error_limit);
 	
-	sm_debug("icp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n",
+	sm_debug("\ticp_outliers: maxPerc %f error_limit: fix %f adaptive %f \n",
 		params->outliers_maxPerc,error_limit1,error_limit2);
 
 	*total_error = 0;
diff --git a/sm/csm/logging.c b/sm/csm/logging.c
index 80b72823b89875a88715890538ec31de11ceda8e..647a6dbcb6d8c1645266c59bf805db2635bc8be4 100644
--- a/sm/csm/logging.c
+++ b/sm/csm/logging.c
@@ -33,7 +33,8 @@ void check_for_xterm_color() {
 	
 	const char * term = getenv("TERM");
 	if(!term) term = "unavailable";
-	xterm_color_available = !strcmp(term, "xterm-color") || !strcmp(term, "xterm");;
+	xterm_color_available = !strcmp(term, "xterm-color") || !strcmp(term, "xterm") 
+		|| !strcmp(term, "rxvt");
 /*	sm_info("Terminal type: '%s', colors: %d\n", term, xterm_color_available); */
 }