00001 #include <string.h>
00002 #include "pan.h"
00003 #include <options/options.h>
00004
00005 struct myparams {
00006 const char * file_in;
00007 const char * file_out;
00008 const char * file_out_stats;
00009 const char * file_jj;
00010 int format;
00011
00012
00013 int algo;
00014 } p;
00015
00016 extern "C" void sm_options(struct sm_params*p, struct option*ops);
00017
00018
00019 int main(int argc, const char*argv[]) {
00020 sm_set_program_name(argv[0]);
00021
00022 struct sm_params params;
00023 struct sm_result result;
00024
00025 struct option* ops = options_allocate(100);
00026 options_string(ops, "in", &p.file_in, "stdin", "Input file ");
00027 options_string(ops, "out", &p.file_out, "stdout", "Output file ");
00028 options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) ");
00029 options_string(ops, "file_jj", &p.file_jj, "",
00030 "File for journaling -- if left empty, journal not open.");
00031 options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:icp 1:gpm-stripped) ");
00032 p.format = 0;
00033
00034
00035
00036 sm_options(¶ms, ops);
00037 if(!options_parse_args(ops, argc, argv)) {
00038 fprintf(stderr, "\n\nUsage:\n");
00039 options_print_help(ops, stderr);
00040 return -1;
00041 }
00042
00043
00044
00045 FILE * file_in = open_file_for_reading(p.file_in);
00046 if(!file_in) return -1;
00047 FILE * file_out = open_file_for_writing(p.file_out);
00048 if(!file_out) return -1;
00049
00050 if(strcmp(p.file_jj, "")) {
00051 FILE * jj = open_file_for_writing(p.file_jj);
00052 if(!jj) return -1;
00053 jj_set_stream(jj);
00054 }
00055
00056 FILE * file_out_stats = 0;
00057 if(strcmp(p.file_out_stats, "")) {
00058 file_out_stats = open_file_for_writing(p.file_out_stats);
00059 if(!file_out_stats) return -1;
00060 }
00061
00062
00063 LDP laser_ref;
00064 if(!(laser_ref = ld_read_smart(file_in))) {
00065 sm_error("Could not read first scan.\n");
00066 return -1;
00067 }
00068 if(!ld_valid_fields(laser_ref)) {
00069 sm_error("Invalid laser data in first scan.\n");
00070 return -2;
00071 }
00072
00073
00074
00075 copy_d(laser_ref->odometry, 3, laser_ref->estimate);
00076
00077 ld_write_as_json(laser_ref, file_out);
00078 int count=-1;
00079 LDP laser_sens;
00080 while( (laser_sens = ld_read_smart(file_in)) ) {
00081 count++;
00082 if(!ld_valid_fields(laser_sens)) {
00083 sm_error("Invalid laser data in (#%d in file).\n", count);
00084 return -(count+2);
00085 }
00086
00087 params.laser_ref = laser_ref;
00088 params.laser_sens = laser_sens;
00089
00090
00091 double odometry[3];
00092 pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry);
00093 double ominus_laser[3], temp[3];
00094 ominus_d(params.laser, ominus_laser);
00095 oplus_d(ominus_laser, odometry, temp);
00096 oplus_d(temp, params.laser, params.first_guess);
00097
00098
00099 switch(p.algo) {
00100 case(0):
00101 sm_icp(¶ms, &result); break;
00102 case(1):
00103 sm_gpm(¶ms, &result); break;
00104 default:
00105 sm_error("Unknown algorithm to run: %d.\n",p.algo);
00106 return -1;
00107 }
00108
00109
00110 oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);
00111
00112
00113 ld_write_as_json(laser_sens, file_out);
00114
00115
00116 if(file_out_stats) {
00117 JO jo = result_to_json(¶ms, &result);
00118 fputs(jo_to_string(jo), file_out_stats);
00119 fputs("\n", file_out_stats);
00120 jo_free(jo);
00121 }
00122
00123 ld_free(laser_ref); laser_ref = laser_sens;
00124 }
00125 ld_free(laser_ref);
00126
00127 return 0;
00128 }