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
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
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
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
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
00247 if(should_consider(&p, counter)) {
00248
00249
00250 struct {
00251 double w[2];
00252 double b[2];
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 }
00285 if(last_valid >= 0)
00286 draw_info[last_valid].end_stroke = 1;
00287 }
00288
00289
00290 if(p.draw_confidence) {
00291 int i;
00292
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
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 }
00320
00321
00322
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