ld_recover.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <options/options.h>
00003 
00004 #include "../csm/csm_all.h"
00005 #include "../csm/laser_data_drawing.h"
00006 
00008 int same_scan(LDP ld1, LDP ld2);
00010 const char * short_desc(LDP ld);
00011 
00012 int main(int argc, const char * argv[]) {
00013         sm_set_program_name(argv[0]);
00014 
00015         const char *in_filename;
00016         const char *ref_filename;
00017         const char *out_filename;
00018         const char *ref_field_string; ld_reference ref_field;
00019         const char *out_field_string; ld_reference out_field;
00020 
00021         struct option* ops = options_allocate(15);
00022         options_string(ops, "in", &in_filename, "stdin", "scan matching log");
00023         options_string(ops, "ref", &ref_filename, "ref.log", "slam log");
00024         options_string(ops, "out", &out_filename, "stdout", "output file");
00025 
00026         options_string(ops, "ref_field", &ref_field_string, "estimate", "What field to find in ref.");
00027         options_string(ops, "out_field", &out_field_string, "true_pose", "What field to copy to.");
00028                 
00029         if(!options_parse_args(ops, argc, argv)) {
00030                 fprintf(stderr, " This program works on two logs: A and B. "
00031                 "For each scan in A, the program searches for the scan in B having the same timestamp. "
00032                 "Then, the true_pose field in B is copied to the scan form A, and it is written to the output.\n");
00033                 options_print_help(ops, stderr);
00034                 return -1;
00035         }
00036         
00037         ref_field = ld_string_to_reference(ref_field_string);
00038         out_field = ld_string_to_reference(out_field_string);
00039         
00040         
00041         FILE * in_stream  = open_file_for_reading(in_filename);
00042         FILE * ref_stream = open_file_for_reading(ref_filename);
00043         FILE * out_stream = open_file_for_writing(out_filename);
00044         
00045         if(!in_stream || !ref_stream || !out_stream) return -1;
00046 
00047         LDP ld_in;
00048         while((ld_in = ld_read_smart(in_stream))) {
00049                 int matched = 0;
00050                 while(1) {
00051                         LDP ld_ref = ld_read_smart(ref_stream);
00052                         if(!ld_ref) break;
00053                         if(same_scan(ld_in, ld_ref)) {
00054                                 matched = 1;
00055                                 const double *ref_pose = ld_get_reference_pose(ld_ref, ref_field);
00056                                 double *out_pose = ld_get_reference_pose_silent(ld_in, out_field);
00057                                 copy_d(ref_pose, 3, out_pose);
00058                                 ld_write_as_json(ld_in, out_stream);
00059                                 fputs("\n", out_stream);
00060                                 break;
00061                         }
00062                         ld_free(ld_ref);
00063                 }
00064 
00065                 if(!matched) {
00066                         sm_error("Could not match %s. \n", short_desc(ld_in));
00067                         if(feof(ref_stream)) {
00068                                 sm_error("..because ref stream has ended.\n");
00069                                 break;
00070                         }
00071                         continue;
00072                 }
00073         
00074                 ld_free(ld_in);
00075         }
00076         
00077         return 0;
00078 }
00079 
00080 
00082 int same_scan(LDP ld1, LDP ld2) {
00083         return (ld1->tv.tv_sec == ld2->tv.tv_sec) && 
00084                 (ld1->tv.tv_usec == ld2->tv.tv_usec);
00085 }
00086 
00087 char buf[100];
00088 const char * short_desc(LDP ld) {
00089         sprintf(buf, "LD, tv=%d,%d", (int) ld->tv.tv_sec, (int) ld->tv.tv_usec);
00090         return buf;
00091 }
00092 


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