map1.cpp
Go to the documentation of this file.
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         /* which algorithm to run */
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 /*      options_int(ops, "format", &p.format, 0,
00034                 "Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/
00035         
00036         sm_options(&params, 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         /* Open input and output files */
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         /* Read first scan */
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         /* For the first scan, set estimate = odometry */
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                 /* Set first guess as the difference in odometry */
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                 /* Do the actual work */
00099                 switch(p.algo) {
00100                         case(0):
00101                                 sm_icp(&params, &result); break;
00102                         case(1):
00103                                 sm_gpm(&params, &result); break;
00104                         default:
00105                                 sm_error("Unknown algorithm to run: %d.\n",p.algo);
00106                                 return -1;
00107                 }
00108                 
00109                 /* Add the result to the previous estimate */
00110                 oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);
00111 
00112                 /* Write the corrected log */
00113                 ld_write_as_json(laser_sens, file_out);
00114 
00115                 /* Write the statistics (if required) */
00116                 if(file_out_stats) {
00117                         JO jo = result_to_json(&params, &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 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29