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
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
00032 int width_pt, height_pt;
00033
00034
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