carmen2pdf.c
Go to the documentation of this file.
00001 #include <time.h>
00002 #include <string.h>
00003 
00004 #ifdef LINUX
00005 #include <linux/limits.h>
00006 #endif
00007 
00008 
00009 #include <cairo.h>
00010 #include <cairo-pdf.h>
00011 
00012 #include <options/options.h>
00013 
00014 #include "../csm/csm_all.h"
00015 
00016 
00017 typedef enum { Invalid = 0, Odometry = 1, Estimate = 2, True_pose = 3 } reference;
00018 const char*reference_name[4] = { "invalid","odometry","estimate","true_pose"};
00019 
00020 struct params {
00021         int interval;
00022         const char*use;
00023         double padding;
00024         double horizon;
00025         double line_threshold;
00026         double dimension;
00027         
00028         int draw_confidence;
00029         double confidence_mult;
00030         
00031         const char*output_filename;
00032         const char*input_filename;
00033 
00034         
00035         FILE*input_file;
00036         reference use_reference;
00037 
00038         double offset_theta_deg;
00039 };
00040 
00041 void carmen2pdf(struct params p);
00042 
00043 double offset_theta = 0;
00044 void ld_getbb(LDP  ld, double*x0, double*y0, double*x1, double*y1, 
00045         reference use_reference, double horizon);
00046 
00047 int main(int argc, const char*argv[]) {
00048         sm_set_program_name(argv[0]);
00049         fprintf(stderr, "carmen2pdf:\t *** Please use log2pdf instead. ***\n\n");
00050 
00051         struct params p;
00052         
00053         struct option * ops = options_allocate(12);
00054         options_int(ops, "interval", &p.interval, 10, "how many to ignore");
00055         options_string(ops, "in", &p.input_filename, "stdin", "input file (Carmen or JSON)");
00056         options_string(ops, "out", &p.output_filename, "", "output file (if empty, input file + '.pdf')");
00057         options_double(ops, "lt", &p.line_threshold, 0.2, "threshold for linking points (m)");
00058         options_double(ops, "horizon", &p.horizon, 8.0, "horizon of the laser (m)");
00059         options_double(ops, "padding", &p.padding, 0.2, "padding around bounding box (m)");
00060         options_double(ops, "dimension", &p.dimension, 500.0, "dimension of the image (points)");
00061         options_int(ops, "draw_confidence", &p.draw_confidence, 0, " Draws confidence (readings_sigma[i]) ");
00062         options_double(ops, "confidence_mult", &p.confidence_mult, 3.0, " 3-sigma ");
00063         options_double(ops, "offset_theta_deg", &p.offset_theta_deg, 0.0, " rotate entire map by this angle (deg) ");
00064 
00065         options_string(ops, "use", &p.use, "estimate", "One in 'odometry','estimate','true_pose'");
00066         
00067         if(!options_parse_args(ops, argc, argv)) {
00068                 sm_error("Could not parse arguments.\n");
00069                 options_print_help(ops, stderr);
00070                 return -1;
00071         }
00072         
00073         /* If out not specified */
00074         if(strlen(p.output_filename)==0) {
00075                 char buf[PATH_MAX];
00076                 sprintf(buf, "%s.pdf", p.input_filename);
00077                 p.output_filename = strdup(buf);
00078                 sm_info("Writing on file '%s'.\n", p.output_filename);
00079         }
00080         
00081         p.use_reference = Invalid;
00082         int i; for(i=1;i<=3;i++) 
00083                 if(!strcmp(p.use, reference_name[i]))
00084                         p.use_reference = (reference) i;
00085         if(Invalid == p.use_reference) {
00086                 sm_error("Invalid reference '%s'. " 
00087                         "Use one in 'odometry','estimate','true_pose'.\n", p.use);
00088                 return -1;
00089         }
00090         
00091         
00092         
00093         
00094         p.input_file = open_file_for_reading(p.input_filename);
00095         if(!p.input_file) return -1;
00096         
00097         carmen2pdf(p);
00098         return 0;
00099 }
00100 
00101 int should_consider(struct params *p, int counter) {
00102         return counter%p->interval == 0;
00103 }
00104 
00105 void ld_get_world(LDP ld, int i, double*x, double*y, reference use_reference);
00106 
00107 struct bounding_box {
00109         double x0,y0,x1,y1;
00110         /* Paper size */
00111         double width, height;
00112 };
00113 
00114 void bb_w2b(struct bounding_box*bb, double wx, double wy, double*bx, double*by){
00115         double scale = GSL_MIN(bb->width / (bb->x1-bb->x0), bb->height / (bb->y1-bb->y0));
00116         *bx = (wx-bb->x0) * scale;
00117         *by = bb->height- (wy-bb->y0) * scale;
00118 }
00119 
00120 void ld_get_buffer_polar(double phi, double rho, const double*pose, 
00121         double*x, double*y,
00122         struct bounding_box*bb, double*bx,double *by);
00123 
00124 
00126 void get_bb(struct params*p, struct bounding_box*bb) {
00127         LDP ld;
00128         int counter = -1, 
00129             considered = 0;
00130 
00131         while((ld = ld_read_smart(p->input_file))) {
00132                 counter++;
00133                 if(should_consider(p, counter))  {
00134                         if(!ld_valid_fields(ld))  {
00135                                 sm_error("Invalid laser data (#%d in file)\n", counter);
00136                                 continue;
00137                         }
00138                         
00139                         double x0,y0,x1,y1;
00140                         ld_getbb(ld,&x0,&y0,&x1,&y1, p->use_reference, p->horizon);
00141                         if(considered > 0) {
00142                                 bb->x0 = GSL_MIN(x0, bb->x0);
00143                                 bb->x1 = GSL_MAX(x1, bb->x1);
00144                                 bb->y0 = GSL_MIN(y0, bb->y0);
00145                                 bb->y1 = GSL_MAX(y1, bb->y1);
00146                         } else {
00147                                 /* this is the first one */
00148                                 bb->x0 = x0;
00149                                 bb->x1 = x1;
00150                                 bb->y0 = y0;
00151                                 bb->y1 = y1;
00152                         }
00153                         
00154                         considered++;
00155                 }
00156                 ld_free(ld);
00157         }
00158         sm_info("Considering %d of %d scans.\n", considered, counter+1);
00159         rewind(p->input_file);
00160         
00161         bb->x0 -= p->padding;
00162         bb->x1 += p->padding;
00163         bb->y0 -= p->padding;
00164         bb->y1 += p->padding;
00165 }
00166 
00167 
00168 double * ld_get_reference(LDP ld, reference use_reference) {
00169         double * pose;
00170         switch(use_reference) {
00171                 case Odometry: pose = ld->odometry; break;
00172                 case Estimate: pose = ld->estimate; break;
00173                 case True_pose: pose = ld->true_pose; break;
00174                 default: exit(-1);
00175         }
00176         if(any_nan(pose, 3)) {
00177                 sm_error("Required field '%s' not set in laser scan.\n", 
00178                         reference_name[use_reference] );
00179                 sm_error("I will abruptly exit() because of a panic attack.\n");
00180                 exit(-1);
00181         }
00182         return pose;
00183 }
00184 
00185 
00186 void carmen2pdf(struct params p) {
00187         
00188         offset_theta += deg2rad(p.offset_theta_deg);
00189 
00190         struct bounding_box bb;
00191         get_bb(&p, &bb);
00192 
00193         double wwidth = bb.x1-bb.x0;
00194         double wheight= bb.y1-bb.y0;
00195         if(wwidth > wheight) {
00196                 bb.width = p.dimension;
00197                 bb.height = bb.width / wwidth * wheight;
00198         } else {
00199                 bb.height = p.dimension;
00200                 bb.width = bb.height / wheight * wwidth;
00201         }
00202         
00203         sm_info("Bounding box: %f %f, %f %f\n",bb.x0,bb.y0,bb.x1,bb.y1);
00204         
00205         cairo_surface_t *surface;
00206         cairo_t *cr;
00207         cairo_status_t status;
00208         
00209         surface = cairo_pdf_surface_create(p.output_filename, bb.width, bb.height);
00210         cr = cairo_create (surface);
00211         status = cairo_status (cr);
00212 
00213         if (status) {
00214                 sm_error("Failed to create pdf surface for file %s: %s\n",
00215                         p.output_filename, cairo_status_to_string (status));
00216                 return;
00217         }
00218         
00219         int counter=0; 
00220         int first_pose=1; double old_pose_bx=0,old_pose_by=0;
00221         LDP ld;
00222         while((ld = ld_read_smart(p.input_file))) {
00223         
00224                 double *pose = ld_get_reference(ld, p.use_reference);
00225 
00226                 /* Draw pose */
00227                 {
00228                         double bx,by;
00229                         ld_get_buffer_polar(0.0,0.0,pose, 0,0, &bb, &bx, &by);
00230                         if(first_pose) { 
00231                                 first_pose = 0; 
00232                         } else {
00233                                 cairo_set_line_width(cr, 0.5);
00234                                 cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
00235                                 cairo_move_to(cr, old_pose_bx, old_pose_by);
00236                                 cairo_line_to(cr, bx, by);
00237                                 cairo_close_path(cr);
00238                                 cairo_stroke(cr);
00239                         }
00240 
00241                         old_pose_bx = bx;
00242                         old_pose_by = by;
00243                 }
00244                 
00245 
00246                 /* If should we draw this sensor scan */
00247                 if(should_consider(&p, counter))  {
00248 
00249                         /* Firstly, find buffer coordinates and whether to cut the stroke */
00250                         struct {
00251                                 double w[2]; /* world coordinates */
00252                                 double b[2]; /* buffer coordinates */
00253                                 int begin_new_stroke;
00254                                 int end_stroke;
00255                                 int valid;
00256                                 } draw_info[ld->nrays];
00257                         
00258                         {
00259                                 int last_valid = -1; int first = 1;
00260                                 int i; for(i=0;i<ld->nrays;i++) {
00261 
00262                                         if( (!ld->valid[i]) || ld->readings[i]>p.horizon) {
00263                                                 draw_info[i].valid = 0;
00264                                                 continue;
00265                                         }
00266                                         draw_info[i].valid = 1;
00267 
00268                                         ld_get_buffer_polar(ld->theta[i], ld->readings[i], 
00269                                                 pose, &(draw_info[i].w[0]), &(draw_info[i].w[1]), 
00270                                                 &bb,  &(draw_info[i].b[0]), &(draw_info[i].b[1]));
00271 
00272                                         if(first) { 
00273                                                 first = 0; 
00274                                                 draw_info[i].begin_new_stroke = 1;
00275                                                 draw_info[i].end_stroke = 0;
00276                                         } else {
00277                                                 int near = square(p.line_threshold) > 
00278                                                         distance_squared_d(draw_info[last_valid].w, draw_info[i].w);
00279                                                 draw_info[i].begin_new_stroke = near ? 0 : 1;
00280                                                 draw_info[i].end_stroke = 0;
00281                                                 draw_info[last_valid].end_stroke = draw_info[i].begin_new_stroke;
00282                                         }
00283                                         last_valid = i;
00284                                 } /*for */
00285                                 if(last_valid >= 0)
00286                                         draw_info[last_valid].end_stroke = 1;
00287                         } /* find buff .. */
00288                         
00289 
00290                         if(p.draw_confidence) { 
00291                                 int i;
00292                                 /* Compute interval */
00293                                 double interval[ld->nrays];
00294                                 double big_interval = 0.3;
00295                                 for(i=0;i<ld->nrays;i++) { if(draw_info[i].valid==0) continue;
00296                                         double sigma = ld->readings_sigma[i];
00297                                         if(!is_nan(cov)) {
00298                                                 interval[i] = p.confidence_mult * sigma;
00299                                         } else interval[i] = big_interval;
00300                                 }
00301 
00302                                 cairo_set_source_rgb(cr, 1.0, 0.5, 0.5);
00303                                 cairo_set_line_width(cr, 0.1);
00304                                 /* draw one */
00305                                 int j=0; for(j=0;j<2;j++)
00306                                 for(i=0;i<ld->nrays;i++) { if(draw_info[i].valid==0) continue;
00307                                         double b[2];
00308                                         ld_get_buffer_polar(ld->theta[i], 
00309                                                 ld->readings[i] + (j ? interval[i] : -interval[i]), 
00310                                                 pose, 0, 0, &bb,  &(b[0]), &(b[1]));
00311 
00312                                         if(draw_info[i].begin_new_stroke)
00313                                                 cairo_move_to(cr, b[0], b[1]);
00314                                         else
00315                                                 cairo_line_to(cr, b[0], b[1]);
00316                                         if(draw_info[i].end_stroke)
00317                                                 cairo_stroke(cr);
00318                                 }
00319                         } /* draw confidence */
00320                         
00321                         /* draw contour: begin_new_stroke and end_stroke tell 
00322                         when to interrupt the stroke */
00323                         int i; 
00324                         cairo_set_source_rgb (cr, 0.0, 0.0, 0.0);
00325                         cairo_set_line_width(cr, 0.5);
00326                         for(i=0;i<ld->nrays;i++) {
00327                                 if(draw_info[i].valid==0) continue;
00328                                 double *b = draw_info[i].b;
00329                                 if(draw_info[i].begin_new_stroke)
00330                                         cairo_move_to(cr, b[0], b[1]);
00331                                 else
00332                                         cairo_line_to(cr, b[0], b[1]);
00333                                 if(draw_info[i].end_stroke)
00334                                         cairo_stroke(cr);
00335                         }
00336 
00337                         
00338                         
00339                 }
00340                 counter++;
00341                 ld_free(ld);
00342         }
00343 
00344         cairo_show_page (cr);
00345 
00346         cairo_destroy (cr);
00347         cairo_surface_destroy (surface);
00348 }
00349 
00350 void ld_get_buffer_polar(double phi, double rho, const double*pose, 
00351         double*x, double*y,
00352         struct bounding_box*bb, double*bx,double *by) {
00353         
00354         double point[2];
00355         point[0] = cos(phi) * rho;
00356         point[1] = sin(phi) * rho;
00357 
00358         double frame[3] = { 0, 0, offset_theta};
00359         double pose2[3];
00360         oplus_d(frame, pose, pose2);
00361         double pw[2];
00362         transform_d(point, pose2, pw);
00363         
00364         if( (bb!=0) & (bx!=0) & (by!=0) )
00365         bb_w2b(bb, pw[0], pw[1],  bx, by);
00366         
00367         if((x!=0) && (y!=0)) {
00368                 *x = pw[0]; *y = pw[1];
00369         }
00370 }
00371 
00372 void ld_getbb(LDP  ld, double*x0, double*y0, double*x1, double*y1,
00373         reference use_reference, double horizon) {
00374         double *pose = ld_get_reference(ld, use_reference);
00375         
00376         int nrays_used = 0;
00377         int first=1;
00378         int i; for(i=0;i<ld->nrays;i++) {
00379                 if(!ld->valid[i]) continue;
00380                 if(ld->readings[i]>horizon) continue;
00381                 double x,y;
00382                 ld_get_buffer_polar(ld->theta[i], ld->readings[i], pose, &x, &y, 0, 0,0);
00383 
00384                 if(first) {
00385                         *x0 = *x1 = x;
00386                         *y0 = *y1 = y;
00387                         first = 0;
00388                 } else {
00389                         *x0 = GSL_MIN(*x0, x);
00390                         *y0 = GSL_MIN(*y0, y);
00391                         *x1 = GSL_MAX(*x1, x);
00392                         *y1 = GSL_MAX(*y1, y);
00393                 }
00394                 nrays_used++;
00395         }
00396 }
00397 
00398 
00399 
00400 


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