laser_data_cairo.c
Go to the documentation of this file.
00001 
00002 #include <string.h>
00003 
00004 #include <cairo-pdf.h>
00005 #include <stdlib.h>
00006 #include <limits.h>
00007 
00008 #include "laser_data_cairo.h"
00009 
00010 const char* cat(const char*a, const char*b);
00011 void cr_ld_draw_rays(cairo_t*, LDP);
00012 void cr_ld_draw_countour(cairo_t*, LDP, double, double);
00013 void cr_ld_draw_points(cairo_t*, LDP, double radius);
00014 void cr_ld_draw_normals(cairo_t*cr, LDP ld, double length);
00015 
00016 
00017 
00018 /* ----------------------------------------------- */
00019 
00020 void cr_ld_draw_corr(cairo_t*cr, LDP laser_ref, LDP laser_sens) {
00021         int i;
00022         for(i=0; i < laser_sens->nrays; i++) {
00023                 if(!ld_valid_corr(laser_sens, i)) continue;
00024 
00025                 if(!laser_sens->corr[i].valid) continue;
00026 
00027                 int j1 = laser_sens->corr[i].j1;
00028                 int j2 = laser_sens->corr[i].j2;
00029                 
00030                 const double *p_j1  = laser_ref->points[j1].p;
00031                 const double *p_j2  = laser_ref->points[j2].p;
00032                 const double *p_i_w = laser_sens->points_w[i].p;
00033                 double proj[2];
00034         
00035                 if(laser_sens->corr[i].type == corr_pl)
00036                         projection_on_line_d(p_j1,  p_j2, p_i_w, proj, 0);
00037                 else
00038                         projection_on_segment_d(p_j1,  p_j2, p_i_w, proj);
00039         
00040                 cairo_move_to(cr, p_i_w[0], p_i_w[1]);
00041                 cairo_line_to(cr, proj[0],  proj[1]);
00042                 cairo_stroke(cr);       
00043         }
00044 }
00045 
00046 const char* cat(const char*a, const char*b) {
00047         size_t la = strlen(a);
00048         size_t lb = strlen(b);
00049         char* buf = malloc(la+lb+3);
00050         strcpy(buf, a);
00051         strcpy(buf+la, b);
00052         return buf;
00053 }
00054 
00055 void ls_add_options(line_style*ls, struct option*ops, 
00056         const char*prefix, const char*desc_prefix) 
00057 {
00058         options_int(ops, cat(prefix, "draw"), &(ls->draw), 
00059                 ls->draw, cat(desc_prefix, "Whether to draw it (0,1)"));
00060         
00061         options_string(ops, cat(prefix, "color"), &(ls->color), 
00062                 ls->color, cat(desc_prefix, "Color ('red', '#f00')"));
00063 
00064         options_double(ops, cat(prefix, "width"), &(ls->width), 
00065                 ls->width, cat(desc_prefix, "line width (meters)"));
00066                 
00067 }
00068 
00069 
00070 void lds_add_options(ld_style*lds, struct option*ops, 
00071         const char*prefix, const char*desc_prefix) 
00072 {
00073         ls_add_options(&(lds->rays), ops, cat(prefix, "rays_"),  cat(desc_prefix, "Rays | "));
00074         ls_add_options(&(lds->countour), ops, cat(prefix, "countour_"),  cat(desc_prefix, "Countour | "));
00075         ls_add_options(&(lds->points), ops, cat(prefix, "points_"),  cat(desc_prefix, "Points | "));
00076         
00077         options_double(ops, cat(prefix, "points_radius"), &(lds->points_radius), 
00078                 lds->points_radius, cat(desc_prefix, "Point radius"));
00079 
00080 
00081         ls_add_options(&(lds->pose), ops, cat(prefix, "pose_"),  cat(desc_prefix, "PoseMarker | "));
00082         
00083         options_double(ops, cat(prefix, "pose_radius"), &(lds->pose_radius), 
00084                 lds->pose_radius, cat(desc_prefix, "Point radius"));
00085 
00086         ls_add_options(&(lds->normals), ops, cat(prefix, "normals_"),  cat(desc_prefix, "Normals | "));
00087 
00088         options_double(ops, cat(prefix, "normals_length"), &(lds->normals_length), 
00089                 lds->normals_length, cat(desc_prefix, "Length of normals sticks (meters)"));
00090 
00091         ls_add_options(&(lds->sigma), ops, cat(prefix, "sigma_"),  cat(desc_prefix, "Sigma | "));
00092 
00093         options_double(ops, cat(prefix, "sigma_multiplier"), &(lds->sigma_multiplier), 
00094                 lds->sigma_multiplier, cat(desc_prefix, "Multiplier for sigma"));
00095 
00096 
00097         options_double(ops, cat(prefix, "connect_threshold"), &(lds->connect_threshold), 
00098                 lds->connect_threshold, cat(desc_prefix, "Threshold under which points are connected (m)."));
00099         options_double(ops, cat(prefix, "horizon"), &(lds->horizon), 
00100                 lds->horizon, cat(desc_prefix, "Maximum distance to plot (m)."));
00101 }
00102 
00103 void cr_set_color(cairo_t *cr, const char* color) {
00104         if(strlen(color) == 4 && color[0] == '#') {
00105                 char buf[2] = {0, 0};
00106                 double rgb[3];
00107                 int i; for(i=0;i<3;i++) {
00108                         buf[0] = color[1+i];
00109                         char* endptr;
00110                         rgb[i] = (1/15.0) * strtol(buf, &endptr, 16);
00111                         if(endptr == buf) {
00112                                 sm_error("Unknown color component: %s.\n", buf);
00113                         }
00114                 }
00115                 cairo_set_source_rgb (cr, rgb[0], rgb[1], rgb[2]);
00116         } else if(strlen(color) == 5 && color[0] == '#') {
00117                         char buf[2] = {0, 0};
00118                         double rgba[4];
00119                         int i; for(i=0;i<4;i++) {
00120                                 buf[0] = color[1+i];
00121                                 char* endptr;
00122                                 rgba[i] = (1/15.0) * strtol(buf, &endptr, 16);
00123                                 if(endptr == buf) {
00124                                         sm_error("Unknown color component: %s.\n", buf);
00125                                 }
00126                         }
00127                         cairo_set_source_rgba (cr, rgba[0], rgba[1], rgba[2], rgba[3]);
00128         } else {
00129                 if(!strcmp(color, "black")) {
00130                         cairo_set_source_rgb (cr, 0.0, 0.0, 0.0);
00131                 } else {
00132                         sm_error("Unknown color: %s.\n", color);
00133                         cairo_set_source_rgb (cr, 0.0, 1.0, 1.0);               
00134                 }
00135         }
00136 }
00137 
00138 void cr_set_style(cairo_t *cr, line_style *ls) {
00139         cr_set_color(cr, ls->color);
00140         cairo_set_line_width(cr, ls->width);
00141 }
00142 
00143 void cr_lda_draw_pose_path(cairo_t*cr, LDP*scans, int nscans, ld_reference use_reference) {
00144         int k; int first_pose=1;
00145         for(k=0;k<nscans;k++) {
00146                 LDP ld = scans[k];
00147                 double *pose = ld_get_reference_pose(ld, use_reference);
00148                 if(!pose) {
00149                         sm_error("No '%s' pose specified for scan #%d, continuing.\n",
00150                                 ld_reference_to_string(use_reference));
00151                         continue;
00152                 }
00153         
00154                 if(first_pose) { 
00155                         first_pose = 0; 
00156                         cairo_move_to(cr, pose[0], pose[1]);
00157                 } else {
00158                         cairo_line_to(cr, pose[0], pose[1]);
00159                 }
00160         }
00161         cairo_stroke(cr);
00162 }
00163 
00164 
00165 
00166 void cr_ld_draw_rays(cairo_t*cr, LDP ld) {
00167         int i; for(i=0;i<ld->nrays;i++) {
00168                 if(!ld_valid_ray(ld, i)) continue;
00169                 
00170                 double threshold = 0.03;
00171                 
00172 /*              if(ld->readings[i]<2) continue;*/
00173                 
00174                 double x1 = threshold * cos(ld->theta[i]);
00175                 double y1 = threshold * sin(ld->theta[i]);
00176                 double x2 = ld->readings[i] * cos(ld->theta[i]);
00177                 double y2 = ld->readings[i] * sin(ld->theta[i]);
00178                 
00179                 cairo_move_to(cr,x1,y1);
00180                 cairo_line_to(cr,x2,y2);
00181                 cairo_stroke(cr);
00182         }
00183 }
00184 
00185 void cr_ld_draw_countour(cairo_t*cr, LDP ld, double horizon, double connect_threshold) {
00186         struct stroke_sequence draw_info[ld->nrays];
00187         compute_stroke_sequence(ld, draw_info, horizon, connect_threshold);
00188         
00189         /* draw contour: begin_new_stroke and end_stroke tell 
00190         when to interrupt the stroke */
00191         int i; 
00192         for(i=0;i<ld->nrays;i++) {
00193 
00194                 if(draw_info[i].valid==0) continue;
00195 
00196                 double *p = draw_info[i].p;
00197                 
00198                 if(draw_info[i].begin_new_stroke)
00199                         cairo_move_to(cr, p[0], p[1]);
00200                 else
00201                         cairo_line_to(cr, p[0], p[1]);
00202 /*              if(draw_info[i].end_stroke)*/
00203         }
00204         cairo_stroke(cr);
00205 }
00206 
00207 void cr_ld_draw_points(cairo_t*cr, LDP ld, double radius) {
00208         int i; for(i=0;i<ld->nrays;i++) {
00209                 if(!ld_valid_ray(ld, i)) continue;
00210 
00211                 double x = ld->readings[i] * cos(ld->theta[i]);
00212                 double y = ld->readings[i] * sin(ld->theta[i]);
00213 
00214                 cairo_arc (cr, x, y, radius, 0.0, 2*M_PI);
00215                 cairo_fill(cr);
00216         }
00217 }
00218 
00219 void cr_ld_draw_normals(cairo_t*cr, LDP ld, double length) {
00220         int i; for(i=0;i<ld->nrays;i++) {
00221                 if(!ld_valid_ray(ld, i) || !ld_valid_alpha(ld, i)) continue;
00222 
00223                 double alpha = ld->alpha[i];
00224                 double x1 = ld->readings[i] * cos(ld->theta[i]);
00225                 double y1 = ld->readings[i] * sin(ld->theta[i]);
00226                 double x2 = x1 + cos(alpha) * length;
00227                 double y2 = y1 + sin(alpha) * length;
00228 
00229                 cairo_move_to(cr, x1, y1);
00230                 cairo_line_to(cr, x2, y2);
00231         }
00232         cairo_stroke (cr);
00233 }
00234 
00235 void cr_ld_draw_sigma(cairo_t*cr, LDP ld, double multiplier) {
00236         int i; for(i=0;i<ld->nrays;i++) {
00237                 if(!ld_valid_ray(ld, i) || is_nan(ld->readings_sigma[i])) continue;
00238 
00239                 double theta = ld->theta[i];
00240                 double length = ld->readings_sigma[i] * multiplier;
00241                 
00242                 double x0 = ld->readings[i] * cos(theta);
00243                 double y0 = ld->readings[i] * sin(theta);
00244                 double x1 = x0 + cos(theta) * length;
00245                 double y1 = y0 + sin(theta) * length;
00246                 double x2 = x0 - cos(theta) * length;
00247                 double y2 = y0 - sin(theta) * length;
00248 
00249                 cairo_move_to(cr, x1, y1);
00250                 cairo_line_to(cr, x2, y2);
00251         }
00252         cairo_stroke (cr);
00253 }
00254 
00255 
00256 void cr_ld_draw(cairo_t* cr, LDP ld, ld_style *p) {
00257         if(p->rays.draw) {
00258                 cr_set_style(cr, &(p->rays));
00259                 cr_ld_draw_rays(cr, ld);
00260         }
00261                 
00262         if(p->countour.draw)  {
00263                 cr_set_style(cr, &(p->countour));
00264                 cr_ld_draw_countour(cr, ld, p->horizon, p->connect_threshold);
00265         }
00266 
00267         if(p->points.draw) {
00268                 cr_set_style(cr, &(p->points));
00269                 cr_ld_draw_points(cr, ld, p->points_radius);
00270         }
00271 
00272         if(p->normals.draw) {
00273                 cr_set_style(cr, &(p->normals));
00274                 cr_ld_draw_normals(cr, ld, p->normals_length);
00275         }
00276 
00277         if(p->sigma.draw) {
00278                 cr_set_style(cr, &(p->sigma));
00279                 cr_ld_draw_sigma(cr, ld, p->sigma_multiplier);
00280         }
00281         
00282         if(p->pose.draw) {
00283                 cr_set_style(cr, &(p->pose));
00284                 cairo_move_to(cr, 0.0, 0.0);
00285                 cairo_arc (cr, 0.0, 0.0, p->pose_radius, 0.0, 2*M_PI);
00286                 cairo_fill (cr);
00287         }
00288 }
00289 
00290 void cr_set_reference(cairo_t*cr, double*pose) {
00291         cairo_translate(cr,pose[0],pose[1]);
00292         cairo_rotate(cr,pose[2]);
00293 }
00294 
00295 void ls_set_defaults(line_style*ls) {
00296         ls->draw = 1;
00297         ls->color = "black";
00298         ls->width = 0.002;
00299 }
00300 
00301 void lds_set_defaults(ld_style*lds) {
00302         ls_set_defaults(&(lds->rays));
00303         lds->rays.color = "#f00";
00304         lds->rays.width = 0.0002;
00305         
00306         ls_set_defaults(&(lds->countour));
00307         ls_set_defaults(&(lds->points));
00308         lds->points_radius = 0.003;
00309         lds->points.color = "#f00";
00310         
00311         ls_set_defaults(&(lds->pose));
00312         lds->pose.color = "#f73";
00313         lds->pose_radius = 0.24;
00314         
00315         lds->normals_length = 0.10;
00316         ls_set_defaults(&(lds->normals));
00317 
00318         lds->sigma_multiplier = 3;
00319         ls_set_defaults(&(lds->sigma));
00320         lds->sigma.draw = 0;
00321         
00322         lds->connect_threshold = 0.20;
00323         lds->horizon = 10;
00324 }
00325 
00326 int create_pdf_surface(const char*file, int max_width_points, int max_height_points,
00327         double bb_min[2], double bb_max[2], cairo_surface_t**surface_p, cairo_t **cr) {
00328         double bb_width = bb_max[0] - bb_min[0], bb_height = bb_max[1] - bb_min[1];
00329         
00330         
00331         double surface_width, surface_height;
00332         if( bb_width > bb_height ) {
00333                 /* largo e basso */
00334                 surface_width = max_width_points;
00335                 surface_height =  (surface_width / bb_width) * bb_height;
00336         } else {
00337                 /* stretto e alto */
00338                 surface_height = max_height_points;
00339                 surface_width =  (surface_height / bb_height) * bb_width;
00340         }
00341 
00342         sm_debug("bb: %f %f\n", bb_width, bb_height);
00343         sm_debug("surface: %f %f\n", surface_width, surface_height);
00344         
00345         *surface_p = cairo_pdf_surface_create(file, surface_width, surface_height);
00346         *cr = cairo_create (*surface_p);
00347         cairo_status_t status = cairo_status (*cr);
00348 
00349         if (status) {
00350                 sm_error("Failed to create pdf surface for file %s: %s\n",
00351                         file, cairo_status_to_string (status));
00352                 return 0;
00353         }
00354 
00355         double world_to_surface = surface_width / bb_width;
00356         cairo_scale(*cr, world_to_surface, -world_to_surface );
00357         cairo_translate(*cr, -bb_min[0], -bb_max[1]);
00358         
00359         return 1;
00360 }
00361 
00362 int create_image_surface(int max_width_pixels, int max_height_pixels,
00363         double bb_min[2], double bb_max[2], cairo_surface_t**surface_p, cairo_t **cr) {
00364         double bb_width = bb_max[0] - bb_min[0], bb_height = bb_max[1] - bb_min[1];
00365 
00366         double surface_width, surface_height;
00367         if( bb_width > bb_height ) {
00368                 /* largo e basso */
00369                 surface_width = max_width_pixels;
00370                 surface_height =  (surface_width / bb_width) * bb_height;
00371         } else {
00372                 /* stretto e alto */
00373                 surface_height = max_height_pixels;
00374                 surface_width =  (surface_height / bb_height) * bb_width;
00375         }
00376 
00377         sm_debug("bb: %f %f\n", bb_width, bb_height);
00378         sm_debug("surface: %f %f\n", surface_width, surface_height);
00379 
00380         *surface_p = cairo_image_surface_create(CAIRO_FORMAT_ARGB32, (int) surface_width, (int)surface_height);
00381         *cr = cairo_create (*surface_p);
00382         cairo_status_t status = cairo_status (*cr);
00383 
00384         if (status) {
00385                 sm_error("Failed to create image surface: %s\n",
00386                         cairo_status_to_string (status));
00387                 return 0;
00388         }
00389 
00390         double world_to_surface = surface_width / bb_width;
00391         cairo_scale(*cr, world_to_surface, -world_to_surface );
00392         cairo_translate(*cr, -bb_min[0], -bb_max[1]);
00393 
00394         return 1;
00395 }
00396 
00397 


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