leaf-parameters.h
Go to the documentation of this file.
```00001 /*
00002 Copyright (C) 2012 Babette Dellen
00003
00004 Algorithm to find the surface normal and center point of a segment.
00005
00006 */
00007
00008 #ifndef LEAF_PARAMETERS
00009 #define LEAF_PARAMETERS
00010 #include <iostream>
00011 //#include "cv.h"
00012 #include <cstdlib>
00013 #include <image.h>
00014 #include <misc.h>
00015 #include <filter.h>
00016 //#include "segment-image.h"
00017 #include <vector>
00018 #define PI 3.14159265
00019 using std::vector;
00020
00021 double FittingErrorPlane(double * x_val, double * y_val, double * z_val, int num_p, double * param_cur)//int current_label)
00022 {
00023
00024 double surf_out = 0;
00025
00026
00027     for (int j=0;j<num_p; j++)
00028     {
00029     surf_out = surf_out +  pow((param_cur[0]*x_val[j] + param_cur[1]*y_val[j] + param_cur[2] - z_val[j]),2);
00030     }
00031
00032     surf_out = surf_out/((double)(num_p));
00033     return (surf_out);
00034 }
00036
00037 //****************************************************************************80
00038
00039 void nelmin_plane (int n, double * start, double * xmin,
00040   double * ynewlo, double reqmin, double * step, int konvge, int kcount,
00041   int *icount, int *numres, int *ifault,double * valX, double * valY,double * valZ, int number_points)
00042
00043
00044 //****************************************************************************80
00045 //
00046 //  Purpose:
00047 //
00048 //    NELMIN minimizes a function using the Nelder-Mead algorithm.
00049 //
00050 //  Discussion:
00051 //
00052 //    This routine seeks the minimum value of a user-specified function.
00053 //
00054 //    Simplex function minimisation procedure due to Nelder+Mead(1965),
00055 //    as implemented by O'Neill(1971, Appl.Statist. 20, 338-45), with
00056 //    subsequent comments by Chambers+Ertel(1974, 23, 250-1), Benyon(1976,
00057 //    25, 97) and Hill(1978, 27, 380-2)
00058 //
00059 //    The function to be minimized must be defined by a function of
00060 //    the form
00061 //
00062 //      function fn ( x, f )
00063 //      double fn
00064 //      double x(*)
00065 //
00066 //    and the name of this subroutine must be declared EXTERNAL in the
00067 //    calling routine and passed as the argument FN.
00068 //
00069 //    This routine does not include a termination test using the
00070 //    fitting of a quadratic surface.
00071 //
00072 //  Licensing:
00073 //
00075 //
00076 //  Modified:
00077 //
00078 //    27 February 2008
00079 //
00080 //  Author:
00081 //
00082 //    Original FORTRAN77 version by R ONeill.
00083 //    C++ version by John Burkardt.
00084 //
00085 //  Reference:
00086 //
00087 //    John Nelder, Roger Mead,
00088 //    A simplex method for function minimization,
00089 //    Computer Journal,
00090 //    Volume 7, 1965, pages 308-313.
00091 //
00092 //    R ONeill,
00093 //    Algorithm AS 47:
00094 //    Function Minimization Using a Simplex Procedure,
00095 //    Applied Statistics,
00096 //    Volume 20, Number 3, 1971, pages 338-345.
00097 //
00098 //  Parameters:
00099 //
00100 //    Input, double FN ( double x[] ), the name of the routine which evaluates
00101 //    the function to be minimized.
00102 //
00103 //    Input, int N, the number of variables.
00104 //
00105 //    Input/output, double START[N].  On input, a starting point
00106 //    for the iteration.  On output, this data may have been overwritten.
00107 //
00108 //    Output, double XMIN[N], the coordinates of the point which
00109 //    is estimated to minimize the function.
00110 //
00111 //    Output, double YNEWLO, the minimum value of the function.
00112 //
00113 //    Input, double REQMIN, the terminating limit for the variance
00114 //    of function values.
00115 //
00116 //    Input, double STEP[N], determines the size and shape of the
00117 //    initial simplex.  The relative magnitudes of its elements should reflect
00118 //    the units of the variables.
00119 //
00120 //    Input, int KONVGE, the convergence check is carried out
00121 //    every KONVGE iterations.
00122 //
00123 //    Input, int KCOUNT, the maximum number of function
00124 //    evaluations.
00125 //
00126 //    Output, int *ICOUNT, the number of function evaluations
00127 //    used.
00128 //
00129 //    Output, int *NUMRES, the number of restarts.
00130 //
00131 //    Output, int *IFAULT, error indicator.
00132 //    0, no errors detected.
00133 //    1, REQMIN, N, or KONVGE has an illegal value.
00134 //    2, iteration terminated because KCOUNT was exceeded without convergence.
00135 //
00136 {
00137   double ccoeff = 0.5;
00138   double del;
00139   double dn;
00140   double dnn;
00141   double ecoeff = 2.0;
00142   double eps = 0.001;
00143   int i;
00144   int ihi;
00145   int ilo;
00146   int j;
00147   int jcount;
00148   int l;
00149   int nn;
00150   double *p;
00151   double *p2star;
00152   double *pbar;
00153   double *pstar;
00154   double rcoeff = 1.0;
00155   double rq;
00156   double x;
00157   double *y;
00158   double y2star;
00159   double ylo;
00160   double ystar;
00161   double z;
00162   double * start_init;
00163 //
00164 //  Check the input parameters.
00165 //
00166   if ( reqmin <= 0.0 )
00167   {
00168     *ifault = 1;
00169     return;
00170   }
00171
00172   if ( n < 1 )
00173   {
00174     *ifault = 1;
00175     return;
00176   }
00177
00178   if ( konvge < 1 )
00179   {
00180     *ifault = 1;
00181     return;
00182   }
00183
00184   p = new double[n*(n+1)];
00185   pstar = new double[n];
00186   p2star = new double[n];
00187   pbar = new double[n];
00188   y = new double[n+1];
00189   //start_init = new double[n];
00190
00191 //   for ( i = 0; i < n; i++ )
00192 //     {
00193 //       start_init[i] = start[i];
00194 //     }
00195 //
00196
00197   *icount = 0;
00198   *numres = 0;
00199
00200   jcount = konvge;
00201   dn = ( double ) ( n );
00202   nn = n + 1;
00203   dnn = ( double ) ( nn );
00204   del = 1.0;
00205   rq = reqmin * dn;
00206 //
00207 //  Initial or restarted loop.
00208 //
00209   for ( ; ; )
00210   {
00211     for ( i = 0; i < n; i++ )
00212     {
00213       p[i+n*n] = start[i];
00214     }
00215     y[n] = FittingErrorPlane(valX, valY, valZ, number_points,start);
00216     //FittingError(x_val,y_val,z_val,start,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( start );
00217     *icount = *icount + 1;
00218
00219     for ( j = 0; j < n; j++ )
00220     {
00221       x = start[j];
00222       start[j] = start[j] + step[j] * del;
00223       for ( i = 0; i < n; i++ )
00224       {
00225 //      if (i==j)
00226 //      {start[i]=start_init[j] + step[j] * del;
00227 //      p[i+j*n] =start_init[j] + step[j] * del;
00228 //      }
00229 //      else
00230 //      {start[i]=start_init[i];
00231 //      p[i+j*n]=start_init[i];
00232 //      }
00233
00234
00235         p[i+j*n] = start[i];
00236       }
00237       y[j] = FittingErrorPlane(valX, valY, valZ, number_points,start);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,start);
00238       //FittingError(x_val,y_val,z_val,start,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( start );
00239       *icount = *icount + 1;
00240       start[j] = x;
00241     }
00242 //
00243 //  The simplex construction is complete.
00244 //
00245 //  Find highest and lowest Y values.  YNEWLO = Y(IHI) indicates
00246 //  the vertex of the simplex to be replaced.
00247 //
00248     ylo = y[0];
00249     ilo = 0;
00250
00251     for ( i = 1; i < nn; i++ )
00252     {
00253       if ( y[i] < ylo )
00254       {
00255         ylo = y[i];
00256         ilo = i;
00257       }
00258     }
00259 //
00260 //  Inner loop.
00261 //
00262     for ( ; ; )
00263     {
00264       if ( kcount <= *icount )
00265       {
00266         break;
00267       }
00268       *ynewlo = y[0];
00269       ihi = 0;
00270
00271       for ( i = 1; i < nn; i++ )
00272       {
00273         if ( *ynewlo < y[i] )
00274         {
00275           *ynewlo = y[i];
00276           ihi = i;
00277         }
00278       }
00279 //
00280 //  Calculate PBAR, the centroid of the simplex vertices
00281 //  excepting the vertex with Y value YNEWLO.
00282 //
00283       for ( i = 0; i < n; i++ )
00284       {
00285         z = 0.0;
00286         for ( j = 0; j < nn; j++ )
00287         {
00288           z = z + p[i+j*n];
00289         }
00290         z = z - p[i+ihi*n];
00291         pbar[i] = z / dn;
00292       }
00293 //
00294 //  Reflection through the centroid.
00295 //
00296       for ( i = 0; i < n; i++ )
00297       {
00298         pstar[i] = pbar[i] + rcoeff * ( pbar[i] - p[i+ihi*n] );
00299       }
00300       ystar = FittingErrorPlane(valX, valY, valZ, number_points,pstar);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
00301       //FittingError(x_val,y_val,z_val,p2star,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( pstar );
00302       *icount = *icount + 1;
00303 //
00304 //  Successful reflection, so extension.
00305 //
00306       if ( ystar < ylo )
00307       {
00308         for ( i = 0; i < n; i++ )
00309         {
00310           p2star[i] = pbar[i] + ecoeff * ( pstar[i] - pbar[i] );
00311         }
00312         y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
00313         //FittingError(x_val,y_val,z_val,p2star,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( p2star );
00314         *icount = *icount + 1;
00315 //
00316 //  Check extension.
00317 //
00318         if ( ystar < y2star )
00319         {
00320           for ( i = 0; i < n; i++ )
00321           {
00322             p[i+ihi*n] = pstar[i];
00323           }
00324           y[ihi] = ystar;
00325         }
00326 //
00327 //  Retain extension or contraction.
00328 //
00329         else
00330         {
00331           for ( i = 0; i < n; i++ )
00332           {
00333             p[i+ihi*n] = p2star[i];
00334           }
00335           y[ihi] = y2star;
00336         }
00337       }
00338 //
00339 //  No extension.
00340 //
00341       else
00342       {
00343         l = 0;
00344         for ( i = 0; i < nn; i++ )
00345         {
00346           if ( ystar < y[i] )
00347           {
00348             l = l + 1;
00349           }
00350         }
00351
00352         if ( 1 < l )
00353         {
00354           for ( i = 0; i < n; i++ )
00355           {
00356             p[i+ihi*n] = pstar[i];
00357           }
00358           y[ihi] = ystar;
00359         }
00360 //
00361 //  Contraction on the Y(IHI) side of the centroid.
00362 //
00363         else if ( l == 0 )
00364         {
00365           for ( i = 0; i < n; i++ )
00366           {
00367             p2star[i] = pbar[i] + ccoeff * ( p[i+ihi*n] - pbar[i] );
00368           }
00369           y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
00370           //FittingError(x_val,y_val,z_val,p2star,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( p2star );
00371           *icount = *icount + 1;
00372 //
00373 //  Contract the whole simplex.
00374 //
00375           if ( y[ihi] < y2star )
00376           {
00377             for ( j = 0; j < nn; j++ )
00378             {
00379               for ( i = 0; i < n; i++ )
00380               {
00381                 p[i+j*n] = ( p[i+j*n] + p[i+ilo*n] ) * 0.5;
00382                 xmin[i] = p[i+j*n];
00383               }
00384               y[j] = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
00385               //FittingError(x_val,y_val,z_val,xmin,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( xmin );
00386               *icount = *icount + 1;
00387             }
00388             ylo = y[0];
00389             ilo = 0;
00390
00391             for ( i = 1; i < nn; i++ )
00392             {
00393               if ( y[i] < ylo )
00394               {
00395                 ylo = y[i];
00396                 ilo = i;
00397               }
00398             }
00399             continue;
00400           }
00401 //
00402 //  Retain contraction.
00403 //
00404           else
00405           {
00406             for ( i = 0; i < n; i++ )
00407             {
00408               p[i+ihi*n] = p2star[i];
00409             }
00410             y[ihi] = y2star;
00411           }
00412         }
00413 //
00414 //  Contraction on the reflection side of the centroid.
00415 //
00416         else if ( l == 1 )
00417         {
00418           for ( i = 0; i < n; i++ )
00419           {
00420             p2star[i] = pbar[i] + ccoeff * ( pstar[i] - pbar[i] );
00421           }
00422           y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
00423           //FittingError(x_val,y_val,z_val,p2star,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( p2star );
00424           *icount = *icount + 1;
00425 //
00426 //  Retain reflection?
00427 //
00428           if ( y2star <= ystar )
00429           {
00430             for ( i = 0; i < n; i++ )
00431             {
00432               p[i+ihi*n] = p2star[i];
00433             }
00434             y[ihi] = y2star;
00435           }
00436           else
00437           {
00438             for ( i = 0; i < n; i++ )
00439             {
00440               p[i+ihi*n] = pstar[i];
00441             }
00442             y[ihi] = ystar;
00443           }
00444         }
00445       }
00446 //
00447 //  Check if YLO improved.
00448 //
00449       if ( y[ihi] < ylo )
00450       {
00451         ylo = y[ihi];
00452         ilo = ihi;
00453       }
00454       jcount = jcount - 1;
00455
00456       if ( 0 < jcount )
00457       {
00458         continue;
00459       }
00460 //
00461 //  Check to see if minimum reached.
00462 //
00463       if ( *icount <= kcount )
00464       {
00465         jcount = konvge;
00466
00467         z = 0.0;
00468         for ( i = 0; i < nn; i++ )
00469         {
00470           z = z + y[i];
00471         }
00472         x = z / dnn;
00473
00474         z = 0.0;
00475         for ( i = 0; i < nn; i++ )
00476         {
00477           z = z + pow ( y[i] - x, 2 );
00478         }
00479
00480         if ( z <= rq )
00481         {
00482           break;
00483         }
00484       }
00485     }
00486 //
00487 //  Factorial tests to check that YNEWLO is a local minimum.
00488 //
00489     for ( i = 0; i < n; i++ )
00490     {
00491       xmin[i] = p[i+ilo*n];
00492     }
00493     *ynewlo = y[ilo];
00494
00495     if ( kcount < *icount )
00496     {
00497       *ifault = 2;
00498       break;
00499     }
00500
00501     *ifault = 0;
00502
00503     for ( i = 0; i < n; i++ )
00504     {
00505       del = step[i] * eps;
00506       xmin[i] = xmin[i] + del;
00507       z = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
00508       //FittingError(x_val,y_val,z_val,xmin,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( xmin );
00509       *icount = *icount + 1;
00510       if ( z < *ynewlo )
00511       {
00512         *ifault = 2;
00513         break;
00514       }
00515       xmin[i] = xmin[i] - del - del;
00516       z = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
00517       //FittingError(x_val,y_val,z_val,xmin,number_of_elements_in_segment,current_label_list, number_current_labels);//num_p,current_label);//fn ( xmin );
00518       *icount = *icount + 1;
00519       if ( z < *ynewlo )
00520       {
00521         *ifault = 2;
00522         break;
00523       }
00524       xmin[i] = xmin[i] + del;
00525     }
00526
00527     if ( *ifault == 0 )
00528     {
00529       break;
00530     }
00531 //
00532 //  Restart the procedure.
00533 //
00534     for ( i = 0; i < n; i++ )
00535     {
00536       start[i] = xmin[i];
00537     }
00538     del = eps;
00539     *numres = *numres + 1;
00540   }
00541   delete [] p;
00542   delete [] pstar;
00543   delete [] p2star;
00544   delete [] pbar;
00545   delete [] y;
00546
00547   return;
00548 }
00549
00550
00552
00553
00554 double * leaf_parameters(int ** clusters, image<rgb> *pc, int num_clusters, double * fit_score_list)
00555 {
00556   double * parameter_list = (double*)malloc(sizeof(double)*9);
00557   int width = pc->width();
00558   int height = pc->height();
00559
00561
00562   int label_select = 0;
00563   double best_fit = 1;
00564   double fit_value;
00565
00566   for (int j=0;j<num_clusters-1;j++)
00567   {
00568     fit_value = fit_score_list[j];
00569     if (fit_value<best_fit)
00570     {
00571       best_fit = fit_value;
00572       label_select = j+1;
00573     }
00574   }
00575
00576   printf("best_fit %f x_c \n", best_fit);
00577   double * coorX = (double*)malloc(sizeof(double));
00578   double * coorY = (double*)malloc(sizeof(double));
00579   double * coorZ = (double*)malloc(sizeof(double));
00580   int comp_size = 0;
00581   double meanX = 0;
00582   double meanY = 0;
00583   double mean_depth = 0;
00584   double depth_value;
00586
00587   for (int x=0;x<width;x++){
00588     for (int y=0;y<height;y++){
00589       if (clusters[y][x]==label_select){
00590         depth_value = (double)(imRef(pc, x, y).b);
00591         if (depth_value>0){
00593           coorX = (double*)realloc(coorX,(comp_size+1)*sizeof(double));
00594           coorX[comp_size] = (double)(imRef(pc, x, y).r);
00595
00596           coorY = (double*)realloc(coorY,(comp_size+1)*sizeof(double));
00597           coorY[comp_size] = (double)(imRef(pc, x, y).g);
00598
00599           coorZ = (double*)realloc(coorZ,(comp_size+1)*sizeof(double));
00600           coorZ[comp_size] = (double)(imRef(pc, x, y).b);
00601           mean_depth = mean_depth + (double)(imRef(pc, x, y).b);
00602
00603           comp_size = comp_size+1;
00604
00605           meanX = meanX + x;
00606           meanY = meanY + y;
00607         }
00608       }
00609     }
00610   }
00611
00612   meanX = meanX/(double)(comp_size);
00613   meanY = meanY/(double)(comp_size);
00614   mean_depth = mean_depth/(double)(comp_size);
00615
00617   int mx = round(meanX);
00618   int my = round(meanY);
00619   printf("mean position %d center_index_x \n", mx);
00620   printf("mean position %d center_index_y \n", my);
00621
00622   double x_c = 0;
00623   double y_c = 0;
00624   double z_c = 0;
00625   int j_count = 0;
00626   for (int i_x=mx-10;i_x<(mx+10);i_x++){
00627     for (int i_y=my-10;i_y<(my+10);i_y++){
00628       if ((i_x>=0)&&(i_x<width)&&(i_y>=0)&&(i_y<height)){
00629               double x_value = (double)(imRef(pc, i_x, i_y).r);
00630               double y_value = (double)(imRef(pc, i_x, i_y).g);
00631               depth_value = (double)(imRef(pc, i_x, i_y).b);
00632         if (depth_value>0 && x_value<500 && x_value>-500 && y_value<500 && y_value>-500 ){
00633           if (clusters[i_y][i_x]==label_select){
00634             x_c = x_c + (double)(imRef(pc, i_x, i_y).r);
00635                   y_c = y_c + (double)(imRef(pc, i_x, i_y).g);
00636                   z_c = z_c + (double)(imRef(pc, i_x, i_y).b);
00637                   j_count = j_count +1;
00638           }
00639               }
00640       }
00641     }
00642   }
00643
00644   if (j_count>0)
00645   {
00646     x_c = x_c/(double)(j_count);
00647     y_c = y_c/(double)(j_count);
00648     z_c = z_c/(double)(j_count);
00649   }
00650
00651   parameter_list[0] = x_c;
00652   parameter_list[1] = y_c;
00653   parameter_list[2] = z_c;
00654
00655
00657
00658
00659 double initial_parameters [3] = {-0.1,-0.1,mean_depth-5};//mean_depth};scale,trans_x,trans_y,angle
00660 double parameters_at_minimum [3] = {100,100,100};
00661 double min_fit_value [1] = {100};
00662 double required_min = 0.1;
00663 double initial_simplex_step [3] = {0.1,0.1,10};
00664 int konvergence_check = 100;
00665 int max_number_evaluations = 2000;
00666 int used_number_evaluations [1] = {0};
00667 int number_restarts [1] = {0};
00668 int error_detected [1] = {0};
00669 int parameter_num = 3;
00670 double length_normal;
00671
00672 nelmin_plane(parameter_num,initial_parameters, parameters_at_minimum, min_fit_value, required_min, initial_simplex_step, konvergence_check, max_number_evaluations,used_number_evaluations,number_restarts,error_detected,coorX,coorY,coorZ,comp_size);
00673   length_normal = sqrt(pow(parameters_at_minimum[0],2) + pow(parameters_at_minimum[1],2) +1);
00674   parameter_list[3] = parameters_at_minimum[0]/length_normal;
00675   parameter_list[4] = parameters_at_minimum[1]/length_normal;
00676   parameter_list[5] = 1/length_normal;//parameters_at_minimum[2];
00677   //parameter_list[5] = parameters_at_minimum[2]/length_normal;
00678   parameter_list[6] = label_select;
00679   parameter_list[7] = mx;
00680   parameter_list[8] = my;
00682
00683   free(coorX);
00684   free(coorY);
00685   free(coorZ);
00686
00687   return(parameter_list);
00688 }
00689
00690 #endif
```

zyonz_image_based_leaf_probing
Author(s): Sergi Foix
autogenerated on Fri Dec 6 2013 23:25:27