laser_data_drawing.c
Go to the documentation of this file.
00001 #include <string.h>
00002 #include <strings.h>
00003 #include "laser_data_drawing.h"
00004 #include "logging.h"
00005 #include "math_utils.h"
00006 
00007 const char*ld_reference_name[4] = { "invalid","odometry","estimate","true_pose"};
00008 
00009 const char*ld_reference_to_string(ld_reference r) {
00010         return ld_reference_name[r];
00011 }
00012 
00013 ld_reference ld_string_to_reference(const char*s) {
00014         int i; for(i=1;i<=3;i++) 
00015                 if(!strcasecmp(s, ld_reference_to_string( (ld_reference) i) ))
00016                         return (ld_reference) i;
00017                         
00018         sm_error("Could not translate string '%s' to a reference name.\n", s);
00019         return Invalid;
00020 }
00021 
00022 int ld_get_bounding_box(LDP ld, double bb_min[2], double bb_max[2],
00023         double pose[3], double horizon) {
00024 
00025         int rays_used = 0;
00026         int i; for(i=0;i<ld->nrays;i++) {
00027                 if(!ld->valid[i]) continue;
00028                 if(ld->readings[i]>horizon) continue;
00029 
00030                 double p0[2] = {
00031                         cos(ld->theta[i]) * ld->readings[i],
00032                         sin(ld->theta[i]) * ld->readings[i]
00033                 };
00034                 
00035                 double p[2];
00036                 transform_d(p0,pose,p);
00037                 
00038                 if(0 == rays_used) {
00039                         bb_min[0] = bb_max[0] = p[0];
00040                         bb_min[1] = bb_max[1] = p[1];
00041                 } else {
00042                         int j=0; for(j=0;j<2;j++) {
00043                                 bb_min[j] = GSL_MIN(bb_min[j], p[j]);
00044                                 bb_max[j] = GSL_MAX(bb_max[j], p[j]);
00045                         }
00046                 }
00047 
00048                 rays_used++;
00049         }
00050                 
00051         return rays_used > 3;
00052 }
00053 
00054 /*void lda_get_bb2(LDP *ld, int nld, BB2 bb2, ld_reference use_reference, double horizon) {
00055         double bb_min[2], bb_max[2], offset[3] = {0,0,0};
00056         lda_get_bounding_box(ld,nld, bb2->bb_)
00057 }*/
00058 
00059         
00060 void lda_get_bounding_box(LDP *lda, int nld, double bb_min[2], double bb_max[2],
00061         double offset[3], ld_reference use_reference, double horizon) {
00062         
00063         int k;
00064         for(k=0;k<nld;k++) {
00065                 LDP ld = lda[k];
00066 
00067                 double *ref = ld_get_reference_pose(ld, use_reference);
00068                 if(!ref) {
00069                         sm_error("Pose %s not set in scan #%d.\n", 
00070                                 ld_reference_to_string(use_reference), k);
00071                         continue;
00072                 }
00073                 
00074                 double pose[3];
00075                 oplus_d(offset, ref, pose);
00076         
00077                 if(k==0) 
00078                         ld_get_bounding_box(ld, bb_min, bb_max, pose, horizon);
00079                 else {
00080                         double this_min[2], this_max[2];
00081                         ld_get_bounding_box(ld, this_min, this_max, pose, horizon);
00082                         int i; for(i=0;i<2;i++) {
00083                                 bb_min[i] = GSL_MIN(bb_min[i], this_min[i]);
00084                                 bb_max[i] = GSL_MAX(bb_max[i], this_max[i]);
00085                         }
00086                 }
00087         }
00088 }
00089 
00090 double * ld_get_reference_pose_silent(LDP ld, ld_reference use_reference) {
00091         double * pose;
00092         switch(use_reference) {
00093                 case Odometry: pose = ld->odometry; break;
00094                 case Estimate: pose = ld->estimate; break;
00095                 case True_pose: pose = ld->true_pose; break;
00096                 default: 
00097                         sm_error("Could not find pose identified by %d.\n", (int) use_reference);
00098                         return 0;
00099         }
00100         return pose;
00101 }
00102 
00103 double * ld_get_reference_pose(LDP ld, ld_reference use_reference) {
00104         double * pose = ld_get_reference_pose_silent(ld, use_reference);
00105         if(any_nan(pose, 3)) {
00106                 sm_error("Required field '%s' not set in laser scan.\n", 
00107                         ld_reference_to_string(use_reference) );
00108                 return 0;
00109         }
00110         return pose;
00111 }
00112 
00113 
00114 void compute_stroke_sequence(LDP ld, struct stroke_sequence*draw_info,
00115         double horizon, double connect_threshold) {
00116         int last_valid = -1; int first = 1;
00117         int i; for(i=0;i<ld->nrays;i++) {
00118                 if( (!ld_valid_ray(ld,i)) || (ld->readings[i] > horizon) ) {
00119                         draw_info[i].valid = 0;
00120                         continue;
00121                 }
00122                 draw_info[i].valid = 1;
00123                 draw_info[i].p[0] = ld->readings[i] * cos(ld->theta[i]);
00124                 draw_info[i].p[1] = ld->readings[i] * sin(ld->theta[i]);
00125                 
00126                 if(first) { 
00127                         first = 0; 
00128                         draw_info[i].begin_new_stroke = 1;
00129                         draw_info[i].end_stroke = 0;
00130                 } else {
00131                         int near =  square(connect_threshold) > 
00132                                 distance_squared_d(draw_info[last_valid].p, draw_info[i].p);
00133                         draw_info[i].begin_new_stroke = near ? 0 : 1;
00134                         draw_info[i].end_stroke = 0;
00135                         draw_info[last_valid].end_stroke = draw_info[i].begin_new_stroke;
00136                 }
00137                 last_valid = i;
00138         } /*for */
00139         if(last_valid >= 0)
00140                 draw_info[last_valid].end_stroke = 1;
00141 } /* find buff .. */
00142 
00143 
00144 


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