raytracer.cpp
Go to the documentation of this file.
00001 #include "raytracer.h"
00002 
00003 struct raytracer_params {
00004         const char * file_map;
00005         const char * file_poses;
00006         const char * file_output;
00007         double fov_deg;
00008         int nrays;
00009         double max_reading;
00010 };
00011 
00012 
00013 bool load_env_from_json(Environment& env, JO jo);
00014 double cosine_between_angles(double a1, double a2);
00015 
00016 
00017 int main(int argc, const char** argv)
00018 {
00019         sm_set_program_name(argv[0]);
00020         struct raytracer_params p;
00021         
00022         struct option* ops = options_allocate(60);
00023         options_string(ops, "map", &p.file_map, "map.json", "Environment description");
00024         options_string(ops, "poses", &p.file_poses, "-", "List of poses");
00025         options_string(ops, "out", &p.file_output, "stdout", "Output file ");
00026         options_int(ops, "nrays", &p.nrays, 181,  "Number of rays");
00027         options_double(ops, "fov_deg", &p.fov_deg, 180.0,  "Field of view (degrees)");
00028         options_double(ops, "max_reading", &p.max_reading, 80.0,  "Sensor horizon (meters)");
00029         
00030         if(!options_parse_args(ops, argc, argv)) {
00031                 sm_info(" simulats one scan from map. \n\nUsage:\n");
00032                 options_print_help(ops, stderr);
00033                 return -1;
00034         }
00035         
00036         
00037         FILE * file_map = open_file_for_reading(p.file_map);
00038         if(!file_map) return -2;
00039         
00040         JO jo_map = json_read_stream(file_map); 
00041         if(!jo_map) {
00042                 sm_error("Could not read JSON from file '%s'\n", p.file_map);
00043                 return -3;
00044         }
00045         if(json_read_stream(file_map)) {
00046                 sm_error("I expect only 1 JSON object in file '%s'\n", p.file_map);
00047                 return -4;
00048         }
00049 
00050         Environment env;
00051         
00052         load_env_from_json(env, jo_map);
00053         
00054         
00055         FILE * file_output = open_file_for_writing(p.file_output);
00056         if(!file_output) return -1;
00057 
00058 
00059         FILE * file_poses = open_file_for_reading(p.file_poses);
00060         if(!file_poses) return -2;
00061         
00062         JO jo_pose; int num = 0;
00063         while( (jo_pose = json_read_stream(file_poses)) )
00064         {
00065                 num++;
00066         
00067                 LDP ld = ld_alloc_new(p.nrays);
00068 
00069                 if(!jo_read_from_double_array (jo_pose, ld->true_pose, 3, GSL_NAN) || any_nan(ld->true_pose, 3)) {
00070                         sm_error("Bad pose: '%s'.\n", jo_to_string(jo_pose));
00071                         return -5;
00072                 }
00073                 
00074                 
00075                 double fov = deg2rad(p.fov_deg);
00076                 
00077                 ld->min_theta = -fov/2;
00078                 ld->max_theta = +fov/2;
00079                 
00080                 for(int i=0;i<ld->nrays;i++) {
00081                         ld->theta[i] = - fov/2 + fov * i / (ld->nrays-1);;
00082                         
00083                         double rho, alpha; int stuff_id;
00084                         if(env.ray_tracing(ld->true_pose, ld->theta[i] + ld->true_pose[2], rho, alpha, &stuff_id) && (rho<p.max_reading)) {
00085                         
00086                                 ld->valid[i] = 1;
00087                                 ld->readings[i] = rho;
00088                                 
00089                                 double relative_alpha = alpha-ld->true_pose[2];
00090 
00091                                 /* Make sure alpha points out of the wall */
00092                                 if( cosine_between_angles(relative_alpha, ld->theta[i]) > 0) {
00093                                         relative_alpha += M_PI;
00094                                 }
00095                                 
00096                                 ld->true_alpha[i] = normalize_0_2PI(relative_alpha);
00097 
00098                         } else {
00099                                 ld->valid[i] = 0;
00100                         } 
00101                         
00102                 }
00103                 
00104                 ld->tv.tv_sec = num;
00105 
00106 
00107                 JO jo = ld_to_json(ld);
00108                 fputs(jo_to_string(jo), file_output);
00109                 fputs("\n", file_output);
00110                 jo_free(jo);
00111 
00112                 ld_free(ld);
00113         }       
00114 
00115         if(num == 0) {
00116                 sm_error("It looks like there wasn't any pose in the file '%s'\n", p.file_poses);
00117                 return -4;
00118         }
00119 
00120         jo_free(jo_map);
00121 }
00122 
00123 
00124 
00125 #define jo_expect_array(a) (a!=0 && json_object_is_type(a, json_type_array))
00126 #define jo_expect_object(a) (a!=0 &&  json_object_is_type(a, json_type_object))
00127 #define jo_expect_string(a) (a!=0 && json_object_is_type(a, json_type_string))
00128 #define jo_expect_array_size(a,n) ( (a!=0) && (json_object_is_type(a, json_type_array)&& (jo_array_length(a)==n)))
00129 #define jo_expect_array_size_min(a,n) ( (a!=0) && (json_object_is_type(a, json_type_array)&& (jo_array_length(a)>=n)))
00130 
00131 
00132 #define expect(a) if(!a) { \
00133                 sm_error("Invalid format: \n\t %s \n", #a); \
00134                         return false; \
00135                 }
00136 
00137 #define expect_s(a, s) if(!a) { \
00138                 sm_error("Invalid format: %s \n\t %s \n", s, #a); \
00139                         return false; \
00140                 }
00141                 
00142 double cosine_between_angles(double a1, double a2) {
00143         return cos(a1)*cos(a2)+sin(a1)*sin(a2);
00144 }
00145 
00146 
00147 bool load_env_from_json(Environment& env, JO jo_map) {
00148         jo_expect_object(jo_map);
00149         
00150         JO jo_objects = json_object_object_get(jo_map, "objects");
00151                 expect(jo_expect_array(jo_objects));
00152         
00153         for(int i=0; i < jo_array_length(jo_objects); i++) {
00154                 JO jo = jo_array_get(jo_objects, i);
00155                         expect(jo_expect_object(jo));
00156                 
00157                 JO jo_type = jo_get(jo, "type"); 
00158                         expect(jo_expect_string(jo_type));
00159                         
00160                 if(!strcmp(jo_get_string(jo_type), "line")) {
00161                         
00162                         JO points = jo_get(jo, "points");
00163                         expect(jo_expect_array_size_min(points, 2));
00164                         
00165                         
00166                         for(int p=0;p<jo_array_length(points)-1;p++) {
00167                                 Segment * s = new Segment();
00168                                 expect(jo_read_from_double_array (jo_array_get(points, p  ), s->p0, 2, NAN));
00169                                 expect(jo_read_from_double_array (jo_array_get(points, p+1), s->p1, 2, NAN));
00170                                 env.stuff.push_back(s);
00171                         }
00172 
00173                 } else if(!strcmp(jo_get_string(jo_type), "square")) {
00174                                 
00175                                 JO corners = jo_get(jo, "corners");
00176                                 expect_s(jo_expect_array_size(corners, 2), jo_to_string(corners));
00177 
00178                                 double pmin[2],pmax[2];
00179 
00180                                 expect(jo_read_from_double_array (jo_array_get(corners, 0  ), pmin, 2, NAN));
00181                                 expect(jo_read_from_double_array (jo_array_get(corners, 1), pmax, 2, NAN));
00182 
00183                                 env.stuff.push_back(new Segment(pmin[0],pmin[1],pmax[0],pmin[1]));
00184                                 env.stuff.push_back(new Segment(pmax[0],pmax[1],pmax[0],pmin[1]));
00185                                 env.stuff.push_back(new Segment(pmax[0],pmax[1],pmin[0],pmax[1]));
00186                                 env.stuff.push_back(new Segment(pmin[0],pmin[1],pmin[0],pmax[1]));
00187 
00188                 } else {
00189                         sm_error("unknown object type: '%s'\n", jo_get_string(jo_type));
00190                         return false;
00191                 }
00192         }
00193         
00194         return true;
00195 }
00196 
00197 


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