carmen2pdf.c
Go to the documentation of this file.
1 #include <time.h>
2 #include <string.h>
3 
4 #ifdef LINUX
5 #include <linux/limits.h>
6 #endif
7 
8 
9 #include <cairo.h>
10 #include <cairo-pdf.h>
11 
12 #include <options/options.h>
13 
14 #include "../csm/csm_all.h"
15 
16 
17 typedef enum { Invalid = 0, Odometry = 1, Estimate = 2, True_pose = 3 } reference;
18 const char*reference_name[4] = { "invalid","odometry","estimate","true_pose"};
19 
20 struct params {
21  int interval;
22  const char*use;
23  double padding;
24  double horizon;
25  double line_threshold;
26  double dimension;
27 
28  int draw_confidence;
29  double confidence_mult;
30 
31  const char*output_filename;
32  const char*input_filename;
33 
34 
35  FILE*input_file;
36  reference use_reference;
37 
38  double offset_theta_deg;
39 };
40 
41 void carmen2pdf(struct params p);
42 
43 double offset_theta = 0;
44 void ld_getbb(LDP ld, double*x0, double*y0, double*x1, double*y1,
45  reference use_reference, double horizon);
46 
47 int main(int argc, const char*argv[]) {
48  sm_set_program_name(argv[0]);
49  fprintf(stderr, "carmen2pdf:\t *** Please use log2pdf instead. ***\n\n");
50 
51  struct params p;
52 
53  struct option * ops = options_allocate(12);
54  options_int(ops, "interval", &p.interval, 10, "how many to ignore");
55  options_string(ops, "in", &p.input_filename, "stdin", "input file (Carmen or JSON)");
56  options_string(ops, "out", &p.output_filename, "", "output file (if empty, input file + '.pdf')");
57  options_double(ops, "lt", &p.line_threshold, 0.2, "threshold for linking points (m)");
58  options_double(ops, "horizon", &p.horizon, 8.0, "horizon of the laser (m)");
59  options_double(ops, "padding", &p.padding, 0.2, "padding around bounding box (m)");
60  options_double(ops, "dimension", &p.dimension, 500.0, "dimension of the image (points)");
61  options_int(ops, "draw_confidence", &p.draw_confidence, 0, " Draws confidence (readings_sigma[i]) ");
62  options_double(ops, "confidence_mult", &p.confidence_mult, 3.0, " 3-sigma ");
63  options_double(ops, "offset_theta_deg", &p.offset_theta_deg, 0.0, " rotate entire map by this angle (deg) ");
64 
65  options_string(ops, "use", &p.use, "estimate", "One in 'odometry','estimate','true_pose'");
66 
67  if(!options_parse_args(ops, argc, argv)) {
68  sm_error("Could not parse arguments.\n");
69  options_print_help(ops, stderr);
70  return -1;
71  }
72 
73  /* If out not specified */
74  if(strlen(p.output_filename)==0) {
75  char buf[PATH_MAX];
76  sprintf(buf, "%s.pdf", p.input_filename);
77  p.output_filename = strdup(buf);
78  sm_info("Writing on file '%s'.\n", p.output_filename);
79  }
80 
81  p.use_reference = Invalid;
82  int i; for(i=1;i<=3;i++)
83  if(!strcmp(p.use, reference_name[i]))
84  p.use_reference = (reference) i;
85  if(Invalid == p.use_reference) {
86  sm_error("Invalid reference '%s'. "
87  "Use one in 'odometry','estimate','true_pose'.\n", p.use);
88  return -1;
89  }
90 
91 
92 
93 
94  p.input_file = open_file_for_reading(p.input_filename);
95  if(!p.input_file) return -1;
96 
97  carmen2pdf(p);
98  return 0;
99 }
100 
101 int should_consider(struct params *p, int counter) {
102  return counter%p->interval == 0;
103 }
104 
105 void ld_get_world(LDP ld, int i, double*x, double*y, reference use_reference);
106 
107 struct bounding_box {
109  double x0,y0,x1,y1;
110  /* Paper size */
111  double width, height;
112 };
113 
114 void bb_w2b(struct bounding_box*bb, double wx, double wy, double*bx, double*by){
115  double scale = GSL_MIN(bb->width / (bb->x1-bb->x0), bb->height / (bb->y1-bb->y0));
116  *bx = (wx-bb->x0) * scale;
117  *by = bb->height- (wy-bb->y0) * scale;
118 }
119 
120 void ld_get_buffer_polar(double phi, double rho, const double*pose,
121  double*x, double*y,
122  struct bounding_box*bb, double*bx,double *by);
123 
124 
126 void get_bb(struct params*p, struct bounding_box*bb) {
127  LDP ld;
128  int counter = -1,
129  considered = 0;
130 
131  while((ld = ld_read_smart(p->input_file))) {
132  counter++;
133  if(should_consider(p, counter)) {
134  if(!ld_valid_fields(ld)) {
135  sm_error("Invalid laser data (#%d in file)\n", counter);
136  continue;
137  }
138 
139  double x0,y0,x1,y1;
140  ld_getbb(ld,&x0,&y0,&x1,&y1, p->use_reference, p->horizon);
141  if(considered > 0) {
142  bb->x0 = GSL_MIN(x0, bb->x0);
143  bb->x1 = GSL_MAX(x1, bb->x1);
144  bb->y0 = GSL_MIN(y0, bb->y0);
145  bb->y1 = GSL_MAX(y1, bb->y1);
146  } else {
147  /* this is the first one */
148  bb->x0 = x0;
149  bb->x1 = x1;
150  bb->y0 = y0;
151  bb->y1 = y1;
152  }
153 
154  considered++;
155  }
156  ld_free(ld);
157  }
158  sm_info("Considering %d of %d scans.\n", considered, counter+1);
159  rewind(p->input_file);
160 
161  bb->x0 -= p->padding;
162  bb->x1 += p->padding;
163  bb->y0 -= p->padding;
164  bb->y1 += p->padding;
165 }
166 
167 
168 double * ld_get_reference(LDP ld, reference use_reference) {
169  double * pose;
170  switch(use_reference) {
171  case Odometry: pose = ld->odometry; break;
172  case Estimate: pose = ld->estimate; break;
173  case True_pose: pose = ld->true_pose; break;
174  default: exit(-1);
175  }
176  if(any_nan(pose, 3)) {
177  sm_error("Required field '%s' not set in laser scan.\n",
178  reference_name[use_reference] );
179  sm_error("I will abruptly exit() because of a panic attack.\n");
180  exit(-1);
181  }
182  return pose;
183 }
184 
185 
186 void carmen2pdf(struct params p) {
187 
188  offset_theta += deg2rad(p.offset_theta_deg);
189 
190  struct bounding_box bb;
191  get_bb(&p, &bb);
192 
193  double wwidth = bb.x1-bb.x0;
194  double wheight= bb.y1-bb.y0;
195  if(wwidth > wheight) {
196  bb.width = p.dimension;
197  bb.height = bb.width / wwidth * wheight;
198  } else {
199  bb.height = p.dimension;
200  bb.width = bb.height / wheight * wwidth;
201  }
202 
203  sm_info("Bounding box: %f %f, %f %f\n",bb.x0,bb.y0,bb.x1,bb.y1);
204 
205  cairo_surface_t *surface;
206  cairo_t *cr;
207  cairo_status_t status;
208 
209  surface = cairo_pdf_surface_create(p.output_filename, bb.width, bb.height);
210  cr = cairo_create (surface);
211  status = cairo_status (cr);
212 
213  if (status) {
214  sm_error("Failed to create pdf surface for file %s: %s\n",
215  p.output_filename, cairo_status_to_string (status));
216  return;
217  }
218 
219  int counter=0;
220  int first_pose=1; double old_pose_bx=0,old_pose_by=0;
221  LDP ld;
222  while((ld = ld_read_smart(p.input_file))) {
223 
224  double *pose = ld_get_reference(ld, p.use_reference);
225 
226  /* Draw pose */
227  {
228  double bx,by;
229  ld_get_buffer_polar(0.0,0.0,pose, 0,0, &bb, &bx, &by);
230  if(first_pose) {
231  first_pose = 0;
232  } else {
233  cairo_set_line_width(cr, 0.5);
234  cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
235  cairo_move_to(cr, old_pose_bx, old_pose_by);
236  cairo_line_to(cr, bx, by);
237  cairo_close_path(cr);
238  cairo_stroke(cr);
239  }
240 
241  old_pose_bx = bx;
242  old_pose_by = by;
243  }
244 
245 
246  /* If should we draw this sensor scan */
247  if(should_consider(&p, counter)) {
248 
249  /* Firstly, find buffer coordinates and whether to cut the stroke */
250  struct {
251  double w[2]; /* world coordinates */
252  double b[2]; /* buffer coordinates */
253  int begin_new_stroke;
254  int end_stroke;
255  int valid;
256  } draw_info[ld->nrays];
257 
258  {
259  int last_valid = -1; int first = 1;
260  int i; for(i=0;i<ld->nrays;i++) {
261 
262  if( (!ld->valid[i]) || ld->readings[i]>p.horizon) {
263  draw_info[i].valid = 0;
264  continue;
265  }
266  draw_info[i].valid = 1;
267 
268  ld_get_buffer_polar(ld->theta[i], ld->readings[i],
269  pose, &(draw_info[i].w[0]), &(draw_info[i].w[1]),
270  &bb, &(draw_info[i].b[0]), &(draw_info[i].b[1]));
271 
272  if(first) {
273  first = 0;
274  draw_info[i].begin_new_stroke = 1;
275  draw_info[i].end_stroke = 0;
276  } else {
277  int near = square(p.line_threshold) >
278  distance_squared_d(draw_info[last_valid].w, draw_info[i].w);
279  draw_info[i].begin_new_stroke = near ? 0 : 1;
280  draw_info[i].end_stroke = 0;
281  draw_info[last_valid].end_stroke = draw_info[i].begin_new_stroke;
282  }
283  last_valid = i;
284  } /*for */
285  if(last_valid >= 0)
286  draw_info[last_valid].end_stroke = 1;
287  } /* find buff .. */
288 
289 
290  if(p.draw_confidence) {
291  int i;
292  /* Compute interval */
293  double interval[ld->nrays];
294  double big_interval = 0.3;
295  for(i=0;i<ld->nrays;i++) { if(draw_info[i].valid==0) continue;
296  double sigma = ld->readings_sigma[i];
297  if(!is_nan(cov)) {
298  interval[i] = p.confidence_mult * sigma;
299  } else interval[i] = big_interval;
300  }
301 
302  cairo_set_source_rgb(cr, 1.0, 0.5, 0.5);
303  cairo_set_line_width(cr, 0.1);
304  /* draw one */
305  int j=0; for(j=0;j<2;j++)
306  for(i=0;i<ld->nrays;i++) { if(draw_info[i].valid==0) continue;
307  double b[2];
308  ld_get_buffer_polar(ld->theta[i],
309  ld->readings[i] + (j ? interval[i] : -interval[i]),
310  pose, 0, 0, &bb, &(b[0]), &(b[1]));
311 
312  if(draw_info[i].begin_new_stroke)
313  cairo_move_to(cr, b[0], b[1]);
314  else
315  cairo_line_to(cr, b[0], b[1]);
316  if(draw_info[i].end_stroke)
317  cairo_stroke(cr);
318  }
319  } /* draw confidence */
320 
321  /* draw contour: begin_new_stroke and end_stroke tell
322  when to interrupt the stroke */
323  int i;
324  cairo_set_source_rgb (cr, 0.0, 0.0, 0.0);
325  cairo_set_line_width(cr, 0.5);
326  for(i=0;i<ld->nrays;i++) {
327  if(draw_info[i].valid==0) continue;
328  double *b = draw_info[i].b;
329  if(draw_info[i].begin_new_stroke)
330  cairo_move_to(cr, b[0], b[1]);
331  else
332  cairo_line_to(cr, b[0], b[1]);
333  if(draw_info[i].end_stroke)
334  cairo_stroke(cr);
335  }
336 
337 
338 
339  }
340  counter++;
341  ld_free(ld);
342  }
343 
344  cairo_show_page (cr);
345 
346  cairo_destroy (cr);
347  cairo_surface_destroy (surface);
348 }
349 
350 void ld_get_buffer_polar(double phi, double rho, const double*pose,
351  double*x, double*y,
352  struct bounding_box*bb, double*bx,double *by) {
353 
354  double point[2];
355  point[0] = cos(phi) * rho;
356  point[1] = sin(phi) * rho;
357 
358  double frame[3] = { 0, 0, offset_theta};
359  double pose2[3];
360  oplus_d(frame, pose, pose2);
361  double pw[2];
362  transform_d(point, pose2, pw);
363 
364  if( (bb!=0) & (bx!=0) & (by!=0) )
365  bb_w2b(bb, pw[0], pw[1], bx, by);
366 
367  if((x!=0) && (y!=0)) {
368  *x = pw[0]; *y = pw[1];
369  }
370 }
371 
372 void ld_getbb(LDP ld, double*x0, double*y0, double*x1, double*y1,
373  reference use_reference, double horizon) {
374  double *pose = ld_get_reference(ld, use_reference);
375 
376  int nrays_used = 0;
377  int first=1;
378  int i; for(i=0;i<ld->nrays;i++) {
379  if(!ld->valid[i]) continue;
380  if(ld->readings[i]>horizon) continue;
381  double x,y;
382  ld_get_buffer_polar(ld->theta[i], ld->readings[i], pose, &x, &y, 0, 0,0);
383 
384  if(first) {
385  *x0 = *x1 = x;
386  *y0 = *y1 = y;
387  first = 0;
388  } else {
389  *x0 = GSL_MIN(*x0, x);
390  *y0 = GSL_MIN(*y0, y);
391  *x1 = GSL_MAX(*x1, x);
392  *y1 = GSL_MAX(*y1, y);
393  }
394  nrays_used++;
395  }
396 }
397 
398 
399 
400 
True_pose
@ True_pose
Definition: carmen2pdf.c:17
any_nan
int any_nan(const double *d, int n)
Definition: math_utils.c:64
transform_d
void transform_d(const double point2d[2], const double pose[3], double result2d[2])
Definition: math_utils.c:108
laser_data::readings_sigma
double *restrict readings_sigma
Definition: laser_data.h:32
bounding_box::x0
double x0
Definition: carmen2pdf.c:109
bounding_box::x1
double x1
Definition: carmen2pdf.c:109
bounding_box::width
double width
Definition: carmen2pdf.c:111
buf
char buf[100]
Definition: ld_recover.c:87
p
struct @0 p
ld_valid_fields
int ld_valid_fields(LDP ld)
Definition: laser_data.c:179
main
int main(int argc, const char *argv[])
Definition: carmen2pdf.c:47
laser_data::theta
double *restrict theta
Definition: laser_data.h:21
deg2rad
#define deg2rad(x)
Definition: gpc_test.c:22
bounding_box::y1
double y1
Definition: carmen2pdf.c:109
params::horizon
double horizon
Definition: carmen2pdf.c:24
laser_data::readings
double *restrict readings
Definition: laser_data.h:24
params::padding
double padding
Definition: carmen2pdf.c:23
laser_data::odometry
double odometry[3]
Definition: laser_data.h:39
options.h
ld_free
void ld_free(LDP ld)
Definition: laser_data.c:87
square
double square(double x)
Definition: math_utils.c:124
bb_w2b
void bb_w2b(struct bounding_box *bb, double wx, double wy, double *bx, double *by)
Definition: carmen2pdf.c:114
ld_get_world
void ld_get_world(LDP ld, int i, double *x, double *y, reference use_reference)
laser_data::nrays
int nrays
Definition: laser_data.h:17
options_double
void options_double(struct option *, const char *name, double *p, double def_value, const char *desc)
Definition: options_interface.c:62
params::interval
int interval
Definition: carmen2pdf.c:21
sm_set_program_name
void sm_set_program_name(const char *name)
Definition: logging.c:21
Estimate
@ Estimate
Definition: carmen2pdf.c:17
get_bb
void get_bb(struct params *p, struct bounding_box *bb)
Definition: carmen2pdf.c:126
params::input_filename
const char * input_filename
Definition: carmen2pdf.c:32
params::use
const char * use
Definition: carmen2pdf.c:22
params::line_threshold
double line_threshold
Definition: carmen2pdf.c:25
options_allocate
struct option * options_allocate(int n)
Definition: options_interface.c:16
options_int
void options_int(struct option *, const char *name, int *p, int def_value, const char *desc)
Definition: options_interface.c:39
ld_getbb
void ld_getbb(LDP ld, double *x0, double *y0, double *x1, double *y1, reference use_reference, double horizon)
Definition: carmen2pdf.c:372
carmen2pdf
void carmen2pdf(struct params p)
Definition: carmen2pdf.c:186
distance_squared_d
double distance_squared_d(const double a[2], const double b[2])
Definition: math_utils.c:48
bounding_box::y0
double y0
Definition: carmen2pdf.c:109
params::input_file
FILE * input_file
Definition: carmen2pdf.c:35
Odometry
@ Odometry
Definition: carmen2pdf.c:17
open_file_for_reading
FILE * open_file_for_reading(const char *filename)
Definition: utils.c:19
is_nan
int is_nan(double v)
Definition: math_utils.c:60
should_consider
int should_consider(struct params *p, int counter)
Definition: carmen2pdf.c:101
reference_name
const char * reference_name[4]
Definition: carmen2pdf.c:18
laser_data::estimate
double estimate[3]
Definition: laser_data.h:40
ops
struct option * ops
Definition: rb_sm.c:31
params::offset_theta_deg
double offset_theta_deg
Definition: carmen2pdf.c:38
ld_get_buffer_polar
void ld_get_buffer_polar(double phi, double rho, const double *pose, double *x, double *y, struct bounding_box *bb, double *bx, double *by)
Definition: carmen2pdf.c:350
sm_info
void sm_info(const char *msg,...)
Definition: logging.c:71
params::draw_confidence
int draw_confidence
Definition: carmen2pdf.c:28
option
Definition: options.h:49
options_print_help
void options_print_help(struct option *options, FILE *f)
Definition: options.c:398
Invalid
@ Invalid
Definition: carmen2pdf.c:17
interval
double interval
Definition: ld_resample.c:12
params::use_reference
reference use_reference
Definition: carmen2pdf.c:36
bounding_box
Definition: carmen2pdf.c:107
sm_error
void sm_error(const char *msg,...)
Definition: logging.c:49
offset_theta
double offset_theta
Definition: carmen2pdf.c:43
oplus_d
void oplus_d(const double x1[3], const double x2[3], double res[3])
Definition: math_utils.c:96
params::confidence_mult
double confidence_mult
Definition: carmen2pdf.c:29
laser_data
Definition: laser_data.h:16
ld_read_smart
LDP ld_read_smart(FILE *)
Definition: laser_data_load.c:130
reference
reference
Definition: carmen2pdf.c:17
laser_data::true_pose
double true_pose[3]
Definition: laser_data.h:38
options_string
void options_string(struct option *, const char *name, const char **p, const char *def_balue, const char *desc)
Definition: options_interface.c:72
options_parse_args
int options_parse_args(struct option *ops, int argc, const char *argv[])
Definition: options.c:66
laser_data::valid
int *restrict valid
Definition: laser_data.h:23
ld_get_reference
double * ld_get_reference(LDP ld, reference use_reference)
Definition: carmen2pdf.c:168
bounding_box::height
double height
Definition: carmen2pdf.c:111
params::dimension
double dimension
Definition: carmen2pdf.c:26
params::output_filename
const char * output_filename
Definition: carmen2pdf.c:31
params
Definition: carmen2pdf.c:20

csm
Author(s): Andrea Censi
autogenerated on Wed Aug 17 2022 02:50:33