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); points = 0;
00166   cvReleaseMat(&W); W = 0;
00167   cvReleaseMat(&U); U = 0;
00168   cvReleaseMat(&V); V = 0;
00169   cvReleaseMat(&rot_points); rot_points = 0;
00170 
00171   features.push_back(linearity);
00172 
00173   // Compute Circularity
00174   CvMat* A = cvCreateMat( num_points, 3, CV_64FC1);
00175   CvMat* B = cvCreateMat( num_points, 1, CV_64FC1);
00176   {
00177     int j = 0;
00178     for (SampleSet::iterator i = cluster->begin();
00179          i != cluster->end();
00180          i++)
00181     {
00182       float x = (*i)->x;
00183       float y = (*i)->y;
00184 
00185       cvmSet(A, j, 0, -2.0*x);
00186       cvmSet(A, j, 1, -2.0*y);
00187       cvmSet(A, j, 2, 1);
00188 
00189       cvmSet(B, j, 0, -pow(x,2)-pow(y,2));
00190       j++;
00191     }
00192   }
00193   CvMat* sol = cvCreateMat( 3, 1, CV_64FC1);
00194 
00195   cvSolve(A, B, sol, CV_SVD);
00196 
00197   float xc = cvmGet(sol, 0, 0);
00198   float yc = cvmGet(sol, 1, 0);
00199   float rc = sqrt(pow(xc,2) + pow(yc,2) - cvmGet(sol, 2, 0));
00200 
00201   cvReleaseMat(&A); A = 0;
00202   cvReleaseMat(&B); B = 0;
00203   cvReleaseMat(&sol); sol = 0;
00204 
00205   float circularity = 0.0;
00206   for (SampleSet::iterator i = cluster->begin();
00207        i != cluster->end();
00208        i++)
00209   {
00210     circularity += pow( rc - sqrt( pow(xc - (*i)->x, 2) + pow( yc - (*i)->y, 2) ), 2);
00211   }
00212 
00213   features.push_back(circularity);
00214 
00215   // Radius
00216   float radius = rc;
00217 
00218   features.push_back(radius);
00219 
00220   //Curvature:
00221   float mean_curvature = 0.0;
00222 
00223   //Boundary length:
00224   float boundary_length = 0.0;
00225   float last_boundary_seg = 0.0;
00226 
00227   float boundary_regularity = 0.0;
00228   double sum_boundary_reg_sq = 0.0;
00229 
00230   // Mean angular difference
00231   SampleSet::iterator left = cluster->begin();
00232   left++;
00233   left++;
00234   SampleSet::iterator mid = cluster->begin();
00235   mid++;
00236   SampleSet::iterator right = cluster->begin();
00237 
00238   float ang_diff = 0.0;
00239 
00240   while (left != cluster->end())
00241   {
00242     float mlx = (*left)->x - (*mid)->x;
00243     float mly = (*left)->y - (*mid)->y;
00244     float L_ml = sqrt(mlx*mlx + mly*mly);
00245 
00246     float mrx = (*right)->x - (*mid)->x;
00247     float mry = (*right)->y - (*mid)->y;
00248     float L_mr = sqrt(mrx*mrx + mry*mry);
00249 
00250     float lrx = (*left)->x - (*right)->x;
00251     float lry = (*left)->y - (*right)->y;
00252     float L_lr = sqrt(lrx*lrx + lry*lry);
00253 
00254     boundary_length += L_mr;
00255     sum_boundary_reg_sq += L_mr*L_mr;
00256     last_boundary_seg = L_ml;
00257 
00258     float A = (mlx*mrx + mly*mry) / pow(L_mr, 2);
00259     float B = (mlx*mry - mly*mrx) / pow(L_mr, 2);
00260 
00261     float th = atan2(B,A);
00262 
00263     if (th < 0)
00264       th += 2*M_PI;
00265 
00266     ang_diff += th / num_points;
00267 
00268     float s = 0.5*(L_ml+L_mr+L_lr);
00269     float area = sqrt( s*(s-L_ml)*(s-L_mr)*(s-L_lr) );
00270 
00271     if (th > 0)
00272       mean_curvature += 4*(area)/(L_ml*L_mr*L_lr*num_points);
00273     else
00274       mean_curvature -= 4*(area)/(L_ml*L_mr*L_lr*num_points);
00275 
00276     left++;
00277     mid++;
00278     right++;
00279   }
00280 
00281   boundary_length += last_boundary_seg;
00282   sum_boundary_reg_sq += last_boundary_seg*last_boundary_seg;
00283 
00284   boundary_regularity = sqrt( (sum_boundary_reg_sq - pow(boundary_length,2)/num_points)/(num_points - 1) );
00285 
00286   features.push_back(boundary_length);
00287   features.push_back(ang_diff);
00288   features.push_back(mean_curvature);
00289 
00290   features.push_back(boundary_regularity);
00291 
00292 
00293   // Mean angular difference
00294   first = cluster->begin();
00295   mid = cluster->begin();
00296   mid++;
00297   last = cluster->end();
00298   last--;
00299   
00300   double sum_iav = 0.0;
00301   double sum_iav_sq  = 0.0;
00302 
00303   while (mid != last)
00304   {
00305     float mlx = (*first)->x - (*mid)->x;
00306     float mly = (*first)->y - (*mid)->y;
00307     //float L_ml = sqrt(mlx*mlx + mly*mly);
00308 
00309     float mrx = (*last)->x - (*mid)->x;
00310     float mry = (*last)->y - (*mid)->y;
00311     float L_mr = sqrt(mrx*mrx + mry*mry);
00312 
00313     //float lrx = (*first)->x - (*last)->x;
00314     //float lry = (*first)->y - (*last)->y;
00315     //float L_lr = sqrt(lrx*lrx + lry*lry);
00316       
00317     float A = (mlx*mrx + mly*mry) / pow(L_mr, 2);
00318     float B = (mlx*mry - mly*mrx) / pow(L_mr, 2);
00319 
00320     float th = atan2(B,A);
00321 
00322     if (th < 0)
00323       th += 2*M_PI;
00324 
00325     sum_iav += th;
00326     sum_iav_sq += th*th;
00327 
00328     mid++;
00329   }
00330 
00331   float iav = sum_iav / num_points;
00332   float std_iav = sqrt( (sum_iav_sq - pow(sum_iav,2)/num_points)/(num_points - 1) );
00333 
00334   features.push_back(iav);
00335   features.push_back(std_iav);
00336 
00337   return features;
00338 }


leg_detector
Author(s): Caroline Pantofaru
autogenerated on Thu Aug 27 2015 14:18:07