log2pdf.c
Go to the documentation of this file.
00001 #include <time.h>
00002 #include <string.h>
00003 
00004 
00005 #ifdef LINUX
00006 #include <linux/limits.h>
00007 #endif
00008 
00009 #include <cairo.h>
00010 #include <cairo-pdf.h>
00011 
00012 #include <options/options.h>
00013 
00014 #include "../csm/csm_all.h"
00015 #include "../csm/laser_data_drawing.h"
00016 #include "../csm/laser_data_cairo.h"
00017 
00018 
00019 typedef struct {
00020         const char*use;
00021         double padding;
00022         double dimension;
00023         
00024         int draw_confidence;
00025         double confidence_mult;
00026         
00027         const char*output_filename;
00028         const char*input_filename;
00029 
00030         ld_reference use_reference;
00031 
00032         double offset_theta_deg;
00033         
00034         /* Drawing style for scans */
00035         ld_style laser;
00036         /* Drawing style for robot path */
00037         line_style pose_path;
00038         double start_pose_width;
00039         
00040         double distance_xy, distance_th_deg;
00041 } log2pdf_params;
00042 
00043 int log2pdf(log2pdf_params *p);
00044 
00045 double offset_theta = 0;
00046 
00047 const char* banner = 
00048         "This program draws laser scans.\n"
00049         "\n"
00050         "IMPORTANT: it is likely you have to set one or more parameters. \n"
00051         "           the default parameters are OK to draw very long laser logs\n\n";
00052         
00053 int main(int argc, const char* argv[]) {
00054         sm_set_program_name(argv[0]);
00055 
00056         log2pdf_params p;
00057         
00058         lds_set_defaults(&(p.laser));
00059         ls_set_defaults(&(p.pose_path));
00060         
00061         p.laser.rays.draw = 0;
00062         p.laser.points.draw = 0;
00063         p.laser.normals.draw = 0;
00064         p.laser.countour.width = 0.1;
00065         p.pose_path.width = 0.1;
00066         p.pose_path.color = "#f00";
00067         
00068         options_banner(banner);
00069         struct option * ops = options_allocate(100);
00070         options_string(ops, "in", &p.input_filename, "stdin", "input file (Carmen or JSON)");
00071         options_string(ops, "out", &p.output_filename, "", "output file (if empty, input file + '.pdf')");
00072         options_double(ops, "padding", &p.padding, 0.2, "padding around bounding box (m)");
00073         options_double(ops, "dimension", &p.dimension, 500.0, "dimension of the image (points)");
00074         options_double(ops, "offset_theta_deg", &p.offset_theta_deg, 0.0, " rotate entire map by this angle (deg) ");
00075 
00076         options_string(ops, "use", &p.use, "estimate", "One in 'odometry','estimate','true_pose'");
00077         options_double(ops, "distance_xy", &p.distance_xy, 5.0, " Minimum distance between scans (m) ");
00078         options_double(ops, "distance_th_deg", &p.distance_th_deg, 45.0, " Minimum distance between scans (deg) ");
00079         options_double(ops, "start_pose_width", &p.start_pose_width, 0.4, "First pose | Circle width");
00080         lds_add_options(&(p.laser), ops, "laser_", "");
00081         ls_add_options(&(p.pose_path), ops, "path_", "");
00082         
00083         if(!options_parse_args(ops, argc, argv)) {
00084                 sm_error("Could not parse arguments.\n");
00085                 options_print_help(ops, stderr);
00086                 return -1;
00087         }
00088         
00089         /* If out not specified */
00090         if(strlen(p.output_filename)==0) {
00091                 char buf[PATH_MAX];
00092                 sprintf(buf, "%s.pdf", p.input_filename);
00093                 p.output_filename = my_strdup(buf);
00094 /*              sm_info("Writing on file '%s'.\n", p.output_filename);*/
00095         }
00096         
00097         p.use_reference = ld_string_to_reference(p.use);
00098         if(Invalid == p.use_reference) {
00099                 sm_error("Invalid reference '%s'. " 
00100                         "Use one in 'odometry','estimate','true_pose'.\n", p.use);
00101                 return -1;
00102         }
00103 /*      sm_info("Using reference: %s.\n", ld_reference_to_string(p.use_reference));*/
00104         
00105         return !log2pdf(&p);
00106 }
00107 
00108 
00109 int log2pdf(log2pdf_params *p) {
00110 
00112         FILE *input_file = open_file_for_reading(p->input_filename);
00113         if(!input_file) return 0;
00114         
00115         LDP*scans; int nscans;
00116         
00117         if(!ld_read_some_scans_distance(input_file,  &scans, &nscans,
00118                  p->use_reference, p->distance_xy, deg2rad(p->distance_th_deg) ) ){
00119                 sm_error("Could not read map from file '%s'.\n", p->input_filename); 
00120                 return 0;
00121         }
00122         
00123         if(nscans == 0) {
00124                 sm_error("I could not read any scan from file '%s'.\n", p->input_filename);
00125                 return 0;
00126         }
00127         
00128         sm_debug("Read map: %d scans in total.\n", nscans);
00129 
00131         double bb_min[2], bb_max[2];
00132         double offset[3] = {0,0,0};
00133         lda_get_bounding_box(scans, nscans, bb_min, bb_max, offset, p->use_reference, p->laser.horizon);
00134         
00135         bb_min[0] -= p->padding;
00136         bb_min[1] -= p->padding;
00137         bb_max[0] += p->padding;
00138         bb_max[1] += p->padding;
00139         
00140 
00141         sm_debug("Bounding box: %f %f -- %f %f.\n", bb_min[0], bb_min[1],
00142                 bb_max[0], bb_max[1]);
00143 
00144                 
00145         /* Create PDF surface and setup paper size and transformations */
00146         int max_width_points = p->dimension;
00147         int max_height_points = p->dimension;
00148         cairo_surface_t *surface;
00149         cairo_t *cr;
00150 
00151         if(!create_pdf_surface(p->output_filename, max_width_points, max_height_points, 
00152                 bb_min, bb_max, &surface, &cr)) return 0;
00153 
00154         /* Draw pose path */
00155         if(p->pose_path.draw) {
00156                 cairo_save(cr);
00157                 
00158                 cr_set_style(cr, &(p->pose_path));
00159                 cr_lda_draw_pose_path(cr, scans, nscans, p->use_reference);
00160 
00161                 if(nscans > 0 && p->laser.pose.draw) {
00162                         cairo_set_source_rgb(cr, 0.3, 0.0, 1.0);
00163                         double *pose0 = ld_get_reference_pose(scans[0], p->use_reference);
00164                         cairo_arc(cr, pose0[0], pose0[1], p->start_pose_width, 0.0, 2*M_PI);
00165                         cairo_fill(cr);
00166                 }
00167 
00168                 cairo_restore(cr);
00169         }
00170 
00171         /* Draw map */
00172         int k; for(k=0;k<nscans;k++) {
00173                 LDP ld = scans[k];
00174                 double *pose = ld_get_reference_pose(ld, p->use_reference);
00175                 if(!pose) continue;
00176                 
00177                 double offset[3] = {0,0, deg2rad(p->offset_theta_deg) };
00178                 double world_pose[3];
00179                 oplus_d(offset, pose, world_pose);
00180                                 
00181                 cairo_save(cr);
00182                 cr_set_reference(cr, world_pose);
00183                 cr_ld_draw(cr, ld, &(p->laser));
00184                 cairo_restore(cr);
00185         }
00186 
00187         cairo_show_page (cr);
00188 
00189         cairo_destroy (cr);
00190         cairo_surface_destroy (surface);
00191         return 1;
00192 }
00193 
00194 
00195 
00196 


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