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
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
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