37 #include "opencv/cxcore.h" 38 #include "opencv/cv.h" 46 vector<float> features;
49 int num_points = cluster->size();
55 vector<float> x_median_set;
56 vector<float> y_median_set;
57 for (SampleSet::iterator i = cluster->begin();
62 x_mean += ((*i)->x) / num_points;
63 y_mean += ((*i)->y) / num_points;
64 x_median_set.push_back((*i)->x);
65 y_median_set.push_back((*i)->y);
68 std::sort(x_median_set.begin(), x_median_set.end());
69 std::sort(y_median_set.begin(), y_median_set.end());
71 float x_median = 0.5 * (*(x_median_set.begin() + (num_points - 1) / 2) + * (x_median_set.begin() + num_points / 2));
72 float y_median = 0.5 * (*(y_median_set.begin() + (num_points - 1) / 2) + * (y_median_set.begin() + num_points / 2));
76 double sum_std_diff = 0.0;
77 double sum_med_diff = 0.0;
80 for (SampleSet::iterator i = cluster->begin();
85 sum_std_diff += pow((*i)->x - x_mean, 2) + pow((*i)->y - y_mean, 2);
86 sum_med_diff += sqrt(pow((*i)->x - x_median, 2) + pow((*i)->y - y_median, 2));
89 float std = sqrt(1.0 / (num_points - 1.0) * sum_std_diff);
90 float avg_median_dev = sum_med_diff / num_points;
92 features.push_back(std);
93 features.push_back(avg_median_dev);
97 SampleSet::iterator first = cluster->begin();
98 SampleSet::iterator last = cluster->end();
102 int prev_ind = (*first)->index - 1;
103 int next_ind = (*last)->index + 1;
113 prev_jump = sqrt(pow((*first)->x - prev->
x, 2) + pow((*first)->y - prev->
y, 2));
119 if (next_ind < (
int)scan.ranges.size())
124 next_jump = sqrt(pow((*last)->x - next->
x, 2) + pow((*last)->y - next->
y, 2));
129 features.push_back(prev_jump);
130 features.push_back(next_jump);
133 float width = sqrt(pow((*first)->x - (*last)->x, 2) + pow((*first)->y - (*last)->y, 2));
134 features.push_back(width);
138 CvMat* points = cvCreateMat(num_points, 2, CV_64FC1);
141 for (SampleSet::iterator i = cluster->begin();
145 cvmSet(points, j, 0, (*i)->x - x_mean);
146 cvmSet(points, j, 1, (*i)->y - y_mean);
151 CvMat* W = cvCreateMat(2, 2, CV_64FC1);
152 CvMat* U = cvCreateMat(num_points, 2, CV_64FC1);
153 CvMat* V = cvCreateMat(2, 2, CV_64FC1);
154 cvSVD(points, W, U, V);
156 CvMat* rot_points = cvCreateMat(num_points, 2, CV_64FC1);
157 cvMatMul(U, W, rot_points);
159 float linearity = 0.0;
160 for (
int i = 0; i < num_points; i++)
162 linearity += pow(cvmGet(rot_points, i, 1), 2);
165 cvReleaseMat(&points);
173 cvReleaseMat(&rot_points);
176 features.push_back(linearity);
179 CvMat* A = cvCreateMat(num_points, 3, CV_64FC1);
180 CvMat* B = cvCreateMat(num_points, 1, CV_64FC1);
183 for (SampleSet::iterator i = cluster->begin();
190 cvmSet(A, j, 0, -2.0 * x);
191 cvmSet(A, j, 1, -2.0 * y);
194 cvmSet(B, j, 0, -pow(x, 2) - pow(y, 2));
198 CvMat* sol = cvCreateMat(3, 1, CV_64FC1);
200 cvSolve(A, B, sol, CV_SVD);
202 float xc = cvmGet(sol, 0, 0);
203 float yc = cvmGet(sol, 1, 0);
204 float rc = sqrt(pow(xc, 2) + pow(yc, 2) - cvmGet(sol, 2, 0));
213 float circularity = 0.0;
214 for (SampleSet::iterator i = cluster->begin();
218 circularity += pow(rc - sqrt(pow(xc - (*i)->x, 2) + pow(yc - (*i)->y, 2)), 2);
221 features.push_back(circularity);
226 features.push_back(radius);
229 float mean_curvature = 0.0;
232 float boundary_length = 0.0;
233 float last_boundary_seg = 0.0;
235 float boundary_regularity = 0.0;
236 double sum_boundary_reg_sq = 0.0;
239 SampleSet::iterator left = cluster->begin();
242 SampleSet::iterator mid = cluster->begin();
244 SampleSet::iterator right = cluster->begin();
246 float ang_diff = 0.0;
248 while (left != cluster->end())
250 float mlx = (*left)->x - (*mid)->x;
251 float mly = (*left)->y - (*mid)->y;
252 float L_ml = sqrt(mlx * mlx + mly * mly);
254 float mrx = (*right)->x - (*mid)->x;
255 float mry = (*right)->y - (*mid)->y;
256 float L_mr = sqrt(mrx * mrx + mry * mry);
258 float lrx = (*left)->x - (*right)->x;
259 float lry = (*left)->y - (*right)->y;
260 float L_lr = sqrt(lrx * lrx + lry * lry);
262 boundary_length += L_mr;
263 sum_boundary_reg_sq += L_mr * L_mr;
264 last_boundary_seg = L_ml;
266 float A = (mlx * mrx + mly * mry) / pow(L_mr, 2);
267 float B = (mlx * mry - mly * mrx) / pow(L_mr, 2);
269 float th = atan2(B, A);
274 ang_diff += th / num_points;
276 float s = 0.5 * (L_ml + L_mr + L_lr);
277 float area = sqrt(s * (s - L_ml) * (s - L_mr) * (s - L_lr));
280 mean_curvature += 4 * (area) / (L_ml * L_mr * L_lr * num_points);
282 mean_curvature -= 4 * (area) / (L_ml * L_mr * L_lr * num_points);
289 boundary_length += last_boundary_seg;
290 sum_boundary_reg_sq += last_boundary_seg * last_boundary_seg;
292 boundary_regularity = sqrt((sum_boundary_reg_sq - pow(boundary_length, 2) / num_points) / (num_points - 1));
294 features.push_back(boundary_length);
295 features.push_back(ang_diff);
296 features.push_back(mean_curvature);
298 features.push_back(boundary_regularity);
302 first = cluster->begin();
303 mid = cluster->begin();
305 last = cluster->end();
308 double sum_iav = 0.0;
309 double sum_iav_sq = 0.0;
313 float mlx = (*first)->x - (*mid)->x;
314 float mly = (*first)->y - (*mid)->y;
317 float mrx = (*last)->x - (*mid)->x;
318 float mry = (*last)->y - (*mid)->y;
319 float L_mr = sqrt(mrx * mrx + mry * mry);
325 float A = (mlx * mrx + mly * mry) / pow(L_mr, 2);
326 float B = (mlx * mry - mly * mrx) / pow(L_mr, 2);
328 float th = atan2(B, A);
334 sum_iav_sq += th * th;
339 float iav = sum_iav / num_points;
340 float std_iav = sqrt((sum_iav_sq - pow(sum_iav, 2) / num_points) / (num_points - 1));
342 features.push_back(iav);
343 features.push_back(std_iav);
TFSIMD_FORCE_INLINE const tfScalar & y() const
vector< float > calcLegFeatures(SampleSet *cluster, const sensor_msgs::LaserScan &scan)
A namespace containing the laser processor helper classes.
An ordered set of Samples.
A struct representing a single sample from the laser.
static Sample * Extract(int ind, const sensor_msgs::LaserScan &scan)