00001 #include <errno.h>
00002 #include <string.h>
00003 #include <math.h>
00004 #include <ctype.h>
00005
00006 #include "csm_all.h"
00007
00008 void jo_add_timestamp(JO jo, const char*name, struct timeval*tv);
00009 int is_all_nan(const double *v, int n );
00010 void jo_add_double_array_if_not_nan(JO jo, const char*name, const double *v, int n);
00011 int all_is(int *a, int n, int v);
00012
00013
00014
00015
00016 JO matrix_to_json(gsl_matrix*m) {
00017 JO jo = jo_new_array();
00018 if(m->size1>1) {
00019 size_t i,j;
00020 for(i=0;i<m->size1;i++) {
00021 JO row = json_object_new_array();
00022 for(j=0;j<m->size2;j++) {
00023 double v = gsl_matrix_get(m,i,j);
00024 json_object_array_add(row, jo_double_or_null(v));
00025 }
00026 json_object_array_add(jo, row);
00027 }
00028 } else {
00029 size_t i = 1, j;
00030 JO row = jo;
00031 for(j=0;j<m->size2;j++) {
00032 double v = gsl_matrix_get(m,i,j);
00033 json_object_array_add(row, jo_double_or_null(v));
00034 }
00035 }
00036 return jo;
00037 }
00038
00039 int json_to_corr(JO array, struct correspondence*corr, int n) {
00041 int i;
00042 for(i=0;i<n;i++) {
00043 JO element = json_object_array_get_idx(array, i);
00044 if(element == NULL) {
00045 corr[i].valid = 0;
00046 corr[i].j1 = -1;
00047 corr[i].j2 = -1;
00048 } else {
00049 corr[i].valid = 1;
00050 jo_read_int(element, "j1", &(corr[i].j1));
00051 jo_read_int(element, "j2", &(corr[i].j2));
00052 int type;
00053 jo_read_int(element, "type", &(type));
00054 corr[i].type = type;
00055 }
00056 }
00057 return 1;
00058 }
00059
00060 JO corr_to_json(struct correspondence*corr, int n) {
00061 JO jo = jo_new_array();
00062 int i;
00063 for(i=0;i<n;i++) {
00064 if(corr[i].valid) {
00065 JO c = jo_new();
00066 jo_add_int(c, "j1", corr[i].j1);
00067 jo_add_int(c, "j2", corr[i].j2);
00068 jo_add_int(c, "type", (int) corr[i].type);
00069 jo_array_add(jo, c);
00070 } else {
00071 jo_array_add(jo, jo_new_null());
00072 }
00073 }
00074 return jo;
00075 }
00076
00077
00078 JO vector_to_json(gsl_vector*vec) {
00079 JO jo = json_object_new_array();
00080 size_t j;
00081 for(j=0;j<vec->size;j++) {
00082 double v = gsl_vector_get(vec,j);
00083 jo_array_add(jo, jo_double_or_null(v));
00084 }
00085 return jo;
00086 }
00087
00088
00089 void jo_add_timestamp(JO jo, const char*name, struct timeval*tv) {
00090 int array[2] = {tv->tv_sec, tv->tv_usec};
00091 jo_add_int_array(jo, name, array, 2);
00092 }
00093
00094 JO result_to_json(struct sm_params*p, struct sm_result *r) {
00095 JO jo = json_object_new_object();
00096 jo_add_int(jo, "valid", r->valid);
00097
00098 if(r->valid) {
00099 jo_add_double_array(jo, "x", r->x, 3);
00100
00101 if(p->do_compute_covariance) {
00102 jo_add(jo, "cov_x", matrix_to_json(r->cov_x_m ) );
00103 jo_add(jo, "dx_dy1", matrix_to_json(r->dx_dy1_m) );
00104 jo_add(jo, "dx_dy2", matrix_to_json(r->dx_dy2_m) );
00105 }
00106 }
00107 jo_add_int(jo, "iterations", r->iterations);
00108 jo_add_int(jo, "nvalid", r->nvalid);
00109 jo_add_double(jo, "error", r->error);
00110
00111 jo_add_timestamp(jo, "laser_ref_timestamp", &(p->laser_ref->tv));
00112 jo_add_timestamp(jo, "laser_sens_timestamp", &(p->laser_sens->tv));
00113
00114 return jo;
00115 }
00116
00117 int is_all_nan(const double *v, int n ) {
00118 int i; for(i=0;i<n;i++) if(v[i]==v[i]) return 0;
00119 return 1;
00120 }
00121
00123 void jo_add_double_array_if_not_nan(JO jo, const char*name, const double *v, int n) {
00124 if(is_all_nan(v,n)) return;
00125 jo_add_double_array(jo, name, v, n);
00126 }
00127
00129 int all_is(int *a, int n, int v) {
00130 int i; for(i=0;i<n;i++) if(a[i]!=v) return 0;
00131 return 1;
00132 }
00133
00134 JO ld_to_json(LDP ld) {
00135 JO jo = json_object_new_object();
00136 int n = ld->nrays;
00137
00138 jo_add_int(jo, "nrays", ld->nrays);
00139 jo_add_double(jo, "min_theta", ld->min_theta);
00140 jo_add_double(jo, "max_theta", ld->max_theta);
00141
00142 jo_add_double_array(jo, "odometry", ld->odometry, 3);
00143 jo_add_double_array(jo, "estimate", ld->estimate, 3);
00144 jo_add_double_array(jo, "true_pose", ld->true_pose, 3);
00145
00146 jo_add_double_array(jo, "theta", ld->theta, n);
00147 jo_add_double_array(jo, "readings", ld->readings, n);
00148 jo_add_double_array_if_not_nan(jo, "readings_sigma", ld->readings_sigma, n);
00149
00150 jo_add_int_array(jo, "valid", ld->valid, n);
00151
00152 if(!all_is(ld->cluster, n, -1))
00153 jo_add_int_array(jo, "cluster", ld->cluster, n);
00154
00155 jo_add_double_array_if_not_nan(jo, "alpha", ld->alpha, n);
00156 jo_add_double_array_if_not_nan(jo, "cov_alpha", ld->cov_alpha, n);
00157
00158 if(!all_is(ld->alpha_valid, n, 0))
00159 jo_add_int_array(jo, "alpha_valid", ld->alpha_valid, n);
00160
00161 jo_add_double_array_if_not_nan(jo, "true_alpha", ld->true_alpha, n);
00162
00163 int timeval[2] = {ld->tv.tv_sec, ld->tv.tv_usec};
00164 jo_add_int_array(jo, "timestamp", timeval, 2);
00165
00166 return jo;
00167 }
00168
00169
00170 LDP json_to_ld(JO jo) {
00171 int n;
00172 if(!jo_read_int(jo, "nrays", &n)) {
00173 sm_error("Could not read nrays.\n");
00174 return 0;
00175 }
00176
00177 LDP ld = ld_alloc_new(n);
00178
00179 jo_read_double(jo, "min_theta", &ld->min_theta);
00180 jo_read_double(jo, "max_theta", &ld->max_theta);
00181
00182 jo_read_double_array(jo, "theta", ld->theta, n, NAN);
00183 jo_read_double_array(jo, "readings", ld->readings, n, NAN);
00184
00185 if(jo_has_field(jo,"readings_sigma") && !jo_read_double_array(jo, "readings_sigma", ld->readings_sigma, n, NAN))
00186 { sm_error("Error while reading field 'readings_sigma'.\n"); return 0; }
00187
00188 jo_read_int_array(jo, "valid", ld->valid, n, 0);
00189 jo_read_int_array(jo, "cluster", ld->cluster, n, -1);
00190
00191 if(jo_has_field(jo,"alpha") && !jo_read_double_array(jo, "alpha",ld->alpha, n, NAN))
00192 { sm_error("Error while reading field alpha.\n"); return 0; }
00193
00194 if(jo_has_field(jo,"cov_alpha") && !jo_read_double_array(jo, "cov_alpha", ld->cov_alpha, n, NAN))
00195 { sm_error("Error while reading field cov_alpha.\n"); return 0; }
00196
00197 if(jo_has_field(jo,"alpha_valid") && !jo_read_int_array(jo, "alpha_valid", ld->alpha_valid, n, 0))
00198 { sm_error("Error while reading field alpha_valid.\n"); return 0; }
00199
00200 if(jo_has_field(jo,"true_alpha") && !jo_read_double_array(jo, "true_alpha", ld->true_alpha, n, NAN))
00201 { sm_error("Error while reading field true_alpha.\n"); return 0; }
00202
00203
00204 jo_read_double_array(jo, "odometry", ld->odometry, 3, NAN);
00205 jo_read_double_array(jo, "estimate", ld->estimate, 3, NAN);
00206 jo_read_double_array(jo, "true_pose", ld->true_pose, 3, NAN);
00207
00208 int array[2] = {-1,-1};
00209 jo_read_int_array(jo, "timestamp", array, 2,-1);
00210 ld->tv.tv_sec = array[0];
00211 ld->tv.tv_usec = array[1];
00212
00213 return ld;
00214 }
00215
00216 LDP ld_from_json_string(const char*s) {
00217 JO jo = json_parse(s);
00218 if(!jo) {
00219 sm_error("Invalid JSON found.\n");
00220 return 0;
00221 }
00222 LDP ld = json_to_ld(jo);
00223 if(!ld) {
00224 sm_error("Could not read laser_data:\n\n%s\n", json_object_to_json_string(jo));
00225 return 0;
00226 }
00227 jo_free(jo);
00228 return ld;
00229 }
00230
00231 LDP ld_from_json_stream(FILE*file) {
00232 JO jo;
00233 LDP ld;
00234
00235 jo = json_read_stream(file);
00236 if(!jo) {
00237 if(!feof(file)) {
00238 fprintf(stderr, " (!)\n");
00239 sm_error("Invalid JSON found.\n");
00240 }
00241 fprintf(stderr, " EOF\n");
00242
00243 return 0;
00244 }
00245
00246 ld = json_to_ld(jo);
00247 if(!ld) {
00248 sm_error("Could not read laser_data:\n\n%s\n", json_object_to_json_string(jo));
00249 return 0;
00250 }
00251 jo_free(jo);
00252
00253 fprintf(stderr, "l");
00254
00255 return ld;
00256 }
00257
00258 void ld_write_as_json(LDP ld, FILE * stream) {
00259 if(!ld_valid_fields(ld)) {
00260 sm_error("Writing bad data to the stream.\n");
00261 }
00262
00263 JO jo = ld_to_json(ld);
00264 fputs(json_object_to_json_string(jo), stream);
00265 fputs("\n", stream);
00266 jo_free(jo);
00267 }
00268