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
00035 ld_style laser;
00036
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
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
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
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
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
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
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