sm1.c
Go to the documentation of this file.
00001 #include <time.h>
00002 #include <string.h>
00003 
00004 #include "../csm/csm_all.h"
00005 
00006 #include <options/options.h>
00007 
00008 struct sm1_params {
00009         const char * file1;
00010         const char * file2;
00011         const char * file_jj;
00012         const char * file_output;
00013         int debug;
00014         
00015         int algo;
00016         int write_post_mortem;
00017 } p;
00018 
00019 
00020 extern int distance_counter;
00021 extern void sm_options(struct sm_params*p, struct option*ops);
00022 
00023 
00024 const char *sm1_banner = 
00025 
00026 "There are TWO ways to define the input to this program.\n"
00027 "                                                       \n"
00028 "Say that in file A there are the scans   \n"
00029 "                                                       \n"
00030 "   A1 A2 A3 A4 ...  \n"
00031 "                                                       \n"
00032 "and in file B there are the scans  \n"
00033 "                                                       \n"
00034 "   B1 B2 B3 B4 ...  \n"
00035 "                                                       \n"
00036 "With this command line:  \n"
00037 "                                                       \n"
00038 "    sm1 --file1 A --file2 B  \n"
00039 "                                                       \n"
00040 "the matchings will be:  \n"
00041 "                                                       \n"
00042 "    (A1, B1), (A2, B2), etc.\n"
00043 "                                                       \n"
00044 "While with this command line:\n"
00045 "                                                       \n"
00046 "    sm1 --file1 A --file2 A\n"
00047 "                                                       \n"
00048 "the matchings will be \n"
00049 "                                                       \n"
00050 "    (A1, A2), (A3, A4), (A5, A6), ... \n"
00051 "                                                       \n";
00052         
00053         
00054 int main(int argc, const char*argv[]) {
00055         sm_set_program_name(argv[0]);
00056         options_banner(sm1_banner);
00057         
00058         struct sm_params params;
00059         struct sm_result result;
00060         
00061         struct option* ops = options_allocate(100);
00062         options_string(ops, "file1", &p.file1, "file1.txt",
00063                 "File with first series of scans (at pose1)");
00064         options_string(ops, "file2", &p.file2, "file2.txt",
00065                 "File with second series of scans (at pose2)");
00066         options_string(ops, "file_jj", &p.file_jj, "",
00067                 "File for journaling -- if left empty, journal not open.");
00068         options_string(ops, "out", &p.file_output, "stdout", "Output file (JSON structs)");
00069         options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) ");
00070 
00071         options_int(ops, "debug", &p.debug, 0, "Shows debug information");
00072         options_int(ops, "write_post_mortem", &p.write_post_mortem, 1, "In case of failure, writes a post mortem.");
00073 
00074         sm_options(&params, ops);
00075         if(!options_parse_args(ops, argc, argv)) {
00076                 fprintf(stderr, "\n\nUsage:\n");
00077                 options_print_help(ops, stderr);
00078                 return -1;
00079         }
00080 
00081         sm_debug_write(p.debug);
00082 
00083         FILE * file1 = open_file_for_reading(p.file1);
00084         FILE * file2 = !strcmp(p.file1, p.file2) ? file1 
00085                 : open_file_for_reading(p.file2);
00086         if(!file1 || !file2) return -1;
00087         
00088         FILE * out = open_file_for_writing(p.file_output);
00089         if(!out) return -2;
00090         
00091         if(strcmp(p.file_jj, "")) {
00092                 FILE * jj = open_file_for_writing(p.file_jj);
00093                 if(!jj) return -1;
00094                 jj_set_stream(jj);
00095         }
00096         
00097         LDP ld1, ld2;
00098         int count = 0;
00099         while(1) {
00100                 count++;
00101                 
00102                 ld1 = ld_from_json_stream(file1);
00103                 if(!ld1) {
00104                         if(feof(file1)) break;
00105                         sm_error("Invalid data in file1 '%s' \n", p.file1);
00106                         return 2;
00107                 }
00108                 ld2 = ld_from_json_stream(file2);
00109                 if(!ld2) {
00110                         if(feof(file2)) break;
00111                         sm_error("Invalid data in file2 '%s' \n", p.file2);
00112                         return 3;
00113                 }
00114                 
00115                 params.laser_ref = ld1;
00116                 params.laser_sens = ld2;
00117 
00118                 if(     any_nan(params.laser_ref->odometry,3) ||  
00119                         any_nan(params.laser_sens->odometry,3) ) {
00120                                 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");
00121                                 sm_error(" (file %s) laser_ref->odometry = %s \n", p.file1, friendly_pose(params.laser_ref->odometry) );
00122                                 sm_error(" (file %s) laser_sens->odometry = %s \n", p.file2, friendly_pose(params.laser_sens->odometry) );
00123                                 sm_error(" I will quit it here. \n");
00124                                 return 3;
00125                 }
00126                 
00127                 pose_diff_d( params.laser_sens->odometry,  
00128                 /* o minus */ params.laser_ref->odometry,
00129                         /* = */ params.first_guess);
00130 
00131                 /* Do the actual work */
00132                 switch(p.algo) {
00133                         case(0):
00134                                 sm_icp(&params, &result); break;
00135                         case(1):
00136                                 sm_gpm(&params, &result); break;
00137                         case(2):
00138                                 sm_hsm(&params, &result); break;
00139                         default:
00140                                 sm_error("Unknown algorithm to run: %d.\n",p.algo);
00141                                 return -1;
00142                 }
00143                 
00144                 if(p.write_post_mortem && !result.valid) {
00145                         char casename[256];
00146                         sprintf(casename, "sm1_failure_matching%d", count);
00147                         sm_error("sm1: matching #%d failed. Writing a special case %s.\n", count, casename );
00148 //                      save_testcase(&params)
00149                         
00150                         char file_config[256],file1[256],file2[256],file_jj[256],script[256];
00151                         
00152                         sprintf(file_config, "%s.config", casename);
00153                         sprintf(file1, "%s_laser_ref.json", casename);
00154                         sprintf(file2, "%s_laser_sens.json", casename);
00155                         sprintf(file_jj, "%s.journal", casename);
00156                         sprintf(script, "%s.sh", casename);
00157                         
00158                         FILE * f = fopen(file_config, "w");
00159                         options_dump(ops,f,0);
00160                         fclose(f);
00161 
00162                         f = fopen(file1, "w");
00163                         ld_write_as_json(params.laser_ref,f);
00164                         fclose(f);
00165                         
00166                         f = fopen(file2, "w");
00167                         ld_write_as_json(params.laser_sens,f);
00168                         fclose(f);
00169                         
00170                         f = fopen(script, "w");
00171                         fprintf(f, "#!/bin/bash\n");
00172                         fprintf(f, "%s -config %s -file1 %s -file2 %s -debug 1 -file_jj %s -write_post_mortem 0 \n", 
00173                                 argv[0], file_config, file1, file2, file_jj);
00174                         fprintf(f, "sm_animate -in %s -out %s_anim.pdf \n", file_jj, casename);
00175                         fclose(f);
00176                 }
00177                 
00178                 
00179                 JO jo = result_to_json(&params, &result);
00180 
00181                         double true_x[3];
00182                         pose_diff_d(params.laser_sens->true_pose, 
00183                                 params.laser_ref->true_pose, true_x);
00184                         double true_e[3];
00185                         
00186                         pose_diff_d(result.x, true_x, true_e);
00187                 /*      int i=0;for(;i<3;i++) true_e[i] = result.x[i] - true_x[i];*/
00188                         
00189                         jo_add_double_array(jo, "true_x", true_x, 3);
00190                         jo_add_double_array(jo, "true_e", true_e, 3);
00191                         
00192                 fputs(json_object_to_json_string(jo), out);
00193                 fputs("\n",out);
00194                 
00195                 jo_free(jo);
00196                 ld_free(ld1);
00197                 ld_free(ld2);
00198         }
00199         
00200         fclose(file1);
00201         if(file2 != file1) fclose(file2);
00202         fclose(out);
00203         
00204         
00205         return 0;
00206 }


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