39 double angular_discrepance=1;
40 double range_discrepance=500;
43 for(
int k=1;k<object_size-1;k++)
45 start_cur.
r=data->
r[clusters[k]->stp];
46 start_cur.
t=data->
t[clusters[k]->stp];
48 end_cur.
r=data->
r[clusters[k]->enp];
49 end_cur.
t=data->
t[clusters[k]->enp];
51 end_prev.
r=data->
r[clusters[k-1]->enp];
52 end_prev.
t=data->
t[clusters[k-1]->enp];
54 start_next.
r=data->
r[clusters[k+1]->stp];
55 start_next.
t=data->
t[clusters[k+1]->stp];
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;
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;
89 min_dist=sqrt( pow(Ax-Bx,2) + pow(Ay-By,2) );
94 return config->
C0 + min_dist/cos(config->
beta);
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);
104 double min_dist=tan(0.5*M_PI/180.)*r;
105 printf(
"Min dist %f\n",min_dist);
106 printf(
"CALCILC THIS\n");
110 return config->
C0+r*sqrt(2*(1+cos(config->
fi)));
115 int i,cluster_count=0;
116 double x,y,xold=0,yold=0;
117 double dist,threshold;
119 static bool initialise=
true;
140 dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold));
149 clusters[cluster_count]->enp=i-1;
150 clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;
151 clusters[cluster_count]->partialy_occluded=
false;
153 clusters[cluster_count]->stp=i;
154 clusters[cluster_count]->lenght=0;
157 clusters[cluster_count]->lenght+=dist;
160 clusters[cluster_count]->enp=i-1;
161 clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;
162 clusters[cluster_count]->partialy_occluded=
false;
164 clusters[cluster_count]->stp=i;
165 clusters[cluster_count]->lenght=0;
172 clusters[cluster_count]->enp=i;
173 clusters[cluster_count]->n_points=clusters[cluster_count]->enp-clusters[cluster_count]->stp;
179 clusters[cluster_count]->stp=0;
180 clusters[cluster_count]->enp=0;
181 clusters[cluster_count]->lenght=0;
191 *count=cluster_count+1;
199 double x,y,xold=0,yold=0;
200 double dist,threshold;
206 cluster->id=clustersPtr.size();
215 dist = sqrt((x-xold)*(x-xold)+(y-yold)*(y-yold));
221 cluster->n_points=cluster->enp-cluster->stp;
222 cluster->partialy_occluded=
false;
223 clustersPtr.push_back(cluster);
227 cluster->id=clustersPtr.size();
232 cluster->lenght+=dist;
236 cluster->n_points=cluster->enp-cluster->stp;
237 cluster->partialy_occluded=
false;
238 clustersPtr.push_back(cluster);
242 cluster->id=clustersPtr.size();
252 cluster->n_points=cluster->enp-cluster->stp;
276 if(clusters[i]->n_points<threshold)
278 for(e=i;e<*size-1;e++)
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;
284 memset(clusters[*size],0,
sizeof(
t_cluster));
297 if(clusters[i]->n_points>npoints*2)
299 clusters[i]->stp=clusters[i]->stp+npoints;
300 clusters[i]->enp=clusters[i]->enp-npoints;
303 clusters[i]->n_points=clusters[i]->n_points-npoints*2;
315 clusters[i]->lenght=0;
316 for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
318 if(e<clusters[i]->enp-1)
326 clusters[i]->rmin=rmin;
327 clusters[i]->tm=(data->
t[clusters[i]->stp]+data->
t[clusters[i]->enp])/2;
336 for(uint i=0;i<clusters.size();i++)
339 clusters[i]->lenght=0;
340 for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
342 if(e<clusters[i]->enp-1)
350 clusters[i]->rmin=rmin;
351 clusters[i]->tm=(data.
t[clusters[i]->stp]+data.
t[clusters[i]->enp])/2;
361 for(uint i=0;i<clusters.size();i++)
363 object->rmin=clusters[i]->rmin;
364 object->tm=clusters[i]->tm;
365 object->object_found=
false;
367 object->partialy_occluded=clusters[i]->partialy_occluded;
371 objectsPtr.push_back(
object);
385 for(uint i=0;i<objects.size();i++)
391 for(e=0;e<objects[i]->lines.size();e++)
393 r=sqrt(pow(objects[i]->lines[e]->xi,2)+pow(objects[i]->lines[e]->yi,2));
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));
405 objects[i]->cx=r*cos(t);
406 objects[i]->cy=r*sin(t);
408 xi=objects[i]->lines[0]->xi;
409 yi=objects[i]->lines[0]->yi;
411 xf=objects[i]->lines[objects[i]->lines.size()-1]->xf;
412 yf=objects[i]->lines[objects[i]->lines.size()-1]->yf;
428 double mean_variance,max_variance,current_variance;
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];
441 for(i=start;i<end;i++)
444 mean_variance+=current_variance;
446 if(current_variance>max_variance)
448 max_variance=current_variance;
453 mean_variance/=end-start;
454 mean_variance=sqrt(mean_variance);
468 object->lines.push_back(line);
478 for(uint i=0;i<objects.size();i++)
479 objects[i]->lines.clear();
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 ClusteringThreshold(double r1, double t1, double r2, double t2, t_config *config)
This structure contains a single line properties.
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
double point2line_distance(double alpha, double ro, double x, double y)
Calculates the line to point distance.
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
void remove_small_clusters(t_cluster **clusters, int *size, int threshold)
Removes clusters based on their size.
double cluster_break_distance
boost::shared_ptr< t_object > t_objectPtr
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.
void FlagCollisionWithOcclusion(t_cluster **clusters, int object_size, t_data *data, t_config *)
This structure contains object information.
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.
Cluster type class, clusters are groups of points in close proximity.
This structure has all points coordinates.
This structure contains global configurations parameters.
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.
This structure contains global flags parameters.
double max_mean_variance
for the iepf
bool fp_s
first point of scan, first image