sm_animate.c
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <string.h>
00003 #include <float.h>
00004 #include <sys/types.h>
00005 #include <sys/stat.h>
00006 
00007 #include <cairo-pdf.h>
00008 
00009 #include <options/options.h>
00010 
00011 #include "../csm/csm_all.h"
00012 #include "../csm/laser_data_drawing.h"
00013 #include "../csm/laser_data_cairo.h"
00014 
00015 typedef struct {
00016         const char * file_input;
00017 
00018         const char * file_output;
00019         
00020         ld_style laser_ref_s, laser_sens_s;
00021         /* Drawing style for correspondences */
00022         line_style corr;
00023         
00024         
00025         double max_width_cm;
00026         double max_height_cm;
00027         
00028         int max_iterations;
00029         int zoom_ray;
00030         
00031         /* maximum size, in points, of the output pdf */
00032         int width_pt, height_pt;
00033         
00034         /* Padding, in meters, to be added */
00035         double padding;
00036         
00037         int write_info;
00038 } anim_params ;
00039 
00040 int draw_animation( anim_params* p, JO jo, const char*filename);
00041 void set_defaults(anim_params *p);
00042 
00043 
00044 int main(int argc, const char** argv)
00045 {
00046         sm_set_program_name(argv[0]);
00047 
00048         anim_params p;
00049         set_defaults(&p);
00050         
00051         struct option* ops = options_allocate(100);
00052         options_string(ops, "in", &p.file_input, "stdin", "Input file (defaults to stdin)");
00053         options_string(ops, "out", &p.file_output, "sm_animate_%02d.pdf", "Output file ");
00054 
00055         options_int(ops, "write_info", &p.write_info, 0, "Writes informations and statistics in the picture.");
00056         options_int(ops, "max_iterations", &p.max_iterations, 10, "Maximum number of iterations");
00057         options_int(ops, "zoom_ray", &p.zoom_ray, -1, "If >= 0, the action is zoomed on a particular ray.");
00058         options_int(ops, "width_pt", &p.width_pt, 500, "Maximum width, in points, of the PDF.");
00059         options_int(ops, "height_pt", &p.height_pt, 500, "Maximum height, in points, of the PDF.");
00060         options_double(ops, "padding", &p.padding, 0.2, "Padding, in meters, to be added around figure.");
00061 
00062         lds_add_options(&(p.laser_ref_s), ops, "ref_", "");
00063         lds_add_options(&(p.laser_sens_s), ops, "sens_", "");
00064         ls_add_options(&(p.corr), ops, "corr_", "");
00065         
00066         if(!options_parse_args(ops, argc, argv)) {
00067                 sm_info("Draws ICP animation. It reads the output created by sm2 when given the 'file_jj' switch. \n\nUsage:\n");
00068                 options_print_help(ops, stderr);
00069                 return -1;
00070         }
00071 
00072         FILE * input = open_file_for_reading(p.file_input);
00073         if(!input) return -1;
00074         
00075         JO jo; int count = 0;
00076         while( (jo = json_read_stream(input)) ) {
00077                 char filename[100];
00078                 sprintf(filename, p.file_output, count);
00079                 sm_info("Writing frame %s \n", p.file_output);
00080                 if(!draw_animation(&p, jo, filename))
00081                         return -2;
00082                 count++;
00083         }
00084 
00085         return 0;
00086 }
00087 
00089 int draw_animation(anim_params* p, JO jo, const char*filename) {
00090         JO jo_ref = jo_get(jo, "laser_ref");
00091         JO jo_sens = jo_get(jo, "laser_sens");
00092         if(!jo_ref || !jo_sens) {
00093                 sm_error("Could not get laser_ref/laser_sens.\n");
00094                 return 0;
00095         }
00096         
00097         LDP laser_ref = json_to_ld(jo_ref);
00098         LDP laser_sens = json_to_ld(jo_sens); 
00099         if(!laser_ref || !laser_sens) {
00100                 sm_error("Could not read laser_ref/laser_sens from JSON representation.\n");
00101                 return 0;
00102         }
00103         
00104         ld_compute_cartesian(laser_ref);
00105         ld_compute_cartesian(laser_sens);
00106 
00107         double ld_min[2], ld_max[2];
00108         if(p->zoom_ray == -1) {
00109                 double zero[3] = {0,0,0};
00110                 if(!ld_get_bounding_box(laser_ref, ld_min, ld_max, zero, p->laser_ref_s.horizon)){
00111                         sm_error("Not enough good points to establish bounding box.\n");
00112                         return 0;
00113                 }
00114         } else {
00115                 if(p->zoom_ray < 0 || p->zoom_ray >= laser_ref->nrays || !ld_valid_ray(laser_ref, p->zoom_ray)) {
00116                         sm_error("Ray index #%d is not valid in laser_ref.\n", p->zoom_ray);
00117                         return 0;
00118                 }
00119                 
00120                 ld_min[0] = ld_max[0] = laser_ref->points[p->zoom_ray].p[0];
00121                 ld_min[1] = ld_max[1] = laser_ref->points[p->zoom_ray].p[1];
00122         }
00123         
00124         ld_min[0] -= p->padding;
00125         ld_min[1] -= p->padding;
00126         ld_max[0] += p->padding;
00127         ld_max[1] += p->padding;
00128         
00129         sm_info("Bounding box: %f %f -- %f %f\n", ld_min[0], ld_min[1], ld_max[0], ld_max[1]);
00130 
00131         cairo_surface_t *surface;
00132         cairo_t *cr;
00133         
00134         if(!create_pdf_surface(filename, p->width_pt, p->height_pt, 
00135                 ld_min, ld_max, &surface, &cr)) return 0;
00136         
00137         JO iterations = jo_get(jo, "iterations");
00138         if(!iterations || !json_object_is_type(iterations, json_type_array)) {
00139                 fprintf(stderr, "Could not read iterations.\n");
00140                 return 0;
00141         }
00142         
00143         int niterations = json_object_array_length(iterations);
00144         if(niterations>p->max_iterations) niterations = p->max_iterations;
00145         sm_info("Displaying %d iterations.\n", niterations);
00146 
00147         int it;
00148         for(it=0;it<niterations;it++) {
00149                 JO iteration = json_object_array_get_idx(iterations, it);
00150                 
00151                 double x_old[3], x_new[3];
00152                 jo_read_double_array(iteration, "x_old", x_old, 3, NAN);
00153                 jo_read_double_array(iteration, "x_new", x_new, 3, NAN);
00154 
00155 
00156                 cairo_save(cr);
00157                         cr_ld_draw(cr, laser_ref, &(p->laser_ref_s));
00158 
00159                         ld_compute_world_coords(laser_sens, x_old);
00160 
00161 
00162                         JO corr0 = jo_get(iteration, "corr0");
00163                         JO corr1 = jo_get(iteration, "corr1");
00164                         JO corr2 = jo_get(iteration, "corr2");
00165                         if(!corr1 || !corr2 || !corr0) {
00166                                 sm_error("Iteration %d: could not read correspondences (field 'corr<i>'). Probably ICP failed here?\n", it);
00167                         } else {
00168                                 cr_set_style(cr, &(p->corr));
00169                                 cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
00170                                 json_to_corr(corr0, laser_sens->corr, laser_sens->nrays);
00171                                 cr_ld_draw_corr(cr, laser_ref, laser_sens);
00172 
00173                                 cairo_set_source_rgb (cr, 1.0, 0.0, 1.0);
00174                                 json_to_corr(corr1, laser_sens->corr, laser_sens->nrays);
00175                                 cr_ld_draw_corr(cr, laser_ref, laser_sens);
00176 
00177                                 cairo_set_source_rgb (cr, 0.0, 1.0, 0.0);
00178                                 json_to_corr(corr2, laser_sens->corr, laser_sens->nrays);
00179                                 cr_ld_draw_corr(cr, laser_ref, laser_sens);
00180                         }
00181 
00182                         cr_set_reference(cr, x_old);
00183                         cr_ld_draw(cr, laser_sens, &(p->laser_sens_s));
00184 
00185                 cairo_restore(cr);
00186 
00187                 if(p->write_info) {
00188                         cairo_save(cr);
00189                                 cairo_identity_matrix(cr);
00190                                 cairo_set_font_size (cr, 20.0f);
00191                                 cairo_select_font_face (cr, "Sans",
00192                                     CAIRO_FONT_SLANT_NORMAL,
00193                                     CAIRO_FONT_WEIGHT_NORMAL);
00194 
00195                                 char text[100];
00196                                 sprintf(text, "Iteration #%d: x_old: %s", it, friendly_pose(x_old));
00197                                 cairo_move_to(cr,  0.0, -20.0 );
00198                                 cairo_show_text(cr, text);
00199                                 
00200                                 sm_info("%s\n",text);
00201                         cairo_restore(cr);
00202                 }
00203         
00204                 cairo_show_page (cr);
00205         }
00206         
00207         ld_free(laser_ref);
00208         ld_free(laser_sens);
00209 
00210         cairo_destroy (cr);
00211         cairo_surface_destroy (surface);
00212         
00213         return 1;
00214 }
00215 
00216 
00217 void set_defaults(anim_params *p) {
00218         lds_set_defaults(&(p->laser_ref_s));
00219         lds_set_defaults(&(p->laser_sens_s));
00220         ls_set_defaults(&(p->corr));
00221         p->laser_ref_s.points.color = "#00f";
00222         p->laser_sens_s.points.color = "#f00";
00223         p->laser_ref_s.pose.color = p->laser_ref_s.points.color;
00224         p->laser_sens_s.pose.color = p->laser_sens_s.points.color;
00225         p->laser_sens_s.pose_radius = 
00226         p->laser_ref_s.pose_radius = 0.015;
00227         
00228 }
00229 


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