sm2.c
Go to the documentation of this file.
00001 #include <time.h>
00002 #include <string.h>
00003 #include <libgen.h>
00004 
00005 #include <options/options.h>
00006 #include "../csm/csm_all.h"
00007 
00008 struct {
00009         const char * file_in;
00010         const char * file_out;
00011         const char * file_out_stats;
00012         const char * file_jj;
00013         int format;
00014         
00015         /* which algorithm to run */
00016         int algo;
00017         
00018         int recover_from_error;
00019         
00020         int debug;
00021 } p;
00022 
00023 extern void sm_options(struct sm_params*p, struct option*ops);
00024 
00025 void spit(LDP ld, FILE * stream);
00026 
00027 int main(int argc, const char*argv[]) {
00028         sm_set_program_name(argv[0]);
00029         
00030         struct sm_params params;
00031         struct sm_result result;
00032         
00033         struct option* ops = options_allocate(100);
00034         options_string(ops, "in", &p.file_in, "stdin", "Input file ");
00035         options_string(ops, "out", &p.file_out, "stdout", "Output file ");
00036         options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) ");
00037         options_string(ops, "file_jj", &p.file_jj, "",
00038                 "File for journaling -- if left empty, journal not open.");
00039         options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) ");
00040         
00041         options_int(ops, "debug", &p.debug, 0, "Shows debug information");
00042         options_int(ops, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error");
00043         
00044         
00045         p.format = 0;
00046 /*      options_int(ops, "format", &p.format, 0,
00047                 "Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/
00048         
00049         sm_options(&params, ops);
00050         if(!options_parse_args(ops, argc, argv)) {
00051                 fprintf(stderr, "\n\nUsage:\n");
00052                 options_print_help(ops, stderr);
00053                 return -1;
00054         }
00055 
00056         sm_debug_write(p.debug);
00057 
00058         /* Open input and output files */
00059         
00060         FILE * file_in = open_file_for_reading(p.file_in);
00061         if(!file_in) return -1;
00062         FILE * file_out = open_file_for_writing(p.file_out);
00063         if(!file_out) return -1;
00064         
00065         if(strcmp(p.file_jj, "")) {
00066                 FILE * jj = open_file_for_writing(p.file_jj);
00067                 if(!jj) return -1;
00068                 jj_set_stream(jj);
00069         }
00070         
00071         FILE * file_out_stats = 0;
00072         if(strcmp(p.file_out_stats, "")) {
00073                 file_out_stats = open_file_for_writing(p.file_out_stats);
00074                 if(!file_out_stats) return -1;
00075         }
00076         
00077         /* Read first scan */
00078         LDP laser_ref;
00079         if(!(laser_ref = ld_read_smart(file_in))) {
00080                 sm_error("Could not read first scan.\n");
00081                 return -1;
00082         }
00083         if(!ld_valid_fields(laser_ref))  {
00084                 sm_error("Invalid laser data in first scan.\n");
00085                 return -2;
00086         }
00087         
00088         
00089         /* For the first scan, set estimate = odometry */
00090         copy_d(laser_ref->odometry, 3, laser_ref->estimate);
00091         
00092         spit(laser_ref, file_out);
00093         int count=-1;
00094         LDP laser_sens;
00095         while( (laser_sens = ld_read_smart(file_in)) ) {
00096                 
00097                 count++;
00098                 if(!ld_valid_fields(laser_sens))  {
00099                         sm_error("Invalid laser data in (#%d in file).\n", count);
00100                         return -(count+2);
00101                 }
00102                 
00103                 params.laser_ref  = laser_ref;
00104                 params.laser_sens = laser_sens;
00105 
00106                 /* Set first guess as the difference in odometry */
00107                 
00108                 if(     any_nan(params.laser_ref->odometry,3) ||  
00109                         any_nan(params.laser_sens->odometry,3) ) {
00110                                 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");
00111                                 sm_error("  laser_ref->odometry = %s \n",  friendly_pose(params.laser_ref->odometry) );
00112                                 sm_error("  laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) );
00113                                 sm_error(" I will quit it here. \n");
00114                                 return -3;
00115                 }
00116                 
00117                 double odometry[3];
00118                 pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry);
00119                 double ominus_laser[3], temp[3];
00120                 ominus_d(params.laser, ominus_laser);
00121                 oplus_d(ominus_laser, odometry, temp);
00122                 oplus_d(temp, params.laser, params.first_guess);
00123                 
00124                 /* Do the actual work */
00125                 switch(p.algo) {
00126                         case(0):
00127                                 sm_icp(&params, &result); break;
00128                         case(1):
00129                                 sm_gpm(&params, &result); break;
00130                         case(2):
00131                                 sm_hsm(&params, &result); break;
00132                         default:
00133                                 sm_error("Unknown algorithm to run: %d.\n",p.algo);
00134                                 return -1;
00135                 }
00136                 
00137                 if(!result.valid){
00138                         if(p.recover_from_error) {
00139                                 sm_info("One ICP matching failed. Because you passed  -recover_from_error, I will try to recover."
00140                                 " Note, however, that this might not be good in some cases. \n");
00141                                 sm_info("The recover is that the displacement is set to 0. No result stats is output. \n");
00142                                 
00143                                 /* For the first scan, set estimate = odometry */
00144                                 copy_d(laser_ref->estimate, 3, laser_sens->estimate);
00145                                 
00146                                 ld_free(laser_ref); laser_ref = laser_sens;
00147                                 
00148                         } else {
00149                                 sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n");
00150                                 sm_error("Use the option -recover_from_error if you want to try to recover.\n");
00151                                 ld_free(laser_ref);
00152                                 return 2;
00153                         }
00154                 } else {
00155                 
00156                         /* Add the result to the previous estimate */
00157                         oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);
00158 
00159                         /* Write the corrected log */
00160                         spit(laser_sens, file_out);
00161 
00162                         /* Write the statistics (if required) */
00163                         if(file_out_stats) {
00164                                 JO jo = result_to_json(&params, &result);
00165                                 fputs(jo_to_string(jo), file_out_stats);
00166                                 fputs("\n", file_out_stats);
00167                                 jo_free(jo);
00168                         }
00169 
00170                         ld_free(laser_ref); laser_ref = laser_sens;
00171                 }
00172         }
00173         ld_free(laser_ref);
00174         
00175         return 0;
00176 }
00177 
00178 
00179 void spit(LDP ld, FILE * stream) {
00180         switch(p.format) {
00181                 case(0): {
00182                         ld_write_as_json(ld, stream);
00183                         break;
00184                 }
00185                 case(1): {
00186                         /* XXX: to implement */
00187                         break;
00188                 }
00189         }
00190 }


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