Go to the documentation of this file.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 <mtt/mtt_association.h>
00033
00034 unsigned int last_id=0;
00035
00036 void AssociateObjects(vector<t_listPtr> &list,vector<t_objectPtr> &objects,t_config& config,t_flag& flags)
00037 {
00038 for(uint i=0;i<objects.size();i++)
00039 objects[i]->object_found=false;
00040
00041 double min_ret=1;
00042 int min_index=-1;
00043 double ret=1;
00044 bool association_found;
00045 double remove_threshold;
00046
00048 for(uint i=0;i<list.size();i++)
00049 {
00050 min_ret=1;
00051 association_found=false;
00052
00053 for(uint h=0;h<objects.size();h++)
00054 {
00055 ret = CheckAssociationCriteria(*list[i],*objects[h]);
00056
00057 if(ret<min_ret && ret<0)
00058 {
00059 min_ret=ret;
00060 min_index=h;
00061 association_found=true;
00062 }
00063 }
00064
00065
00066 if(association_found)
00067 {
00068
00069
00070 double dist;
00071 double min_dist=1e6;
00072
00073 int index_e=-1;
00074 for(uint e=0;e<list.size();e++)
00075 {
00076 if(i==e)
00077 continue;
00078
00079 dist=point2point_distance(objects[min_index]->cx,objects[min_index]->cy,list[e]->position.predicted_x,list[e]->position.predicted_y);
00080
00081 if(dist<min_dist)
00082 {
00083 min_dist=dist;
00084 index_e=e;
00085 }
00086 }
00087
00088
00089
00090
00091 if(min_dist < config.ezB && list[index_e]->timers.lifetime > list[i]->timers.lifetime)
00092 {
00093
00094 association_found=false;
00095 }
00096 }
00097
00098
00099 if(association_found && objects[min_index]->object_found==false)
00100 {
00101
00102 if(list[i]->classification.partialy_occluded==false && objects[min_index]->partialy_occluded)
00103 {
00104
00105 objects[min_index]->cx=(objects[min_index]->cx+list[i]->position.predicted_x)/2;
00106 objects[min_index]->cy=(objects[min_index]->cy+list[i]->position.predicted_y)/2;
00107
00108 }
00109
00110 SingleObjectAssociation(*list[i],*objects[min_index]);
00111 objects[min_index]->object_found=true;
00112 list[i]->classification.occluded=false;
00113 list[i]->classification.partialy_occluded=objects[min_index]->partialy_occluded;
00114
00115
00116 }else
00117 {
00118
00119
00120 list[i]->classification.occluded=true;
00121 list[i]->timers.occludedtime++;
00122
00123
00124 list[i]->measurements.x=list[i]->position.predicted_x;
00125 list[i]->measurements.y=list[i]->position.predicted_y;
00126
00127 remove_threshold=list[i]->timers.lifetime;
00128
00129 if(remove_threshold>config.max_missing_iterations)
00130 remove_threshold=config.max_missing_iterations;
00131
00132 if(list[i]->timers.occludedtime>remove_threshold)
00133 RemoveFromList(list,list[i]->id);
00134 }
00135 }
00136
00138 double dist_to_object=1e6;
00139 for(uint g=0;g<objects.size();g++)
00140 {
00142 objects[g]->min_distance_to_existing_object=1e6;
00143
00144 for(uint i=0;i<list.size();i++)
00145 {
00146 dist_to_object=point2point_distance(objects[g]->cx,objects[g]->cy,list[i]->measurements.x,list[i]->measurements.y);
00147
00148 if(dist_to_object<objects[g]->min_distance_to_existing_object)
00149 objects[g]->min_distance_to_existing_object=dist_to_object;
00150 }
00151
00152
00153 if(objects[g]->min_distance_to_existing_object < config.ezA && flags.fi==false)
00154 continue;
00155
00156 if(objects[g]->object_found==false)
00157 AddObjectToList(list,*objects[g],config);
00158 }
00159
00160 return;
00161 }
00162
00163 void RemoveFromList(vector<t_listPtr> &list,unsigned int id)
00164 {
00165 vector<t_listPtr>::iterator it;
00166
00167 for(it=list.begin();it!=list.end();it++)
00168 {
00169 if((*it)->id==id)
00170 {
00172 cvReleaseKalman(&((*it)->motion_models.cv_x_kalman));
00173 cvReleaseKalman(&((*it)->motion_models.cv_y_kalman));
00174 cvReleaseKalman(&((*it)->motion_models.ca_x_kalman));
00175 cvReleaseKalman(&((*it)->motion_models.ca_y_kalman));
00176
00177 free((*it)->errors_cv.x_innovation);
00178 free((*it)->errors_cv.x_residue);
00179 free((*it)->errors_cv.y_innovation);
00180 free((*it)->errors_cv.y_residue);
00181 free((*it)->errors_cv.lateral_error);
00182
00183 free((*it)->errors_ca.x_innovation);
00184 free((*it)->errors_ca.x_residue);
00185 free((*it)->errors_ca.y_innovation);
00186 free((*it)->errors_ca.y_residue);
00187 free((*it)->errors_ca.lateral_error);
00188
00190
00192 free((*it)->path_cv.x);
00193 free((*it)->path_cv.y);
00194
00196 free((*it)->path_ca.x);
00197 free((*it)->path_ca.y);
00198
00199 list.erase(it);
00200 return;
00201 }
00202 }
00203 }
00204
00205 void AllocErrors(t_errors*error,t_config&config)
00206 {
00207 error->x_innovation=(double*)malloc(config.estimation_window_size*sizeof(double));
00208
00209 error->x_residue=(double*)malloc(config.estimation_window_size*sizeof(double));
00210
00211 error->y_innovation=(double*)malloc(config.estimation_window_size*sizeof(double));
00212
00213 error->y_residue=(double*)malloc(config.estimation_window_size*sizeof(double));
00214
00215 error->lateral_error=(double*)malloc(config.estimation_window_size*sizeof(double));
00216
00217 error->x_inno_cov=0;
00218 error->x_resi_cov=0;
00219 error->y_inno_cov=0;
00220 error->y_resi_cov=0;
00221 error->lateral_error_cov=0;
00222
00223 error->max_number_points=config.estimation_window_size;
00224 error->number_points=0;
00225 error->position=0;
00226 error->latest=0;
00227 error->next=1;
00228 }
00229
00230 void SingleObjectAssociation(t_list& list,t_object& object)
00231 {
00232 list.measurements.x=object.cx;
00233 list.measurements.y=object.cy;
00234 list.shape=object;
00235
00236 SetOjectMorphology(list,object);
00237 object.object_found=true;
00238 object.id=list.id;
00239
00240 list.timers.lifetime++;
00241
00242 list.timers.occludedtime-=1;
00243 if(list.timers.occludedtime<0)
00244 list.timers.occludedtime=0;
00245 }
00246
00247 void SetOjectMorphology(t_list&list,t_object& object)
00248 {
00249 list.object_morphology.apparent_size=object.size;
00250 }
00251
00252 void SetSearchArea(t_list& list,t_config&config)
00253 {
00254 list.search_area.angle=list.velocity.velocity_angle;
00255 list.search_area.center_x=list.position.predicted_x;
00256 list.search_area.center_y=list.position.predicted_y;
00257
00258
00259
00260
00261
00262 double size=list.object_morphology.apparent_size;
00263 double size_factor=0.1;
00264
00265 double default_size=config.min_ellipse_axis;
00266
00267 double xIl,yIl;
00268
00269 if(list.model==CV)
00270 {
00271 xIl=list.errors_cv.x_inno_cov;
00272 yIl=list.errors_cv.y_inno_cov;
00273 }else
00274 {
00275 xIl=list.errors_ca.x_inno_cov;
00276 yIl=list.errors_ca.y_inno_cov;
00277 }
00278
00279 int not_found_counter=list.timers.occludedtime;
00280 double not_found_factor=2;
00281
00282 double innovation_error=sqrt(sqrt(xIl*xIl+yIl*yIl));
00283 double lateral_error=sqrt(list.errors_cv.lateral_error_cov);
00284 double A_innovation_factor,B_innovation_factor;
00285 double lateral_factor;
00286
00287 if( list.velocity.velocity_module < 0.200 )
00288 {
00289 A_innovation_factor=5;
00290 B_innovation_factor=4;
00291 lateral_factor=1;
00292
00293 }else
00294 {
00295 A_innovation_factor=5;
00296 B_innovation_factor=0.5;
00297 lateral_factor=4;
00298 }
00299
00300 if(innovation_error<0.001)
00301 innovation_error=0.001;
00302
00303 list.search_area.ellipse_A = not_found_factor*pow(not_found_counter,2) + A_innovation_factor*innovation_error + default_size + size_factor*size;
00304 list.search_area.ellipse_B = not_found_factor*pow(not_found_counter,2) + B_innovation_factor*innovation_error + default_size + size_factor*size + lateral_error*lateral_factor;
00305
00306 if(list.search_area.ellipse_A > config.max_ellipse_axis)
00307 list.search_area.ellipse_A = config.max_ellipse_axis + default_size + size_factor*size;
00308
00309 if(list.search_area.ellipse_B > config.max_ellipse_axis)
00310 list.search_area.ellipse_B = config.max_ellipse_axis + default_size + size_factor*size;
00311
00312 if(list.timers.lifetime < 5)
00313 {
00314
00315
00316 }
00317 }
00318
00319 void AddPointPath(t_path*path,double x,double y)
00320 {
00321 path->x[path->position]=x;
00322 path->y[path->position]=y;
00323
00324 path->latest=path->position;
00325 path->position++;
00326
00327 if(path->position==path->max_number_points)
00328 path->position=0;
00329
00330 path->next=path->position;
00331
00332 if(path->number_points<path->max_number_points)
00333 path->number_points++;
00334 }
00335
00336 double CheckAssociationCriteria(t_list&list,t_object& object)
00337 {
00338 double ox=object.cx;
00339 double oy=object.cy;
00340
00341 double angle=-list.search_area.angle;
00342 double s=list.search_area.ellipse_B;
00343 double r=list.search_area.ellipse_A;
00344 double M=cos(angle);
00345 double N=sin(angle);
00346 double c=list.search_area.center_x;
00347 double d=list.search_area.center_y;
00348 double tx=ox-c;
00349 double ty=oy-d;
00350 double A=(M*tx-N*ty)*(M*tx-N*ty);
00351 double B=(N*tx+M*ty)*(N*tx+M*ty);
00352 double Z=s*s*A+r*r*B-r*r*s*s;
00353
00354 if(Z<0)
00355 return Z;
00356 else
00357 return 1;
00358 }
00359
00360 void AddObjectToList(vector<t_listPtr> &list,t_object& object,t_config&config)
00361 {
00362 t_listPtr element(new t_list);
00363
00364 AllocMotionModels(*element,config);
00365
00366 AllocPath(&(element->path_cv),config);
00367 AllocPath(&(element->path_ca),config);
00368
00369 AllocErrors(&(element->errors_cv),config);
00370 AllocErrors(&(element->errors_ca),config);
00371
00372 element->measurements.x=object.cx;
00373 element->measurements.y=object.cy;
00374 element->position.estimated_x=object.cx;
00375 element->position.estimated_y=object.cy;
00376
00377 InitialiseSearchArea(*element,config);
00378 InitialiseTimers(&(element->timers));
00379 InitialiseClassification(&(element->classification));
00380
00381 element->object_morphology.apparent_size=object.size;
00382
00383 element->model=CV;
00384 element->shape=object;
00385
00386 element->id=last_id;
00387
00388
00389
00390
00391 last_id++;
00392
00393 list.push_back(element);
00394
00395 return;
00396 }
00397
00398 void InitialiseClassification(t_classification*classification)
00399 {
00400 classification->velocity_classification=MOVING;
00401 classification->occluded=false;
00402 classification->partialy_occluded=false;
00403 }
00404
00405 void InitialiseTimers(t_timers*timer)
00406 {
00407 timer->occludedtime=0;
00408 timer->lifetime=0;
00409 }
00410
00411 void InitialiseSearchArea(t_list&list,t_config&config)
00412 {
00413 list.search_area.ellipse_A=config.max_ellipse_axis;
00414 list.search_area.ellipse_B=config.max_ellipse_axis;
00415 list.search_area.angle=0;
00416 list.search_area.center_x=list.position.estimated_x;
00417 list.search_area.center_y=list.position.estimated_y;
00418 }
00419
00420 void AllocPath(t_path*path,t_config&config)
00421 {
00422 path->x=(double*)malloc(config.path_lenght*sizeof(double));
00423
00424 path->y=(double*)malloc(config.path_lenght*sizeof(double));
00425
00426 path->max_number_points=config.path_lenght;
00427 path->number_points=0;
00428 path->position=0;
00429 path->latest=0;
00430 path->next=1;
00431
00432 return;
00433 }