hsm_interface.c
Go to the documentation of this file.
00001 #include <options/options.h>
00002 #include <assert.h>
00003 
00004 #include "../csm_all.h"
00005 
00006 void hsm_add_options(struct option* ops, struct hsm_params*p) {
00007         options_double(ops, "hsm_linear_cell_size", &p->linear_cell_size, 0.03, "HSM: Size of a rho cell");
00008         options_double(ops, "hsm_angular_cell_size_deg", &p->angular_cell_size_deg, 1.0, "HSM: Size of angualar cell (deg)");
00009         options_int(ops, "hsm_num_angular_hypotheses", &p->num_angular_hypotheses, 8, "HSM: Number of angular hypotheses.");
00010         options_double(ops, "hsm_xc_directions_min_distance_deg", &p->xc_directions_min_distance_deg, 10.0, "HSM: Min distance between directions for cross corr (deg)");
00011         options_int(ops, "hsm_xc_ndirections", &p->xc_ndirections, 3, "HSM: Number of directions for cross corr (deg)");
00012         options_double(ops, "hsm_angular_hyp_min_distance_deg", &p->angular_hyp_min_distance_deg, 10.0, "HSM: Min distance between different angular hypotheses  (deg)");
00013         
00014         options_int(ops, "hsm_linear_xc_max_npeaks", &p->linear_xc_max_npeaks, 5, "HSM: Number of peaks per direction for linear translation");
00015         options_double(ops, "hsm_linear_xc_peaks_min_distance", &p->linear_xc_peaks_min_distance, 5.0, "HSM: Min distance between different peaks in linear correlation");
00016 }
00017 
00018 int hsm_compute_ht_for_scan(LDP ld, struct hsm_params*p, const double base[3], hsm_buffer *b);
00019 
00020 int hsm_compute_ht_for_scan(LDP ld, struct hsm_params*p, const double base[3], hsm_buffer *b) {
00021         *b = 0;
00022         
00024         double max_reading = max_in_array(ld->readings, ld->nrays);
00025         
00026         if(!(max_reading>0)) {
00027                 sm_error("No valid points.\n");
00028                 return 0;
00029         }
00030         
00031         p->max_norm = norm_d(base) + max_reading;
00032         
00033         *b = hsm_buffer_alloc(p);
00034         hsm_compute_ht_base(*b, base);
00035         
00036         ld_compute_cartesian(ld);
00037         int np = 0;
00038         for(int i=0; i<ld->nrays; i++) {
00039                 if(!ld_valid_ray(ld, i)) continue;
00040                 
00041                 hsm_compute_ht_point(*b, ld->points[i].p[0], ld->points[i].p[1], 1.0);
00042                 
00043                 np++;
00044         }
00045         
00046         sm_debug("Computed HT with %d points.\n", np);
00047         if(np<5) {
00048                 hsm_buffer_free(*b);
00049                 *b = 0;
00050                 return 0;
00051         } else {
00052                 return 1;
00053         }
00054 }
00055         
00056 void sm_hsm(struct sm_params* params, struct sm_result* res) {
00057         res->valid = 0;
00058         
00059         params->first_guess[0]=0.2;
00060         params->first_guess[1]=0;
00061         params->first_guess[2]=0;
00062         
00063 
00064         /* use true information if present */
00065         int has_true1 = !any_nan(params->laser_ref->true_pose, 3);
00066         int has_true2 = !any_nan(params->laser_sens->true_pose, 3);
00067         if(has_true1 && has_true2) {
00068                 params->hsm.debug_true_x_valid = 1;
00069                 
00070                 double true_x[3];
00071                 pose_diff_d(params->laser_sens->true_pose, params->laser_ref->true_pose, true_x);
00072 
00073                 /* This is the difference between results and true_x */
00074                 pose_diff_d(true_x, params->first_guess,  params->hsm.debug_true_x);
00075                 
00076         } else {
00077                 params->hsm.debug_true_x_valid = 0;
00078         }
00079         
00080         double zero[3] = {0,0,0};
00081         hsm_buffer b1, b2;
00082         int ok1 = hsm_compute_ht_for_scan(params->laser_ref, &(params->hsm), zero, &b1);
00083         int ok2 = hsm_compute_ht_for_scan(params->laser_sens,&(params->hsm),  params->first_guess, &b2);
00084         
00085         if(!ok1 || !ok2) {
00086                 sm_error("Could not compute buffers (too few points?).\n");
00087                 if(b1) hsm_buffer_free(b1);
00088                 if(b2) hsm_buffer_free(b2);
00089                 return;
00090         }
00091 
00092         hsm_compute_spectrum(b1);
00093         hsm_compute_spectrum(b2);
00094 
00095         params->hsm.max_translation = max(b1->rho_max, b2->rho_max);
00096         
00097         hsm_match(&(params->hsm),b1,b2);
00098 
00099 
00100         if(b1->num_valid_results)       {
00101                 res->valid = 1;
00102                 double pl[3];
00103                 double d2[3];
00104                 
00105                 pose_diff_d(params->first_guess, b1->results[0], res->x);
00106                 pose_diff_d(b1->results[0], params->first_guess,  d2);
00107                 oplus_d(params->first_guess, b1->results[0], pl);
00108                 
00109                 sm_info("hsm: odo   = %s\n", friendly_pose(params->first_guess));
00110                 sm_info("hsm: res   = %s\n", friendly_pose(b1->results[0]));
00111                 sm_info("hsm: plus  = %s\n", friendly_pose(pl));
00112                 sm_info("hsm: d2  = %s\n", friendly_pose(d2));
00113                 sm_info("hsm: xmin  = %s\n", friendly_pose(res->x));
00114                 res->error = 0;
00115                 res->iterations = 0;
00116                 res->nvalid = 0;
00117         } else {
00118                 sm_error("HSM did not produce any result.\n");
00119                 res->valid = 0;
00120         }
00121         
00122         
00123         hsm_buffer_free(b1);
00124         hsm_buffer_free(b2);
00125 }
00126 


csm
Author(s): Andrea Censi
autogenerated on Fri May 17 2019 02:28:33