00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00032 #include "types_declaration.h"
00033
00034 double get_m_phi(t_pose&i)
00035 {
00036 return i.m_phi;
00037 }
00038
00039 double get_m_vl(t_pose&i)
00040 {
00041 return i.m_vl;
00042 }
00043
00044 double get_phi(t_pose&i)
00045 {
00046 return i.phi;
00047 }
00048
00049 double get_orientation(t_pose&i)
00050 {
00051 return i.orientation;
00052 }
00053
00054 double get_vl(t_pose&i)
00055 {
00056 return i.vl;
00057 }
00058
00059 double max_orientation(const std::vector<t_posePtr>& vec)
00060 {
00061 if(vec.empty())return 0;
00062
00063 double max= vec[0]->orientation;
00064 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00065 if((*i)->orientation > max)
00066 max = (*i)->orientation;
00067
00068 return max;
00069 }
00070
00071 double min_orientation(const std::vector<t_posePtr>& vec)
00072 {
00073 if(vec.empty())return 0;
00074
00075 double min= vec[0]->orientation;
00076 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00077 if((*i)->orientation < min)
00078 min = (*i)->orientation;
00079
00080 return min;
00081 }
00082
00083 double max_x(const std::vector<t_posePtr>& vec)
00084 {
00085 if(vec.empty())return 0;
00086
00087 double max = vec[0]->x;
00088 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00089 if((*i)->x > max)
00090 max = (*i)->x;
00091
00092 return max;
00093 }
00094
00095 double min_x(const std::vector<t_posePtr>& vec)
00096 {
00097 if(vec.empty())return 0;
00098
00099 double min = vec[0]->x;
00100 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00101 if((*i)->x < min)
00102 min = (*i)->x;
00103
00104 return min;
00105 }
00106
00107 double max_y(const std::vector<t_posePtr>& vec)
00108 {
00109 if(vec.empty())return 0;
00110
00111 double max = vec[0]->y;
00112 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00113 if((*i)->y > max)
00114 max = (*i)->y;
00115
00116 return max;
00117 }
00118
00119 double min_y(const std::vector<t_posePtr>& vec)
00120 {
00121 if(vec.empty())return 0;
00122
00123 double min = vec[0]->y;
00124 for(std::vector<t_posePtr>::const_iterator i = vec.begin(); i != vec.end(); ++i)
00125 if((*i)->y < min)
00126 min = (*i)->y;
00127
00128 return min;
00129 }
00130
00131
00132 void RemoveOverlappingPoints(ray_config_t*config,ray_measurment_t*rays)
00133 {
00134 double *range_n;
00135 double *range_l;
00136 double diff;
00137
00138 for(int i=0;i<config->num_rays;i++)
00139 {
00140 if(rays[i].range->total<2)
00141 continue;
00142
00143 for(int e=1;e<rays[i].range->total;e++)
00144 {
00145 range_l=(double*)cvGetSeqElem(rays[i].range,e-1);
00146 range_n=(double*)cvGetSeqElem(rays[i].range,e);
00147
00148 diff=fabs(*range_l-*range_n);
00149
00150 if(diff<config->minimum_delta)
00151 {
00152 *range_n=(*range_l+*range_n)/2;
00153 cvSeqRemove(rays[i].range,e);
00154 e--;
00155 }
00156 }
00157 }
00158 }
00159
00160 void RemoveOverlappingPoints(ray_definition*src)
00161 {
00162 if(!src)
00163 {
00164 printf("Watch out for memory allocation, null pointer in %s\n",__FUNCTION__);
00165 return;
00166 }
00167
00168 RemoveOverlappingPoints(&src->cfg,src->dt);
00169 }
00170
00171 int Theta2Index(double theta,ray_config_t*config)
00172 {
00173 int index=cvRound(theta/config->angular_resolution);
00174
00175 if(index==config->num_rays)
00176 index=0;
00177
00178 return index;
00179 }
00180
00181 void Pointcloud2Rays(pcl::PointCloud<pcl::PointXYZ>& point_cloud,ray_config_t*config,ray_measurment_t*rays)
00182 {
00183 double theta,range,*rg;
00184 int ix;
00185 int e=0;
00186
00187 config->timestamp=point_cloud.header.stamp.toSec();
00188
00189
00190
00191 for(uint i=0;i<point_cloud.points.size();i++)
00192 {
00193 theta=atan2(point_cloud.points[i].y,point_cloud.points[i].x);
00194 if(theta<0)
00195 theta+=2*M_PI;
00196
00197 range=sqrt(pow(point_cloud.points[i].x,2)+pow(point_cloud.points[i].y,2));
00198
00199 if(range<1.5)
00200 continue;
00201
00202 ix=Theta2Index(theta,config);
00203
00204 if(ix<0 || ix>config->num_rays)
00205 {
00206 printf("Error!, index outside grid, maybe you didn't initialized the ray config\n");
00207 continue;
00208 }
00209
00210 for(e=0;e<rays[ix].range->total;e++)
00211 {
00212 rg=(double*)cvGetSeqElem(rays[ix].range,e);
00213
00214 if(range<*rg)
00215 break;
00216 }
00217
00218 cvSeqInsert(rays[ix].range,e,&range);
00219 }
00220 }
00221
00222 void PointCloud2Ray(pcl::PointCloud<pcl::PointXYZ>& point_cloud,ray_definition*dst)
00223 {
00224 if(!dst)
00225 {
00226 printf("Wrong pointer, be carefull with memory allocation!\n");
00227 return;
00228 }
00229
00230
00231 dst->cfg.timestamp=ros::Time::now().toSec();
00232
00233 Pointcloud2Rays(point_cloud,&dst->cfg,dst->dt);
00234 }
00235
00236 void ClearRays(ray_measurment_t*rays,ray_config_t*ray_config)
00237 {
00238 for(int i=0;i<ray_config->num_rays;i++)
00239 cvClearSeq(rays[i].range);
00240 }
00241
00242 void ClearRays(ray_definition*src)
00243 {
00244 if(!src)
00245 {
00246 printf("Watch out for memory allocation, null pointer in %s\n",__FUNCTION__);
00247 return;
00248 }
00249
00250 ClearRays(src->dt,&src->cfg);
00251 }
00252
00253 void InitRayDefinition(ray_definition*src)
00254 {
00255 src->cfg.angular_resolution=deg2rad(0.5);
00256 src->cfg.minimum_delta=0.30;
00257 src->cfg.max_points_per_ray=-1;
00258 src->cfg.num_rays=2*M_PI/src->cfg.angular_resolution;
00259 src->cfg.timestamp=0;
00260 src->cfg.x=0;
00261 src->cfg.y=0;
00262 src->cfg.t=0;
00263 src->dt=(ray_measurment_t*)malloc(src->cfg.num_rays*sizeof(ray_measurment_t));
00264
00265
00266 for(int i=0;i<src->cfg.num_rays;i++)
00267 {
00268 src->dt[i].theta=i*src->cfg.angular_resolution;
00269 src->dt[i].range_storage=cvCreateMemStorage(0);
00270 src->dt[i].range=cvCreateSeq(0,sizeof(CvSeq),sizeof(double),src->dt[i].range_storage);
00271 }
00272 }
00273
00274 void CopyRays(ray_definition*src,ray_definition*dst)
00275 {
00276 if(!src || !dst)
00277 {
00278 printf("Watch out for memory allocation, null pointer in %s\n",__FUNCTION__);
00279 return;
00280 }
00281
00282 memcpy(&dst->cfg,&src->cfg,sizeof(ray_config_t));
00283
00284 CopyRays(&src->cfg,src->dt,dst->dt);
00285 }
00286
00287 void CopyRays(ray_config_t*cfg,ray_measurment_t*src,ray_measurment_t*dst)
00288 {
00289
00290 ClearRays(dst,cfg);
00291 double*range;
00292
00293 for(int i=0;i<cfg->num_rays;i++)
00294 {
00295 dst[i].theta=src[i].theta;
00296
00297 for(int e=0;e<src[i].range->total;e++)
00298 {
00299 range=(double*)cvGetSeqElem(src[i].range,e);
00300 cvSeqInsert(dst[i].range,e,range);
00301 }
00302 }
00303 }
00304
00305 void InitRayHistory(ray_history*h_rays)
00306 {
00307 h_rays->data_storage=cvCreateMemStorage(0);;
00308 h_rays->data=cvCreateSeq(0,sizeof(CvSeq),sizeof(ray_definition),h_rays->data_storage);
00309 }
00310
00311 void AddToHistory(ray_definition*src,ray_history*h_rays)
00312 {
00313 ray_definition rays;
00314
00315 InitRayDefinition(&rays);
00316
00317 CopyRays(src,&rays);
00318
00319 cvSeqPushFront(h_rays->data,&rays);
00320
00321 if(h_rays->data->total>50)
00322 {
00323 ray_definition*rmv=(ray_definition*)cvGetSeqElem(h_rays->data,h_rays->data->total-1);
00324
00325 for(int i=0;i<rmv->cfg.num_rays;i++)
00326 cvReleaseMemStorage(&(rmv->dt[i].range_storage));
00327
00328 cvSeqPop(h_rays->data);
00329 }
00330 }
00331
00332 double get_fps(double dt,t_fps*acc)
00333 {
00334 static bool initialise=TRUE;
00335 static unsigned int max_size=30;
00336
00337 if(initialise)
00338 {
00339 memset(acc->fps,0,max_size*sizeof(double));
00340 acc->position=0;
00341 acc->current_size=0;
00342
00343 initialise=FALSE;
00344 }
00345
00346 acc->fps[acc->position]=1./(double)dt;
00347 acc->position++;
00348
00349 if(acc->current_size<max_size)
00350 acc->current_size++;
00351
00352 if(acc->position==max_size)
00353 acc->position=0;
00354
00355 double mean_fps=0;
00356
00357 for(unsigned int i=0;i<acc->current_size;i++)
00358 mean_fps+=acc->fps[i];
00359
00360 mean_fps/=acc->current_size;
00361
00362 return mean_fps;
00363 }