mtt_clustering.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
32 #include <mtt/mtt_clustering.h>
33 
34 void FlagCollisionWithOcclusion(t_cluster**clusters,int object_size,t_data*data,t_config*/*config*/)
35 {
36  t_point start_cur,end_cur;
37  t_point end_prev,start_next;
38 
39  double angular_discrepance=1;
40  double range_discrepance=500;
41 
42 
43  for(int k=1;k<object_size-1;k++)
44  {
45  start_cur.r=data->r[clusters[k]->stp];
46  start_cur.t=data->t[clusters[k]->stp];
47 
48  end_cur.r=data->r[clusters[k]->enp];
49  end_cur.t=data->t[clusters[k]->enp];
50 
51  end_prev.r=data->r[clusters[k-1]->enp];
52  end_prev.t=data->t[clusters[k-1]->enp];
53 
54  start_next.r=data->r[clusters[k+1]->stp];
55  start_next.t=data->t[clusters[k+1]->stp];
56 
57 
58  if(fabs(start_cur.t-end_prev.t)<angular_discrepance*M_PI/180.)
59  if(start_cur.r-end_prev.r > range_discrepance)
60  clusters[k]->partialy_occluded=true;
61 
62  if(fabs(end_cur.t-start_next.t)<angular_discrepance*M_PI/180.)
63  if(end_cur.r-start_next.r > range_discrepance)
64  clusters[k]->partialy_occluded=true;
65  }
66 }
67 
68 double ClusteringThreshold(double r1,double t1,double r2,double t2,t_config*config)
69 {
70  double min_dist;
71  double Ax,Ay,Bx,By;
72 
73  if(r1<r2)
74  {
75  Ax=r1*cos(t1);
76  Ay=r1*sin(t1);
77 
78  Bx=r1*cos(t1+deg2rad(0.5));
79  By=r1*sin(t1+deg2rad(0.5));
80  }else
81  {
82  Ax=r2*cos(t2+deg2rad(0.5));
83  Ay=r2*sin(t2+deg2rad(0.5));
84 
85  Bx=r2*cos(t2);
86  By=r2*sin(t2);
87  }
88 
89  min_dist=sqrt( pow(Ax-Bx,2) + pow(Ay-By,2) );
90 
91  // printf("Cos beta %f\n",cos(config->beta));
92  // printf("MD %f\n",min_dist);
93 
94  return config->C0 + min_dist/cos(config->beta);
95 }
96 
97 double dietmayer_threshold(double r,t_config*config)
98 {
99  //return config->C0+r*((tan(config->beta)*sqrt(2*(1.-cos(config->fi))))/(cos(config->fi/2)-sin(config->fi/2))); //compute threshold
100  printf("C0 %f\n",config->C0);
101  printf("sqrt(2*(1+cos(config->fi))) %f\n",sqrt(2*(1+cos(config->fi))));
102  printf("Fi %f\n",config->fi);
103  printf("R %f\n",r);
104  double min_dist=tan(0.5*M_PI/180.)*r;
105  printf("Min dist %f\n",min_dist);
106  printf("CALCILC THIS\n");
107 
108  // return min_dist + config->C0 +
109 
110  return config->C0+r*sqrt(2*(1+cos(config->fi)));
111 }
112 
113 t_cluster**clustering(t_data*data,int*count,t_config*config,t_flag*flags)
114 {
115  int i,cluster_count=0;
116  double x,y,xold=0,yold=0;
117  double dist,threshold;
118 
119  static bool initialise=true;
120  static t_cluster**clusters;
121 
122  if(initialise)
123  {
124  clusters=(t_cluster**)malloc(_MAX_CLUSTERS_*sizeof(t_cluster*)); //malloc dos ponteiros
125  memset(clusters,0,_MAX_CLUSTERS_*sizeof(t_cluster*));
126  for(i=0;i<_MAX_CLUSTERS_;i++)
127  {
128  clusters[i]=(t_cluster*)malloc(sizeof(t_cluster));
129  }
130  initialise=false;
131  }
132 
133  for(i=0;i<data->n_points;i++)
134  {
135  x=data->x[i];
136  y=data->y[i];
137 
138  if(i>0)
139  {
140  dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold)); //compute distance
141 
142  // threshold=dietmayer_threshold(rmin,config);
143  threshold=ClusteringThreshold(data->r[i-1],data->t[i-1],data->r[i],data->t[i],config);
144  // printf("Threshold %f\n",threshold);
145  // printf("Dist %f\n",dist);
146  // printf("Rmin %f\n",rmin);
147  if(dist>threshold)
148  {
149  clusters[cluster_count]->enp=i-1; //set the last cluster endpoint
150  clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp; //sets the number of points in the cluster
151  clusters[cluster_count]->partialy_occluded=false;
152  cluster_count++;
153  clusters[cluster_count]->stp=i; //sets the new cluster start and end point
154  clusters[cluster_count]->lenght=0;
155  }else
156  {
157  clusters[cluster_count]->lenght+=dist;
158  if(clusters[cluster_count]->lenght>config->cluster_break_distance)
159  {
160  clusters[cluster_count]->enp=i-1; //set the last cluster endpoint
161  clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp; //sets the number of points in the cluster
162  clusters[cluster_count]->partialy_occluded=false;
163  cluster_count++;
164  clusters[cluster_count]->stp=i; //sets the new cluster start and end point
165  clusters[cluster_count]->lenght=0;
166  }
167  }
168 
169  if(i==(data->n_points-1))//last point
170  {
171  //in case all points are in the same cluster
172  clusters[cluster_count]->enp=i;
173  clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp; //sets the number of points in the cluster
174  }
175 
176  }else//first point
177  {
178  flags->fp_s=0; //negates the first point of scan flag
179  clusters[cluster_count]->stp=0;
180  clusters[cluster_count]->enp=0;
181  clusters[cluster_count]->lenght=0;
182  }
183 
184  xold=x;
185  yold=y;
186  }
187 
188  if(!data->n_points)
189  *count=0;
190  else
191  *count=cluster_count+1;
192 
193  return clusters;
194 }
195 
196 bool clustering(t_data& data,vector<t_clustersPtr> &clustersPtr,t_config*config,t_flag*flags)
197 {
198  int i;
199  double x,y,xold=0,yold=0;
200  double dist,threshold;
201 
202  t_clustersPtr cluster(new t_cluster);
203 
204  clustersPtr.clear();
205 
206  cluster->id=clustersPtr.size();
207 
208  for(i=0;i<data.n_points;i++)
209  {
210  x=data.x[i];
211  y=data.y[i];
212 
213  if(i>0)
214  {
215  dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold)); //compute distance
216  threshold=ClusteringThreshold(data.r[i-1],data.t[i-1],data.r[i],data.t[i],config);
217 
218  if(dist>threshold)
219  {
220  cluster->enp=i-1; //set the last cluster endpoint
221  cluster->n_points=cluster->enp-cluster->stp; //sets the number of points in the cluster
222  cluster->partialy_occluded=false;
223  clustersPtr.push_back(cluster);
224 
225  cluster.reset(new t_cluster);
226 
227  cluster->id=clustersPtr.size();
228  cluster->stp=i; //sets the new cluster start and end point
229  cluster->lenght=0;
230  }else
231  {
232  cluster->lenght+=dist;
233  if(cluster->lenght>config->cluster_break_distance)
234  {
235  cluster->enp=i-1; //set the last cluster endpoint
236  cluster->n_points=cluster->enp-cluster->stp; //sets the number of points in the cluster
237  cluster->partialy_occluded=false;
238  clustersPtr.push_back(cluster);
239 
240  cluster.reset(new t_cluster);
241 
242  cluster->id=clustersPtr.size();
243  cluster->stp=i; //sets the new cluster start and end point
244  cluster->lenght=0;
245  }
246  }
247 
248  if(i==(data.n_points-1))//last point
249  {
250  //in case all points are in the same cluster
251  cluster->enp=i;
252  cluster->n_points=cluster->enp-cluster->stp; //sets the number of points in the cluster
253  }
254 
255  }else//first point
256  {
257  flags->fp_s=0; //negates the first point of scan flag
258  cluster->stp=0;
259  cluster->enp=0;
260  cluster->lenght=0;
261  }
262 
263  xold=x;
264  yold=y;
265  }
266 
267  return true;
268 }
269 
270 void remove_small_clusters(t_cluster**clusters,int*size,int threshold)
271 {
272  int i,e;
273 
274  for(i=0;i<*size;i++)
275  {
276  if(clusters[i]->n_points<threshold)
277  {
278  for(e=i;e<*size-1;e++)
279  {
280  clusters[e]->n_points=clusters[e+1]->n_points;
281  clusters[e]->stp=clusters[e+1]->stp;
282  clusters[e]->enp=clusters[e+1]->enp;
283  }
284  memset(clusters[*size],0,sizeof(t_cluster));
285  (*size)--;
286  i--;
287  }
288  }
289 }
290 
291 void remove_border_points(t_cluster**clusters,int size,int npoints)
292 {
293  int i;
294 
295  for(i=0;i<size;i++)
296  {
297  if(clusters[i]->n_points>npoints*2)
298  {
299  clusters[i]->stp=clusters[i]->stp+npoints;
300  clusters[i]->enp=clusters[i]->enp-npoints;
301  }
302 
303  clusters[i]->n_points=clusters[i]->n_points-npoints*2;
304  }
305 }
306 
307 void calc_cluster_props(t_cluster**clusters,int size,t_data*data,t_config*/*config*/)
308 {
309  double rmin;
310  int i,e;
311 
312  for(i=0;i<size;i++)
313  {
314  rmin=1e12;
315  clusters[i]->lenght=0;
316  for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
317  {
318  if(e<clusters[i]->enp-1)
319  clusters[i]->lenght+=point2point_distance(data->x[e],data->y[e],data->x[e+1],data->y[e+1]);
320 
321  if(data->r[e]<rmin)
322  rmin=data->r[e];
323  }
324 
325 
326  clusters[i]->rmin=rmin;
327  clusters[i]->tm=(data->t[clusters[i]->stp]+data->t[clusters[i]->enp])/2;
328  }
329 }
330 
331 void calc_cluster_props(vector<t_clustersPtr> &clusters,t_data&data)
332 {
333  double rmin;
334  int e;
335 
336  for(uint i=0;i<clusters.size();i++)
337  {
338  rmin=1e12;
339  clusters[i]->lenght=0;
340  for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
341  {
342  if(e<clusters[i]->enp-1)
343  clusters[i]->lenght+=point2point_distance(data.x[e],data.y[e],data.x[e+1],data.y[e+1]);
344 
345  if(data.r[e]<rmin)
346  rmin=data.r[e];
347  }
348 
349 
350  clusters[i]->rmin=rmin;
351  clusters[i]->tm=(data.t[clusters[i]->stp]+data.t[clusters[i]->enp])/2;
352  }
353 }
354 
355 bool clusters2objects(vector<t_objectPtr> &objectsPtr,vector<t_clustersPtr> &clusters,t_data& data,t_config& config)
356 {
357  t_objectPtr object(new t_object);
358 
359  objectsPtr.clear();
360 
361  for(uint i=0;i<clusters.size();i++)
362  {
363  object->rmin=clusters[i]->rmin;
364  object->tm=clusters[i]->tm;
365  object->object_found=false;
366 
367  object->partialy_occluded=clusters[i]->partialy_occluded;
368 
369  recursive_line_fitting(object,*clusters[i],data,config);
370 
371  objectsPtr.push_back(object);
372  object.reset(new t_object);
373  }
374 
375  return true;
376 }
377 
378 void calc_object_props(vector<t_objectPtr> &objects)
379 {
380  uint e;
381  double r,t;
382  double xi,yi,xf,yf;
383  double rmin;
384 
385  for(uint i=0;i<objects.size();i++)
386  {
387  t=objects[i]->tm;
388 
389  rmin=1e12;
390 
391  for(e=0;e<objects[i]->lines.size();e++)
392  {
393  r=sqrt(pow(objects[i]->lines[e]->xi,2)+pow(objects[i]->lines[e]->yi,2));
394 
395  if(r<rmin)
396  rmin=r;
397  }
398 
399  r=sqrt(pow(objects[i]->lines[objects[i]->lines.size()-1]->xf,2)+pow(objects[i]->lines[objects[i]->lines.size()-1]->yf,2));
400  if(r<rmin)
401  rmin=r;
402 
403  r=rmin;
404 
405  objects[i]->cx=r*cos(t);
406  objects[i]->cy=r*sin(t);
407 
408  xi=objects[i]->lines[0]->xi;
409  yi=objects[i]->lines[0]->yi;
410 
411  xf=objects[i]->lines[objects[i]->lines.size()-1]->xf;
412  yf=objects[i]->lines[objects[i]->lines.size()-1]->yf;
413 
414  objects[i]->size=point2point_distance(xi,yi,xf,yf);
415  }
416 }
417 
418 void clean_objets(vector<t_objectPtr> &objects)
419 {
420  free_lines(objects);
421 }
422 
423 void recursive_IEPF(t_objectPtr& object,t_data& data,int start,int end,t_config& config)
424 {
427  int i,index=0;
428  double mean_variance,max_variance,current_variance;
429 
430  t_linePtr line(new t_line);
431 
432  line->alpha=atan2(data.x[start]-data.x[end],data.y[end]-data.y[start])+M_PI;
433  line->ro=data.x[start]*cos(line->alpha)+data.y[start]*sin(line->alpha);
434  line->xi=data.x[start];
435  line->yi=data.y[start];
436  line->xf=data.x[end];
437  line->yf=data.y[end];
438 
439  mean_variance=0;
440  max_variance=0;
441  for(i=start;i<end;i++)
442  {
443  current_variance=pow(point2line_distance(line->alpha,line->ro,data.x[i],data.y[i]),2);
444  mean_variance+=current_variance;
445 
446  if(current_variance>max_variance)
447  {
448  max_variance=current_variance;
449  index=i;
450  }
451  }
452 
453  mean_variance/=end-start;
454  mean_variance=sqrt(mean_variance);
455 
456 // if(object->lines.size()>20)
457 // goto F2;
458 
459  if(mean_variance>config.max_mean_variance)
460  {
461  recursive_IEPF(object,data,start,index,config);
462  recursive_IEPF(object,data,index,end,config);
463  return;
464  }
465 
466 // F2:
467 
468  object->lines.push_back(line);
469 // object->line[object->lines.size()]=(t_line*)malloc(sizeof(t_line));
470 // memcpy(object->line[object->lines.size()],&line,sizeof(t_line));
471 // object->n_lines++;
472 
473  return;
474 }
475 
476 void free_lines(vector<t_objectPtr> &objects)
477 {
478  for(uint i=0;i<objects.size();i++)
479  objects[i]->lines.clear();
480 }
481 
482 void recursive_line_fitting(t_objectPtr& object,t_cluster& cluster,t_data& data,t_config& config)
483 {
484  if(!data.n_points)
485  return;
486 
487  recursive_IEPF(object,data,cluster.stp,cluster.enp,config);
488 }
bool clusters2objects(vector< t_objectPtr > &objectsPtr, vector< t_clustersPtr > &clusters, t_data &data, t_config &config)
Converts clusters of points into objects using recursive line fitting.
double C0
Definition: mtt_common.h:146
double ClusteringThreshold(double r1, double t1, double r2, double t2, t_config *config)
This structure contains a single line properties.
Definition: mtt_common.h:196
Clustering related functions header.
void remove_border_points(t_cluster **clusters, int size, int npoints)
Removes cluster border points.
double dietmayer_threshold(double r, t_config *config)
Computes dietmayer clustering threshold.
boost::shared_ptr< t_line > t_linePtr
Definition: mtt_common.h:205
double r[2160]
Definition: mtt_common.h:118
#define _MAX_CLUSTERS_
Definition: mtt_common.h:58
double t[2160]
Definition: mtt_common.h:118
double point2line_distance(double alpha, double ro, double x, double y)
Calculates the line to point distance.
double y[2160]
Definition: mtt_common.h:117
t_cluster ** clustering(t_data *data, int *count, t_config *config, t_flag *flags)
Performs clustering operation.
void calc_object_props(vector< t_objectPtr > &objects)
Computes object properties, such as centroid and size.
boost::shared_ptr< t_cluster > t_clustersPtr
Definition: mtt_common.h:110
void remove_small_clusters(t_cluster **clusters, int *size, int threshold)
Removes clusters based on their size.
double cluster_break_distance
Definition: mtt_common.h:182
boost::shared_ptr< t_object > t_objectPtr
Definition: mtt_common.h:240
double point2point_distance(double xi, double yi, double xf, double yf)
void recursive_line_fitting(t_objectPtr &object, t_cluster &cluster, t_data &data, t_config &config)
Recursive line fitting, complete process.
double fi
Parameters for the dietmayer clustering.
Definition: mtt_common.h:146
void FlagCollisionWithOcclusion(t_cluster **clusters, int object_size, t_data *data, t_config *)
int n_points
Definition: mtt_common.h:122
This structure contains object information.
Definition: mtt_common.h:210
void recursive_IEPF(t_objectPtr &object, t_data &data, int start, int end, t_config &config)
Recursive iterative end point fit. Single step.
void free_lines(vector< t_objectPtr > &objects)
Removes lines from memory.
Timer t
Definition: k_best.cpp:34
Cluster type class, clusters are groups of points in close proximity.
double t
theta
Definition: mtt_common.h:131
This structure has all points coordinates.
Definition: mtt_common.h:115
double beta
Definition: mtt_common.h:146
This structure contains global configurations parameters.
Definition: mtt_common.h:137
void calc_cluster_props(t_cluster **clusters, int size, t_data *data, t_config *)
Calculates cluster properties.
void clean_objets(vector< t_objectPtr > &objects)
Frees memory associated with objects.
double x[2160]
Definition: mtt_common.h:117
Polar point structure.
Definition: mtt_common.h:126
This structure contains global flags parameters.
Definition: mtt_common.h:369
double max_mean_variance
for the iepf
Definition: mtt_common.h:153
bool fp_s
first point of scan, first image
Definition: mtt_common.h:372
double r
rho
Definition: mtt_common.h:129
#define deg2rad(a)
Definition: mtt_common.h:61


mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18