Skip to content
Snippets Groups Projects
Commit cdc2f16c authored by Andrea Censi's avatar Andrea Censi
Browse files

vedo covarianza

parent 9fbe7784
No related branches found
No related tags found
No related merge requests found
......@@ -64,6 +64,8 @@ int main(int argc, const char*argv[]) {
/* sm_gpm(&params,&result); */
sm_icp(&params,&result);
JO jo = result_to_json(&params, &result);
printf("%s\n", json_object_to_json_string(jo));
}
return 0;
......
......@@ -61,12 +61,12 @@ inline void check_valid_val(val v) { int i = v.cid; v.cid=i;}
void check_valid_val(val v) {
int context = its_context(v);
if(context>cid) {
printf("Val is from invalid context (%d>%d)\n",context,cid);
fprintf(stderr, "Val is from invalid context (%d>%d)\n",context,cid);
error();
}
int var_index = its_var_index(v);
if(var_index >= egsl_contexts[context].nvars) {
printf("Val is invalid (%d>%d)\n",var_index,
fprintf(stderr, "Val is invalid (%d>%d)\n",var_index,
egsl_contexts[context].nvars);
error();
}
......@@ -99,13 +99,13 @@ void egsl_pop() {
}
void egsl_print_stats() {
printf("egsl: total allocations: %d cache hits: %d\n",
fprintf(stderr, "egsl: total allocations: %d cache hits: %d\n",
egsl_total_allocations, egsl_cache_hits);
/* printf("egsl: sizeof(val) = %d\n",(int)sizeof(val)); */
int c; for(c=0;c<MAX_CONTEXTS;c++) {
/* printf("egsl: context #%d\n ",c);
if(0==egsl_contexts[c].nallocated) break; */
printf("egsl: context #%d allocations: %d active: %d\n",
fprintf(stderr, "egsl: context #%d allocations: %d active: %d\n",
c, egsl_contexts[c].nallocated, egsl_contexts[c].nvars);
}
}
......@@ -186,23 +186,23 @@ void egsl_print(const char*str, val v) {
size_t i,j;
int context = its_context(v);
int var_index = its_var_index(v);
printf("%s = (%d x %d) context=%d index=%d\n",
fprintf(stderr, "%s = (%d x %d) context=%d index=%d\n",
str,(int)m->size1,(int)m->size2, context, var_index);
for(i=0;i<m->size1;i++) {
if(i==0)
printf(" [ ");
fprintf(stderr, " [ ");
else
printf(" ");
fprintf(stderr, " ");
for(j=0;j<m->size2;j++)
printf("%f ", gsl_matrix_get(m,i,j));
fprintf(stderr, "%f ", gsl_matrix_get(m,i,j));
if(i==m->size1-1)
printf("] \n");
fprintf(stderr, "] \n");
else
printf("; \n");
fprintf(stderr, "; \n");
}
}
......
......@@ -36,10 +36,10 @@ void egsl_print_spectrum(const char*s, val v) {
egsl_symm_eig(v, eval, evec);
size_t i,j;
for(j=0;j<n;j++) {
printf("%s | eval[%d] = %+5.5f evec[%d]= ",
fprintf(stderr, "%s | eval[%d] = %+5.5f evec[%d]= ",
s, (int)j, eval[j],(int)j);
for(i=0;i<n;i++)
printf("%+4.4f ", egsl_atv(evec[j],i));
printf(" sqrt(eval[%d])=%5.5f \n", (int)j, sqrt(eval[j]));
fprintf(stderr, "%+4.4f ", egsl_atv(evec[j],i));
fprintf(stderr, " sqrt(eval[%d])=%5.5f \n", (int)j, sqrt(eval[j]));
}
}
......@@ -352,7 +352,8 @@ int json_object_get_int(struct json_object *this)
static int json_object_double_to_json_string(struct json_object* this,
struct printbuf *pb)
{
return sprintbuf(pb, "%lf", this->o.c_double);
/* return sprintbuf(pb, "%lf", this->o.c_double);*/
return sprintbuf(pb, "%Lg", this->o.c_double);
}
struct json_object* json_object_new_double(double d)
......
......@@ -121,7 +121,8 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
laser_ref, laser_sens, best_x,
&cov0_x, &dx_dy1, &dx_dy2);
/* val cov_x = sc(params->sigma*params->sigma, cov0_x); */
val cov_x = sc(square(params->sigma), cov0_x);
egsl_v2da(cov_x, res->cov_x);
egsl_print("cov0_x", cov0_x);
egsl_print_spectrum("cov0_x", cov0_x);
......
......@@ -6,6 +6,20 @@
#include "laser_data_json.h"
JO result_to_json(struct sm_params*p, struct sm_result *r) {
JO jo = json_object_new_object();
jo_add(jo, "x", json_double_array(r->x, 3));
if(p->do_compute_covariance) {
/* double cov[9];
int j;for(j=0;j<9;j++) cov[j] = r->x_cov[(j-j%3)/3][j%3];*/
jo_add(jo, "cov_x", json_double_array(r->cov_x, 9));
}
jo_add(jo, "iterations", jo_new_int(r->iterations));
jo_add(jo, "nvalid", jo_new_int(r->nvalid));
jo_add(jo, "error", jo_new_double(r->error));
return jo;
}
JO ld_to_json(LDP ld) {
JO jo = json_object_new_object();
......
#ifndef H_LASER_DATA_JSON
#define H_LASER_DATA_JSON
#include "laser_data.h"
#include "sm.h"
#include <json.h>
JO ld_to_json(LDP);
LDP json_to_ld(JO);
JO result_to_json(struct sm_params*p, struct sm_result *r);
LDP ld_from_json_stream(FILE*);
#endif
\ No newline at end of file
......@@ -65,7 +65,7 @@ struct sm_params {
struct sm_result {
double x[3];
double x_cov[3][3];
double cov_x[9];
int iterations;
int nvalid;
double error;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment