Example #1
0
int main(int argc, const char * argv[]) {
    sm_set_program_name(argv[0]);

    const char*input_filename;
    const char*output_pattern_op;

    struct option* ops = options_allocate(3);
    options_string(ops, "in", &input_filename, "stdin", "input file (JSON)");
    options_string(ops, "out", &output_pattern_op, "./ld_split^02d.json", "output pattern; printf() pattern, but write '^' instead of '%'");

    if(!options_parse_args(ops, argc, argv)) {
        fprintf(stderr, "%s : splits a JSON file into many files."
                "\n\nOptions:\n", argv[0]);
        options_print_help(ops, stderr);
        return -1;
    }


    /* Substitute "$" with "%" */

    char output_pattern[256];
    strcpy(output_pattern, output_pattern_op);
    char *f = output_pattern;
    while(*f) {
        if(*f=='^') *f='%';
        f++;
    }

    fputs(output_pattern, stderr);

    FILE * input_stream = open_file_for_reading(input_filename);

    int count = 0;

    JO jo;
    while( (jo = json_read_stream(input_stream)) ) {
        char filename[1000];
        sprintf(filename, output_pattern, count);
        if(!count) {

        }

        sm_debug("Writing to file (%s) %s\n", output_pattern, filename);
        FILE * f = open_file_for_writing(filename);
        if(!f) return -1;
        fputs(json_object_to_json_string(jo), f);
        jo_free(jo);
        fclose(f);

        count++;
    }


    return 0;
}
Example #2
0
int main(int argc, const char* argv[]) {
	sm_set_program_name(argv[0]);

	log2pdf_params p;
	
	lds_set_defaults(&(p.laser));
	ls_set_defaults(&(p.pose_path));
	
	p.laser.rays.draw = 0;
	p.laser.points.draw = 0;
	p.laser.normals.draw = 0;
	p.laser.countour.width = 0.1;
	p.pose_path.width = 0.1;
	p.pose_path.color = "#f00";
	
	options_banner(banner);
	struct option * ops = options_allocate(100);
	options_string(ops, "in", &p.input_filename, "stdin", "input file (Carmen or JSON)");
	options_string(ops, "out", &p.output_filename, "", "output file (if empty, input file + '.pdf')");
	options_double(ops, "padding", &p.padding, 0.2, "padding around bounding box (m)");
	options_double(ops, "dimension", &p.dimension, 500.0, "dimension of the image (points)");
	options_double(ops, "offset_theta_deg", &p.offset_theta_deg, 0.0, " rotate entire map by this angle (deg) ");

	options_string(ops, "use", &p.use, "estimate", "One in 'odometry','estimate','true_pose'");
	options_double(ops, "distance_xy", &p.distance_xy, 5.0, " Minimum distance between scans (m) ");
	options_double(ops, "distance_th_deg", &p.distance_th_deg, 45.0, " Minimum distance between scans (deg) ");
	options_double(ops, "start_pose_width", &p.start_pose_width, 0.4, "First pose | Circle width");
	lds_add_options(&(p.laser), ops, "laser_", "");
	ls_add_options(&(p.pose_path), ops, "path_", "");
	
	if(!options_parse_args(ops, argc, argv)) {
		sm_error("Could not parse arguments.\n");
		options_print_help(ops, stderr);
		return -1;
	}
	
	/* If out not specified */
	if(strlen(p.output_filename)==0) {
		char buf[PATH_MAX];
		sprintf(buf, "%s.pdf", p.input_filename);
		p.output_filename = my_strdup(buf);
/*		sm_info("Writing on file '%s'.\n", p.output_filename);*/
	}
	
	p.use_reference = ld_string_to_reference(p.use);
	if(Invalid == p.use_reference) {
		sm_error("Invalid reference '%s'. " 
			"Use one in 'odometry','estimate','true_pose'.\n", p.use);
		return -1;
	}
/*	sm_info("Using reference: %s.\n", ld_reference_to_string(p.use_reference));*/
	
	return !log2pdf(&p);
}
Example #3
0
int main(int argc, const char * argv[]) {
	sm_set_program_name(argv[0]);
	
	
	options_banner("ld_purify: Makes sure that the file format is valid. \n * Sets valid=0 if reading is outside interval ");
	
	struct ld_purify_params p;
	
	struct option* ops = options_allocate(20);
	options_double(ops, "threshold_min", &p.threshold_min, 0.01, 
		"Sets valid=0 if readings are less than this threshold.");
	options_double(ops, "threshold_max", &p.threshold_max, 79.0, 
		"Sets valid=0 if readings are more than this threshold.");
	
	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out", &p.file_output, "stdout", "Output file ");
		
		
	if(!options_parse_args(ops, argc, argv)) {
		options_print_help(ops, stderr);
		return -1;
	}

	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out = open_file_for_writing(p.file_output);
	if(!out) return -2;



	LDP ld; int count = -1;
	while( (ld = ld_from_json_stream(in))) {
		
		purify(ld, p.threshold_min, p.threshold_max);
		
		if(!ld_valid_fields(ld))  {
			sm_error("Wait, we didn't purify enough  (#%d in file)\n", count);
			continue;
		}
		
		ld_write_as_json(ld, out);
		ld_free(ld);
	}
	
	return 0;
}
void verifier_options(verifier_params *p, struct option*ops) {

 	options_string(ops, "smStats",
 		&(p->smStats), "", 
 		"Input laser file");

	options_string(ops, "odometry", 
		(&p->odometry), "", 
		"Input odometry file ");

	options_string(ops, "calib_params", 
		&(p->calib_params), "", 
		"Input file with estimated parameters");

	options_string(ops, "out", 
		&(p->out), "stdout", 
		"Output file");
	
	options_int(ops, "use_gsl", 
		&(p->use_gsl), 0, 
		"0: no GSL; 1 use GSL");
}
Example #5
0
int main(int argc, const char ** argv) {
	sm_set_program_name(argv[0]);
	
	struct ld_exp_tro1_params p;
	
	options_banner(banner);
	
	struct option* ops = options_allocate(10);
	options_double(ops, "max_xy_error", &p.max_xy_error, 10.0, "Maximum error for x,y (m)");
	options_double(ops, "max_theta_error_deg", &p.max_theta_error_deg, 10.0, "Maximum error for orientation (deg)");
	options_int   (ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");

	options_int(ops, "num_per_scan", &p.num_per_scan, 10, "Number of trials for each scan.");

	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out1", &p.file_output1, "stdout", "Output file for first scan");
	options_string(ops, "out2", &p.file_output2, "stdout", "Output file for second scan");
	
	options_int(ops, "debug", &p.debug, 0, "Shows debug information");
	
	if(!options_parse_args(ops, argc, argv)) {
		options_print_help(ops, stderr);
		return -1;
	}
	
	sm_debug_write(p.debug);

	gsl_rng_env_setup();
	gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
	if(p.seed != 0)
	gsl_rng_set(rng, (unsigned int) p.seed);
	
	/* Open the two output files (possibly the same one) */
	
	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out1 = open_file_for_writing(p.file_output1);
	if(!out1) return -2;
	
	FILE * out2;
	if(!strcmp(p.file_output1, p.file_output2)) {
		out1 = out2;
	} else {
		out2 = open_file_for_writing(p.file_output2);
		if(!out2) return -2;
	}

	/* Read laser data from input file */
	LDP ld; int count=0;
	while( (ld = ld_read_smart(in))) {
		count++;
		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}
		
		for(int n=0; n < p.num_per_scan; n++) {					
			ld->true_pose[0] = 0;
			ld->true_pose[1] = 0;
			ld->true_pose[2] = 0;
			
			ld->odometry[0] = 0;
			ld->odometry[1] = 0;
			ld->odometry[2] = 0;
			
			ld_write_as_json(ld, out1);

			ld->odometry[0] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error;
			ld->odometry[1] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error;
			ld->odometry[2] = 2*(gsl_rng_uniform(rng)-0.5) * deg2rad(p.max_theta_error_deg);
			
			ld_write_as_json(ld, out2);
		}

		ld_free(ld);
	}
	
	return 0;
}
Example #6
0
int main(int argc, const char * argv[]) {
	sm_set_program_name(argv[0]);

	const char *in_filename;
	const char *ref_filename;
	const char *out_filename;
	const char *ref_field_string; ld_reference ref_field;
	const char *out_field_string; ld_reference out_field;

	struct option* ops = options_allocate(15);
	options_string(ops, "in", &in_filename, "stdin", "scan matching log");
	options_string(ops, "ref", &ref_filename, "ref.log", "slam log");
	options_string(ops, "out", &out_filename, "stdout", "output file");

	options_string(ops, "ref_field", &ref_field_string, "estimate", "What field to find in ref.");
	options_string(ops, "out_field", &out_field_string, "true_pose", "What field to copy to.");
		
	if(!options_parse_args(ops, argc, argv)) {
		fprintf(stderr, " This program works on two logs: A and B. "
		"For each scan in A, the program searches for the scan in B having the same timestamp. "
		"Then, the true_pose field in B is copied to the scan form A, and it is written to the output.\n");
		options_print_help(ops, stderr);
		return -1;
	}
	
	ref_field = ld_string_to_reference(ref_field_string);
	out_field = ld_string_to_reference(out_field_string);
	
	
	FILE * in_stream  = open_file_for_reading(in_filename);
	FILE * ref_stream = open_file_for_reading(ref_filename);
	FILE * out_stream = open_file_for_writing(out_filename);
	
	if(!in_stream || !ref_stream || !out_stream) return -1;

	LDP ld_in;
	while((ld_in = ld_read_smart(in_stream))) {
		int matched = 0;
		while(1) {
			LDP ld_ref = ld_read_smart(ref_stream);
			if(!ld_ref) break;
			if(same_scan(ld_in, ld_ref)) {
				matched = 1;
				const double *ref_pose = ld_get_reference_pose(ld_ref, ref_field);
				double *out_pose = ld_get_reference_pose_silent(ld_in, out_field);
				copy_d(ref_pose, 3, out_pose);
				ld_write_as_json(ld_in, out_stream);
				fputs("\n", out_stream);
				break;
			}
			ld_free(ld_ref);
		}

		if(!matched) {
			sm_error("Could not match %s. \n", short_desc(ld_in));
			if(feof(ref_stream)) {
				sm_error("..because ref stream has ended.\n");
				break;
			}
			continue;
		}
	
		ld_free(ld_in);
	}
	
	return 0;
}
Example #7
0
int main(int argc, const char*argv[]) {
	sm_set_program_name(argv[0]);
	
	struct sm_params params;
	struct sm_result result;
	
	struct option* ops = options_allocate(100);
	options_string(ops, "in", &p.file_in, "stdin", "Input file ");
	options_string(ops, "out", &p.file_out, "stdout", "Output file ");
	options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) ");
	options_string(ops, "file_jj", &p.file_jj, "",
		"File for journaling -- if left empty, journal not open.");
	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, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error");
	
	
	p.format = 0;
