calc_leg_features.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <leg_detector/calc_leg_features.h>
00036 
00037 #include "opencv/cxcore.h"
00038 #include "opencv/cv.h"
00039 
00040 using namespace laser_processor;
00041 using namespace std;
00042 
00043 vector<float> calcLegFeatures(SampleSet* cluster, const sensor_msgs::LaserScan& scan)
00044 {
00045 
00046   vector<float> features;
00047 
00048   // Number of points
00049   int num_points = cluster->size();
00050   //  features.push_back(num_points);
00051 
00052   // Compute mean and median points for future use
00053   float x_mean = 0.0;
00054   float y_mean = 0.0;
00055   vector<float> x_median_set;
00056   vector<float> y_median_set;
00057   for (SampleSet::iterator i = cluster->begin();
00058        i != cluster->end();
00059        i++)
00060 
00061   {
00062     x_mean += ((*i)->x) / num_points;
00063     y_mean += ((*i)->y) / num_points;
00064     x_median_set.push_back((*i)->x);
00065     y_median_set.push_back((*i)->y);
00066   }
00067 
00068   std::sort(x_median_set.begin(), x_median_set.end());
00069   std::sort(y_median_set.begin(), y_median_set.end());
00070 
00071   float x_median = 0.5 * (*(x_median_set.begin() + (num_points - 1) / 2) + * (x_median_set.begin() + num_points / 2));
00072   float y_median = 0.5 * (*(y_median_set.begin() + (num_points - 1) / 2) + * (y_median_set.begin() + num_points / 2));
00073 
00074   //Compute std and avg diff from median
00075 
00076   double sum_std_diff = 0.0;
00077   double sum_med_diff = 0.0;
00078 
00079 
00080   for (SampleSet::iterator i = cluster->begin();
00081        i != cluster->end();
00082        i++)
00083 
00084   {
00085     sum_std_diff += pow((*i)->x - x_mean, 2) + pow((*i)->y - y_mean, 2);
00086     sum_med_diff += sqrt(pow((*i)->x - x_median, 2) + pow((*i)->y - y_median, 2));
00087   }
00088 
00089   float std = sqrt(1.0 / (num_points - 1.0) * sum_std_diff);
00090   float avg_median_dev = sum_med_diff / num_points;
00091 
00092   features.push_back(std);
00093   features.push_back(avg_median_dev);
00094 
00095 
00096   // Take first at last
00097   SampleSet::iterator first = cluster->begin();
00098   SampleSet::iterator last = cluster->end();
00099   last--;
00100 
00101   // Compute Jump distance
00102   int prev_ind = (*first)->index - 1;
00103   int next_ind = (*last)->index + 1;
00104 
00105   float prev_jump = 0;
00106   float next_jump = 0;
00107 
00108   if (prev_ind >= 0)
00109   {
00110     Sample* prev = Sample::Extract(prev_ind, scan);
00111     if (prev)
00112     {
00113       prev_jump = sqrt(pow((*first)->x - prev->x, 2) + pow((*first)->y - prev->y, 2));
00114       delete prev;
00115     }
00116 
00117   }
00118 
00119   if (next_ind < (int)scan.ranges.size())
00120   {
00121     Sample* next = Sample::Extract(next_ind, scan);
00122     if (next)
00123     {
00124       next_jump = sqrt(pow((*last)->x - next->x, 2) + pow((*last)->y - next->y, 2));
00125       delete next;
00126     }
00127   }
00128 
00129   features.push_back(prev_jump);
00130   features.push_back(next_jump);
00131 
00132   // Compute Width
00133   float width = sqrt(pow((*first)->x - (*last)->x, 2) + pow((*first)->y - (*last)->y, 2));
00134   features.push_back(width);
00135 
00136   // Compute Linearity
00137 
00138   CvMat* points = cvCreateMat(num_points, 2, CV_64FC1);
00139   {
00140     int j = 0;
00141     for (SampleSet::iterator i = cluster->begin();
00142          i != cluster->end();
00143          i++)
00144     {
00145       cvmSet(points, j, 0, (*i)->x - x_mean);
00146       cvmSet(points, j, 1, (*i)->y - y_mean);
00147       j++;
00148     }
00149   }
00150 
00151   CvMat* W = cvCreateMat(2, 2, CV_64FC1);
00152   CvMat* U = cvCreateMat(num_points, 2, CV_64FC1);
00153   CvMat* V = cvCreateMat(2, 2, CV_64FC1);
00154   cvSVD(points, W, U, V);
00155 
00156   CvMat* rot_points = cvCreateMat(num_points, 2, CV_64FC1);
00157   cvMatMul(U, W, rot_points);
00158 
00159   float linearity = 0.0;
00160   for (int i = 0; i < num_points; i++)
00161   {
00162     linearity += pow(cvmGet(rot_points, i, 1), 2);
00163   }
00164 
00165   cvReleaseMat(&points);
00166   points = 0;
00167   cvReleaseMat(&W);
00168   W = 0;
00169   cvReleaseMat(&U);
00170   U = 0;
00171   cvReleaseMat(&V);
00172   V = 0;
00173   cvReleaseMat(&rot_points);
00174   rot_points = 0;
00175 
00176   features.push_back(linearity);
00177 
00178   // Compute Circularity
00179   CvMat* A = cvCreateMat(num_points, 3, CV_64FC1);
00180   CvMat* B = cvCreateMat(num_points, 1, CV_64FC1);
00181   {
00182     int j = 0;
00183     for (SampleSet::iterator i = cluster->begin();
00184          i != cluster->end();
00185          i++)
00186     {
00187       float x = (*i)->x;
00188       float y = (*i)->y;
00189 
00190       cvmSet(A, j, 0, -2.0 * x);
00191       cvmSet(A, j, 1, -2.0 * y);
00192       cvmSet(A, j, 2, 1);
00193 
00194       cvmSet(B, j, 0, -pow(x, 2) - pow(y, 2));
00195       j++;
00196     }
00197   }
00198   CvMat* sol = cvCreateMat(3, 1, CV_64FC1);
00199 
00200   cvSolve(A, B, sol, CV_SVD);
00201 
00202   float xc = cvmGet(sol, 0, 0);
00203   float yc = cvmGet(sol, 1, 0);
00204   float rc = sqrt(pow(xc, 2) + pow(yc, 2) - cvmGet(sol, 2, 0));
00205 
00206   cvReleaseMat(&A);
00207   A = 0;
00208   cvReleaseMat(&B);
00209   B = 0;
00210   cvReleaseMat(&sol);
00211   sol = 0;
00212 
00213   float circularity = 0.0;
00214   for (SampleSet::iterator i = cluster->begin();
00215        i != cluster->end();
00216        i++)
00217   {
00218     circularity += pow(rc - sqrt(pow(xc - (*i)->x, 2) + pow(yc - (*i)->y, 2)), 2);
00219   }
00220 
00221   features.push_back(circularity);
00222 
00223   // Radius
00224   float radius = rc;
00225 
00226   features.push_back(radius);
00227 
00228   //Curvature:
00229   float mean_curvature = 0.0;
00230 
00231   //Boundary length:
00232   float boundary_length = 0.0;
00233   float last_boundary_seg = 0.0;
00234 
00235   float boundary_regularity = 0.0;
00236   double sum_boundary_reg_sq = 0.0;
00237 
00238   // Mean angular difference
00239   SampleSet::iterator left = cluster->begin();
00240   left++;
00241   left++;
00242   SampleSet::iterator mid = cluster->begin();
00243   mid++;
00244   SampleSet::iterator right = cluster->begin();
00245 
00246   float ang_diff = 0.0;
00247 
00248   while (left != cluster->end())
00249   {
00250     float mlx = (*left)->x - (*mid)->x;
00251     float mly = (*left)->y - (*mid)->y;
00252     float L_ml = sqrt(mlx * mlx + mly * mly);
00253 
00254     float mrx = (*right)->x - (*mid)->x;
00255     float mry = (*right)->y - (*mid)->y;
00256     float L_mr = sqrt(mrx * mrx + mry * mry);
00257 
00258     float lrx = (*left)->x - (*right)->x;
00259     float lry = (*left)->y - (*right)->y;
00260     float L_lr = sqrt(lrx * lrx + lry * lry);
00261 
00262     boundary_length += L_mr;
00263     sum_boundary_reg_sq += L_mr * L_mr;
00264     last_boundary_seg = L_ml;
00265 
00266     float A = (mlx * mrx + mly * mry) / pow(L_mr, 2);
00267     float B = (mlx * mry - mly * mrx) / pow(L_mr, 2);
00268 
00269     float th = atan2(B, A);
00270 
00271     if (th < 0)
00272       th += 2 * M_PI;
00273 
00274     ang_diff += th / num_points;
00275 
00276     float s = 0.5 * (L_ml + L_mr + L_lr);
00277     float area = sqrt(s * (s - L_ml) * (s - L_mr) * (s - L_lr));
00278 
00279     if (th > 0)
00280       mean_curvature += 4 * (area) / (L_ml * L_mr * L_lr * num_points);
00281     else
00282       mean_curvature -= 4 * (area) / (L_ml * L_mr * L_lr * num_points);
00283 
00284     left++;
00285     mid++;
00286     right++;
00287   }
00288 
00289   boundary_length += last_boundary_seg;
00290   sum_boundary_reg_sq += last_boundary_seg * last_boundary_seg;
00291 
00292   boundary_regularity = sqrt((sum_boundary_reg_sq - pow(boundary_length, 2) / num_points) / (num_points - 1));
00293 
00294   features.push_back(boundary_length);
00295   features.push_back(ang_diff);
00296   features.push_back(mean_curvature);
00297 
00298   features.push_back(boundary_regularity);
00299 
00300 
00301   // Mean angular difference
00302   first = cluster->begin();
00303   mid = cluster->begin();
00304   mid++;
00305   last = cluster->end();
00306   last--;
00307 
00308   double sum_iav = 0.0;
00309   double sum_iav_sq  = 0.0;
00310 
00311   while (mid != last)
00312   {
00313     float mlx = (*first)->x - (*mid)->x;
00314     float mly = (*first)->y - (*mid)->y;
00315     //float L_ml = sqrt(mlx*mlx + mly*mly);
00316 
00317     float mrx = (*last)->x - (*mid)->x;
00318     float mry = (*last)->y - (*mid)->y;
00319     float L_mr = sqrt(mrx * mrx + mry * mry);
00320 
00321     //float lrx = (*first)->x - (*last)->x;
00322     //float lry = (*first)->y - (*last)->y;
00323     //float L_lr = sqrt(lrx*lrx + lry*lry);
00324 
00325     float A = (mlx * mrx + mly * mry) / pow(L_mr, 2);
00326     float B = (mlx * mry - mly * mrx) / pow(L_mr, 2);
00327 
00328     float th = atan2(B, A);
00329 
00330     if (th < 0)
00331       th += 2 * M_PI;
00332 
00333     sum_iav += th;
00334     sum_iav_sq += th * th;
00335 
00336     mid++;
00337   }
00338 
00339   float iav = sum_iav / num_points;
00340   float std_iav = sqrt((sum_iav_sq - pow(sum_iav, 2) / num_points) / (num_points - 1));
00341 
00342   features.push_back(iav);
00343   features.push_back(std_iav);
00344 
00345   return features;
00346 }


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Apr 13 2017 02:41:52