mtt: /home/laradmin/lar/perception/planarobstacles/mtt/src/mtt

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00032 #include <mtt/mtt_clustering.h>
00033 
00034 void FlagCollisionWithOcclusion(t_cluster**clusters,int object_size,t_data*data,t_config*/*config*/)
00035 {
00036         t_point start_cur,end_cur;
00037         t_point end_prev,start_next;
00038         
00039         double angular_discrepance=1;
00040         double range_discrepance=500;
00041         
00042         
00043         for(int k=1;k<object_size-1;k++)
00044         {
00045                 start_cur.r=data->r[clusters[k]->stp];
00046                 start_cur.t=data->t[clusters[k]->stp];
00047                 
00048                 end_cur.r=data->r[clusters[k]->enp];
00049                 end_cur.t=data->t[clusters[k]->enp];
00050                 
00051                 end_prev.r=data->r[clusters[k-1]->enp];
00052                 end_prev.t=data->t[clusters[k-1]->enp];
00053                 
00054                 start_next.r=data->r[clusters[k+1]->stp];
00055                 start_next.t=data->t[clusters[k+1]->stp];
00056                 
00057                 
00058                 if(fabs(start_cur.t-end_prev.t)<angular_discrepance*M_PI/180.)
00059                         if(start_cur.r-end_prev.r > range_discrepance)
00060                                 clusters[k]->partialy_occluded=true;
00061                         
00062                         if(fabs(end_cur.t-start_next.t)<angular_discrepance*M_PI/180.)
00063                                 if(end_cur.r-start_next.r > range_discrepance)
00064                                         clusters[k]->partialy_occluded=true;
00065         }
00066 }
00067 
00068 double ClusteringThreshold(double r1,double t1,double r2,double t2,t_config*config)
00069 {
00070         double min_dist;
00071         double Ax,Ay,Bx,By;
00072         
00073         if(r1<r2)
00074         {
00075                 Ax=r1*cos(t1);
00076                 Ay=r1*sin(t1);
00077                 
00078                 Bx=r1*cos(t1+deg2rad(0.5));
00079                 By=r1*sin(t1+deg2rad(0.5));
00080         }else
00081         {
00082                 Ax=r2*cos(t2+deg2rad(0.5));
00083                 Ay=r2*sin(t2+deg2rad(0.5));
00084                 
00085                 Bx=r2*cos(t2);
00086                 By=r2*sin(t2);
00087         }
00088         
00089         min_dist=sqrt( pow(Ax-Bx,2) + pow(Ay-By,2) );
00090         
00091         //      printf("Cos beta %f\n",cos(config->beta));
00092         //      printf("MD %f\n",min_dist);
00093         
00094         return config->C0 + min_dist/cos(config->beta);
00095 }
00096 
00097 double dietmayer_threshold(double r,t_config*config)
00098 {
00099         //return config->C0+r*((tan(config->beta)*sqrt(2*(1.-cos(config->fi))))/(cos(config->fi/2)-sin(config->fi/2)));         //compute threshold
00100         printf("C0 %f\n",config->C0);
00101         printf("sqrt(2*(1+cos(config->fi))) %f\n",sqrt(2*(1+cos(config->fi))));
00102         printf("Fi %f\n",config->fi);
00103         printf("R %f\n",r);
00104         double min_dist=tan(0.5*M_PI/180.)*r;
00105         printf("Min dist %f\n",min_dist);
00106         printf("CALCILC THIS\n");
00107         
00108         //      return min_dist + config->C0 + 
00109         
00110         return config->C0+r*sqrt(2*(1+cos(config->fi)));
00111 }
00112 
00113 t_cluster**clustering(t_data*data,int*count,t_config*config,t_flag*flags)
00114 {
00115         int i,cluster_count=0;
00116         double x,y,xold=0,yold=0;
00117         double dist,threshold;
00118         
00119         static bool initialise=true;
00120         static t_cluster**clusters;
00121         
00122         if(initialise)
00123         {
00124                 clusters=(t_cluster**)malloc(_MAX_CLUSTERS_*sizeof(t_cluster*));        //malloc dos ponteiros
00125                 memset(clusters,0,_MAX_CLUSTERS_*sizeof(t_cluster*));
00126                 for(i=0;i<_MAX_CLUSTERS_;i++)
00127                 {
00128                         clusters[i]=(t_cluster*)malloc(sizeof(t_cluster));
00129                 }
00130                 initialise=false;
00131         }
00132         
00133         for(i=0;i<data->n_points;i++)
00134         {
00135                 x=data->x[i];
00136                 y=data->y[i];
00137                 
00138                 if(i>0)
00139                 {
00140                         dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold));               //compute distance
00141                         
00142                         //                      threshold=dietmayer_threshold(rmin,config);
00143                         threshold=ClusteringThreshold(data->r[i-1],data->t[i-1],data->r[i],data->t[i],config);
00144                         //                      printf("Threshold %f\n",threshold);
00145                         //                      printf("Dist %f\n",dist);
00146                         //                      printf("Rmin %f\n",rmin);
00147                         if(dist>threshold)
00148                         {
00149                                 clusters[cluster_count]->enp=i-1;               //set the last cluster endpoint
00150                                 clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;    //sets the number of points in the cluster
00151                                 clusters[cluster_count]->partialy_occluded=false;
00152                                 cluster_count++;
00153                                 clusters[cluster_count]->stp=i;                                                                                 //sets the new cluster start and end point
00154                                 clusters[cluster_count]->lenght=0;
00155                         }else
00156                         {
00157                                 clusters[cluster_count]->lenght+=dist;
00158                                 if(clusters[cluster_count]->lenght>config->cluster_break_distance)
00159                                 {
00160                                         clusters[cluster_count]->enp=i-1;               //set the last cluster endpoint
00161                                         clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;    //sets the number of points in the cluster
00162                                         clusters[cluster_count]->partialy_occluded=false;
00163                                         cluster_count++;
00164                                         clusters[cluster_count]->stp=i;                                                                                 //sets the new cluster start and end point
00165                                         clusters[cluster_count]->lenght=0;
00166                                 }
00167                         }
00168                         
00169                         if(i==(data->n_points-1))//last point
00170                         {
00171                                 //in case all points are in the same cluster
00172                                 clusters[cluster_count]->enp=i;
00173                                 clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;    //sets the number of points in the cluster
00174                         }
00175                         
00176                 }else//first point
00177                 {
00178                         flags->fp_s=0;                  //negates the first point of scan flag
00179                         clusters[cluster_count]->stp=0;
00180                         clusters[cluster_count]->enp=0;
00181                         clusters[cluster_count]->lenght=0;
00182                 }
00183                 
00184                 xold=x;
00185                 yold=y;
00186         }
00187         
00188         if(!data->n_points)
00189                 *count=0;
00190         else
00191                 *count=cluster_count+1;
00192         
00193         return clusters;
00194 }
00195 
00196 bool clustering(t_data& data,vector<t_clustersPtr> &clustersPtr,t_config*config,t_flag*flags)
00197 {
00198         int i;
00199         double x,y,xold=0,yold=0;
00200         double dist,threshold;
00201         
00202         t_clustersPtr cluster(new t_cluster);
00203         
00204         clustersPtr.clear();
00205         
00206         cluster->id=clustersPtr.size();
00207         
00208         for(i=0;i<data.n_points;i++)
00209         {
00210                 x=data.x[i];
00211                 y=data.y[i];
00212                 
00213                 if(i>0)
00214                 {
00215                         dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold));               //compute distance
00216                         threshold=ClusteringThreshold(data.r[i-1],data.t[i-1],data.r[i],data.t[i],config);
00217                         
00218                         if(dist>threshold)
00219                         {
00220                                 cluster->enp=i-1;               //set the last cluster endpoint
00221                                 cluster->n_points=cluster->enp-cluster->stp;    //sets the number of points in the cluster
00222                                 cluster->partialy_occluded=false;
00223                                 clustersPtr.push_back(cluster);
00224                                 
00225                                 cluster.reset(new t_cluster);
00226                                 
00227                                 cluster->id=clustersPtr.size();
00228                                 cluster->stp=i;                                                                                 //sets the new cluster start and end point
00229                                 cluster->lenght=0;
00230                         }else
00231                         {
00232                                 cluster->lenght+=dist;
00233                                 if(cluster->lenght>config->cluster_break_distance)
00234                                 {
00235                                         cluster->enp=i-1;               //set the last cluster endpoint
00236                                         cluster->n_points=cluster->enp-cluster->stp;    //sets the number of points in the cluster
00237                                         cluster->partialy_occluded=false;
00238                                         clustersPtr.push_back(cluster);
00239                                         
00240                                         cluster.reset(new t_cluster);
00241                                         
00242                                         cluster->id=clustersPtr.size();
00243                                         cluster->stp=i;                                                                                 //sets the new cluster start and end point
00244                                         cluster->lenght=0;
00245                                 }
00246                         }
00247                         
00248                         if(i==(data.n_points-1))//last point
00249                         {
00250                                 //in case all points are in the same cluster
00251                                 cluster->enp=i;
00252                                 cluster->n_points=cluster->enp-cluster->stp;    //sets the number of points in the cluster
00253                         }
00254                         
00255                 }else//first point
00256                 {
00257                         flags->fp_s=0;                  //negates the first point of scan flag
00258                         cluster->stp=0;
00259                         cluster->enp=0;
00260                         cluster->lenght=0;
00261                 }
00262                 
00263                 xold=x;
00264                 yold=y;
00265         }
00266         
00267         return true;
00268 }
00269 
00270 void remove_small_clusters(t_cluster**clusters,int*size,int threshold)
00271 {
00272         int i,e;
00273         
00274         for(i=0;i<*size;i++)
00275         {
00276                 if(clusters[i]->n_points<threshold)
00277                 {       
00278                         for(e=i;e<*size-1;e++)
00279                         {
00280                                 clusters[e]->n_points=clusters[e+1]->n_points;
00281                                 clusters[e]->stp=clusters[e+1]->stp;
00282                                 clusters[e]->enp=clusters[e+1]->enp;
00283                         }
00284                         memset(clusters[*size],0,sizeof(t_cluster));
00285                         (*size)--;
00286                         i--;
00287                 }
00288         }
00289 }
00290 
00291 void remove_border_points(t_cluster**clusters,int size,int npoints)
00292 {
00293         int i;
00294         
00295         for(i=0;i<size;i++)
00296         {
00297                 if(clusters[i]->n_points>npoints*2)
00298                 {
00299                         clusters[i]->stp=clusters[i]->stp+npoints;
00300                         clusters[i]->enp=clusters[i]->enp-npoints;
00301                 }
00302                 
00303                 clusters[i]->n_points=clusters[i]->n_points-npoints*2;
00304         }
00305 }
00306 
00307 void calc_cluster_props(t_cluster**clusters,int size,t_data*data,t_config*/*config*/)
00308 {
00309         double rmin;
00310         int i,e;
00311         
00312         for(i=0;i<size;i++)
00313         {
00314                 rmin=1e12;
00315                 clusters[i]->lenght=0;
00316                 for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
00317                 {
00318                         if(e<clusters[i]->enp-1)
00319                                 clusters[i]->lenght+=point2point_distance(data->x[e],data->y[e],data->x[e+1],data->y[e+1]);
00320                         
00321                         if(data->r[e]<rmin)
00322                                 rmin=data->r[e];
00323                 }
00324                 
00325                 
00326                 clusters[i]->rmin=rmin;
00327                 clusters[i]->tm=(data->t[clusters[i]->stp]+data->t[clusters[i]->enp])/2;
00328         }
00329 }
00330 
00331 void calc_cluster_props(vector<t_clustersPtr> &clusters,t_data&data)
00332 {
00333         double rmin;
00334         int e;
00335         
00336         for(uint i=0;i<clusters.size();i++)
00337         {
00338                 rmin=1e12;
00339                 clusters[i]->lenght=0;
00340                 for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
00341                 {
00342                         if(e<clusters[i]->enp-1)
00343                                 clusters[i]->lenght+=point2point_distance(data.x[e],data.y[e],data.x[e+1],data.y[e+1]);
00344                         
00345                         if(data.r[e]<rmin)
00346                                 rmin=data.r[e];
00347                 }
00348                 
00349                 
00350                 clusters[i]->rmin=rmin;
00351                 clusters[i]->tm=(data.t[clusters[i]->stp]+data.t[clusters[i]->enp])/2;
00352         }
00353 }
00354 
00355 bool clusters2objects(vector<t_objectPtr> &objectsPtr,vector<t_clustersPtr> &clusters,t_data& data,t_config& config)
00356 {
00357         t_objectPtr object(new t_object);
00358         
00359         objectsPtr.clear();
00360 
00361         for(uint i=0;i<clusters.size();i++)
00362         {
00363                 object->rmin=clusters[i]->rmin;
00364                 object->tm=clusters[i]->tm;
00365                 object->object_found=false;
00366                 
00367                 object->partialy_occluded=clusters[i]->partialy_occluded;
00368                 
00369                 recursive_line_fitting(object,*clusters[i],data,config);
00370                 
00371                 objectsPtr.push_back(object);
00372                 object.reset(new t_object);
00373         }
00374         
00375         return true;
00376 }
00377 
00378 void calc_object_props(vector<t_objectPtr> &objects)
00379 {
00380         uint e;
00381         double r,t;
00382         double xi,yi,xf,yf;
00383         double rmin;
00384         
00385         for(uint i=0;i<objects.size();i++)
00386         {
00387                 t=objects[i]->tm;
00388                 
00389                 rmin=1e12;
00390                 
00391                 for(e=0;e<objects[i]->lines.size();e++)
00392                 {
00393                         r=sqrt(pow(objects[i]->lines[e]->xi,2)+pow(objects[i]->lines[e]->yi,2));
00394                         
00395                         if(r<rmin)
00396                                 rmin=r;
00397                 }
00398                 
00399                 r=sqrt(pow(objects[i]->lines[objects[i]->lines.size()-1]->xf,2)+pow(objects[i]->lines[objects[i]->lines.size()-1]->yf,2));
00400                 if(r<rmin)
00401                         rmin=r;
00402                 
00403                 r=rmin;
00404                 
00405                 objects[i]->cx=r*cos(t);
00406                 objects[i]->cy=r*sin(t);
00407                 
00408                 xi=objects[i]->lines[0]->xi;
00409                 yi=objects[i]->lines[0]->yi;
00410                 
00411                 xf=objects[i]->lines[objects[i]->lines.size()-1]->xf;
00412                 yf=objects[i]->lines[objects[i]->lines.size()-1]->yf;
00413                 
00414                 objects[i]->size=point2point_distance(xi,yi,xf,yf);
00415         }
00416 }
00417 
00418 void clean_objets(vector<t_objectPtr> &objects)
00419 {
00420         free_lines(objects);
00421 }
00422 
00423 void recursive_IEPF(t_objectPtr& object,t_data& data,int start,int end,t_config& config)
00424 {
00427         int i,index=0;
00428         double mean_variance,max_variance,current_variance;
00429         
00430         t_linePtr line(new t_line);
00431         
00432         line->alpha=atan2(data.x[start]-data.x[end],data.y[end]-data.y[start])+M_PI;
00433         line->ro=data.x[start]*cos(line->alpha)+data.y[start]*sin(line->alpha);
00434         line->xi=data.x[start];
00435         line->yi=data.y[start];
00436         line->xf=data.x[end];
00437         line->yf=data.y[end];
00438         
00439         mean_variance=0;
00440         max_variance=0;
00441         for(i=start;i<end;i++)
00442         {
00443                 current_variance=pow(point2line_distance(line->alpha,line->ro,data.x[i],data.y[i]),2);
00444                 mean_variance+=current_variance;
00445                 
00446                 if(current_variance>max_variance)
00447                 {
00448                         max_variance=current_variance;
00449                         index=i;
00450                 }               
00451         }
00452         
00453         mean_variance/=end-start;
00454         mean_variance=sqrt(mean_variance);
00455         
00456 //      if(object->lines.size()>20)
00457 //              goto F2;
00458         
00459         if(mean_variance>config.max_mean_variance)
00460         {
00461                 recursive_IEPF(object,data,start,index,config);
00462                 recursive_IEPF(object,data,index,end,config);
00463                 return; 
00464         }
00465         
00466 //      F2:
00467         
00468         object->lines.push_back(line);
00469 //      object->line[object->lines.size()]=(t_line*)malloc(sizeof(t_line));
00470 //      memcpy(object->line[object->lines.size()],&line,sizeof(t_line));
00471 //      object->n_lines++;
00472         
00473         return;
00474 }
00475 
00476 void free_lines(vector<t_objectPtr> &objects)
00477 {       
00478         for(uint i=0;i<objects.size();i++)
00479                 objects[i]->lines.clear();
00480 }
00481 
00482 void recursive_line_fitting(t_objectPtr& object,t_cluster& cluster,t_data& data,t_config& config)
00483 {       
00484         if(!data.n_points)
00485                 return;
00486         
00487         recursive_IEPF(object,data,cluster.stp,cluster.enp,config);
00488 }
/home/laradmin/lar/perception/planarobstacles/mtt/src/mtt_clustering.cpp