/*	options_int(ops, "format", &p.format, 0,
		"Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/
	
	sm_options(&params, ops);
	if(!options_parse_args(ops, argc, argv)) {
		fprintf(stderr, "\n\nUsage:\n");
		options_print_help(ops, stderr);
		return -1;
	}

	sm_debug_write(p.debug);

	/* Open input and output files */
	
	FILE * file_in = open_file_for_reading(p.file_in);
	if(!file_in) return -1;
	FILE * file_out = open_file_for_writing(p.file_out);
	if(!file_out) return -1;
	
	if(strcmp(p.file_jj, "")) {
		FILE * jj = open_file_for_writing(p.file_jj);
		if(!jj) return -1;
		jj_set_stream(jj);
	}
	
	FILE * file_out_stats = 0;
	if(strcmp(p.file_out_stats, "")) {
		file_out_stats = open_file_for_writing(p.file_out_stats);
		if(!file_out_stats) return -1;
	}
	
	/* Read first scan */
	LDP laser_ref;
	if(!(laser_ref = ld_read_smart(file_in))) {
		sm_error("Could not read first scan.\n");
		return -1;
	}
	if(!ld_valid_fields(laser_ref))  {
		sm_error("Invalid laser data in first scan.\n");
		return -2;
	}
	
	
	/* For the first scan, set estimate = odometry */
	copy_d(laser_ref->odometry, 3, laser_ref->estimate);
	
	spit(laser_ref, file_out);
	int count=-1;
	LDP laser_sens;
	while( (laser_sens = ld_read_smart(file_in)) ) {
		
		count++;
		if(!ld_valid_fields(laser_sens))  {
			sm_error("Invalid laser data in (#%d in file).\n", count);
			return -(count+2);
		}
		
		params.laser_ref  = laser_ref;
		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];
		ominus_d(params.laser, ominus_laser);
		oplus_d(ominus_laser, odometry, temp);
		oplus_d(temp, params.laser, params.first_guess);
		
		/* Do the actual work */
		switch(p.algo) {
			case(0):
				sm_icp(&params, &result); break;
			case(1):
				sm_gpm(&params, &result); break;
			case(2):
				sm_hsm(&params, &result); break;
			default:
				sm_error("Unknown algorithm to run: %d.\n",p.algo);
				return -1;
		}
		
		if(!result.valid){
			if(p.recover_from_error) {
				sm_info("One ICP matching failed. Because you passed  -recover_from_error, I will try to recover."
				" Note, however, that this might not be good in some cases. \n");
				sm_info("The recover is that the displacement is set to 0. No result stats is output. \n");
				
				/* For the first scan, set estimate = odometry */
				copy_d(laser_ref->estimate, 3, laser_sens->estimate);
				
				ld_free(laser_ref); laser_ref = laser_sens;
				
			} else {
				sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n");
				sm_error("Use the option -recover_from_error if you want to try to recover.\n");
				ld_free(laser_ref);
				return 2;
			}
		} else {
		
			/* Add the result to the previous estimate */
			oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);

			/* Write the corrected log */
			spit(laser_sens, file_out);

			/* Write the statistics (if required) */
			if(file_out_stats) {
				JO jo = result_to_json(&params, &result);
				fputs(jo_to_string(jo), file_out_stats);
				fputs("\n", file_out_stats);
				jo_free(jo);
			}

			ld_free(laser_ref); laser_ref = laser_sens;
		}
	}
	ld_free(laser_ref);
	
	return 0;
}
Example #8
0
int main(int argc, const char * argv[]) {
	sm_set_program_name(argv[0]);
	
	options_banner("ld_noise: Adds noise to readings in a scan");
	
	struct ld_noise_params p;
	
	struct option* ops = options_allocate(20);
	options_double(ops, "discretization", &p.discretization, 0.0, 
		"Size of discretization (disabled if 0)");
	options_double(ops, "sigma", &p.sigma, 0.0, 
		"Std deviation of gaussian noise (disabled if 0)");
	options_int(ops, "lambertian", &p.lambertian, 0, 
		"Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha.");
	options_int(ops, "seed", &p.seed, 0, 
		"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out", &p.file_output, "stdout", "Output file ");
		
		
	if(!options_parse_args(ops, argc, argv)) {
		fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n");
		options_print_help(ops, stderr);
		return -1;
	}

	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out = open_file_for_writing(p.file_output);
	if(!out) return -2;


	gsl_rng_env_setup();
	gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
	if(p.seed != 0)
	gsl_rng_set(rng, (unsigned int) p.seed);

	LDP ld; int count = 0;
	while( (ld = ld_from_json_stream(in))) {
		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}
		
		int i;
		for(i=0;i<ld->nrays;i++) {
			if(!ld->valid[i]) continue;
			
			double * reading = ld->readings + i;
			if(p.sigma > 0) {
				double add_sigma = p.sigma;
				
				if(p.lambertian) {

					int have_alpha = 0;
					double alpha = 0;
					if(!is_nan(ld->true_alpha[i])) {
						alpha = ld->true_alpha[i];
						have_alpha = 1;
					} else if(ld->alpha_valid[i]) {
						alpha = ld->alpha[i];;
						have_alpha = 1;
					} else have_alpha = 0;

					if(have_alpha) {
						/* Recall that alpha points outside the surface */
						double beta = (alpha+M_PI) - ld->theta[i];
					    add_sigma = p.sigma / cos(beta);
					} else {
						sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]");
						ld_write_as_json(ld, stderr);
						return -1;
					}
					
				} 
				
			   *reading += gsl_ran_gaussian(rng, add_sigma);
				
				if(is_nan(ld->readings_sigma[i])) {
					ld->readings_sigma[i] = add_sigma;
				} else {
					ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i]));
				}
			}
			if(p.discretization > 0)
				*reading -= fmod(*reading , p.discretization);
		}
	
		ld_write_as_json(ld, out);
		ld_free(ld);
	}
	
	return 0;
}