leaf-parameters.h

Go to the documentation of this file.

/*
Copyright (C) 2012 Babette Dellen

Algorithm to find the surface normal and center point of a segment. 

*/

#ifndef LEAF_PARAMETERS
#define LEAF_PARAMETERS
#include <iostream>
//#include "cv.h"
#include <cstdlib>
#include <image.h>
#include <misc.h>
#include <filter.h>
//#include "segment-image.h"
#include <vector>
#define PI 3.14159265
using std::vector;

double FittingErrorPlane(double * x_val, double * y_val, double * z_val, int num_p, double * param_cur)//int current_label)
{

double surf_out = 0;  


    for (int j=0;j<num_p; j++)
    {
    surf_out = surf_out +  pow((param_cur[0]*x_val[j] + param_cur[1]*y_val[j] + param_cur[2] - z_val[j]),2);
    } 

    surf_out = surf_out/((double)(num_p));
    return (surf_out);  
}

//****************************************************************************80

void nelmin_plane (int n, double * start, double * xmin, 
  double * ynewlo, double reqmin, double * step, int konvge, int kcount, 
  int *icount, int *numres, int *ifault,double * valX, double * valY,double * valZ, int number_points)
  
  
//****************************************************************************80
//
//  Purpose:
//
//    NELMIN minimizes a function using the Nelder-Mead algorithm.
//
//  Discussion:
//
//    This routine seeks the minimum value of a user-specified function.
//
//    Simplex function minimisation procedure due to Nelder+Mead(1965),
//    as implemented by O'Neill(1971, Appl.Statist. 20, 338-45), with
//    subsequent comments by Chambers+Ertel(1974, 23, 250-1), Benyon(1976,
//    25, 97) and Hill(1978, 27, 380-2)
//
//    The function to be minimized must be defined by a function of
//    the form
//
//      function fn ( x, f )
//      double fn
//      double x(*)
//
//    and the name of this subroutine must be declared EXTERNAL in the
//    calling routine and passed as the argument FN.
//
//    This routine does not include a termination test using the
//    fitting of a quadratic surface.
//
//  Licensing:
//
//    This code is distributed under the GNU LGPL license. 
//
//  Modified:
//
//    27 February 2008
//
//  Author:
//
//    Original FORTRAN77 version by R ONeill.
//    C++ version by John Burkardt.
//
//  Reference:
//
//    John Nelder, Roger Mead,
//    A simplex method for function minimization,
//    Computer Journal,
//    Volume 7, 1965, pages 308-313.
//
//    R ONeill,
//    Algorithm AS 47:
//    Function Minimization Using a Simplex Procedure,
//    Applied Statistics,
//    Volume 20, Number 3, 1971, pages 338-345.
//
//  Parameters:
//
//    Input, double FN ( double x[] ), the name of the routine which evaluates
//    the function to be minimized.
//
//    Input, int N, the number of variables.
//
//    Input/output, double START[N].  On input, a starting point
//    for the iteration.  On output, this data may have been overwritten.
//
//    Output, double XMIN[N], the coordinates of the point which
//    is estimated to minimize the function.
//
//    Output, double YNEWLO, the minimum value of the function.
//
//    Input, double REQMIN, the terminating limit for the variance
//    of function values.
//
//    Input, double STEP[N], determines the size and shape of the
//    initial simplex.  The relative magnitudes of its elements should reflect
//    the units of the variables.
//
//    Input, int KONVGE, the convergence check is carried out 
//    every KONVGE iterations.
//
//    Input, int KCOUNT, the maximum number of function 
//    evaluations.
//
//    Output, int *ICOUNT, the number of function evaluations 
//    used.
//
//    Output, int *NUMRES, the number of restarts.
//
//    Output, int *IFAULT, error indicator.
//    0, no errors detected.
//    1, REQMIN, N, or KONVGE has an illegal value.
//    2, iteration terminated because KCOUNT was exceeded without convergence.
//
{
  double ccoeff = 0.5;
  double del;
  double dn;
  double dnn;
  double ecoeff = 2.0;
  double eps = 0.001;
  int i;
  int ihi;
  int ilo;
  int j;
  int jcount;
  int l;
  int nn;
  double *p;
  double *p2star;
  double *pbar;
  double *pstar;
  double rcoeff = 1.0;
  double rq;
  double x;
  double *y;
  double y2star;
  double ylo;
  double ystar;
  double z;
  double * start_init;
//
//  Check the input parameters.
//
  if ( reqmin <= 0.0 )
  {
    *ifault = 1;
    return;
  }

  if ( n < 1 )
  {
    *ifault = 1;
    return;
  }

  if ( konvge < 1 )
  {
    *ifault = 1;
    return;
  }

  p = new double[n*(n+1)];
  pstar = new double[n];
  p2star = new double[n];
  pbar = new double[n];
  y = new double[n+1];
  //start_init = new double[n];
  
//   for ( i = 0; i < n; i++ )
//     { 
//       start_init[i] = start[i];
//     }
//   

  *icount = 0;
  *numres = 0;

  jcount = konvge; 
  dn = ( double ) ( n );
  nn = n + 1;
  dnn = ( double ) ( nn );
  del = 1.0;
  rq = reqmin * dn;
//
//  Initial or restarted loop.
//
  for ( ; ; )
  {
    for ( i = 0; i < n; i++ )
    { 
      p[i+n*n] = start[i];
    }
    y[n] = FittingErrorPlane(valX, valY, valZ, number_points,start);
    //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 );
    *icount = *icount + 1;

    for ( j = 0; j < n; j++ )
    {
      x = start[j];
      start[j] = start[j] + step[j] * del;
      for ( i = 0; i < n; i++ )
      {
//      if (i==j)
//      {start[i]=start_init[j] + step[j] * del;
//      p[i+j*n] =start_init[j] + step[j] * del;
//      }
//      else
//      {start[i]=start_init[i];
//      p[i+j*n]=start_init[i];
//      }
        
          
        p[i+j*n] = start[i];
      }
      y[j] = FittingErrorPlane(valX, valY, valZ, number_points,start);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,start);
      //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 );
      *icount = *icount + 1;
      start[j] = x;
    }
//                    
//  The simplex construction is complete.
//                    
//  Find highest and lowest Y values.  YNEWLO = Y(IHI) indicates
//  the vertex of the simplex to be replaced.
//                
    ylo = y[0];
    ilo = 0;

    for ( i = 1; i < nn; i++ )
    {
      if ( y[i] < ylo )
      {
        ylo = y[i];
        ilo = i;
      }
    }
//
//  Inner loop.
//
    for ( ; ; )
    {
      if ( kcount <= *icount )
      {
        break;
      }
      *ynewlo = y[0];
      ihi = 0;

      for ( i = 1; i < nn; i++ )
      {
        if ( *ynewlo < y[i] )
        {
          *ynewlo = y[i];
          ihi = i;
        }
      }
//
//  Calculate PBAR, the centroid of the simplex vertices
//  excepting the vertex with Y value YNEWLO.
//
      for ( i = 0; i < n; i++ )
      {
        z = 0.0;
        for ( j = 0; j < nn; j++ )
        { 
          z = z + p[i+j*n];
        }
        z = z - p[i+ihi*n];  
        pbar[i] = z / dn;
      }
//
//  Reflection through the centroid.
//
      for ( i = 0; i < n; i++ )
      {
        pstar[i] = pbar[i] + rcoeff * ( pbar[i] - p[i+ihi*n] );
      }
      ystar = FittingErrorPlane(valX, valY, valZ, number_points,pstar);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
      //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 );
      *icount = *icount + 1;
//
//  Successful reflection, so extension.
//
      if ( ystar < ylo )
      {
        for ( i = 0; i < n; i++ )
        {
          p2star[i] = pbar[i] + ecoeff * ( pstar[i] - pbar[i] );
        }
        y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
        //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 );
        *icount = *icount + 1;
//
//  Check extension.
//
        if ( ystar < y2star )
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = pstar[i];
          }
          y[ihi] = ystar;
        }
//
//  Retain extension or contraction.
//
        else
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = p2star[i];
          }
          y[ihi] = y2star;
        }
      }
//
//  No extension.
//
      else
      {
        l = 0;
        for ( i = 0; i < nn; i++ )
        {
          if ( ystar < y[i] )
          {
            l = l + 1;
          }
        }

        if ( 1 < l )
        {
          for ( i = 0; i < n; i++ )
          {
            p[i+ihi*n] = pstar[i];
          }
          y[ihi] = ystar;
        }
//
//  Contraction on the Y(IHI) side of the centroid.
//
        else if ( l == 0 )
        {
          for ( i = 0; i < n; i++ )
          {
            p2star[i] = pbar[i] + ccoeff * ( p[i+ihi*n] - pbar[i] );
          }
          y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
          //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 );
          *icount = *icount + 1;
//
//  Contract the whole simplex.
//
          if ( y[ihi] < y2star )
          {
            for ( j = 0; j < nn; j++ )
            {
              for ( i = 0; i < n; i++ )
              {
                p[i+j*n] = ( p[i+j*n] + p[i+ilo*n] ) * 0.5;
                xmin[i] = p[i+j*n];
              }
              y[j] = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
              //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 );
              *icount = *icount + 1;
            }
            ylo = y[0];
            ilo = 0;

            for ( i = 1; i < nn; i++ )
            {
              if ( y[i] < ylo )
              {
                ylo = y[i];
                ilo = i;
              }
            }
            continue;
          }
//
//  Retain contraction.
//
          else
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = p2star[i];
            }
            y[ihi] = y2star;
          }
        }
//
//  Contraction on the reflection side of the centroid.
//
        else if ( l == 1 )
        {
          for ( i = 0; i < n; i++ )
          {
            p2star[i] = pbar[i] + ccoeff * ( pstar[i] - pbar[i] );
          }
          y2star = FittingErrorPlane(valX, valY, valZ, number_points,p2star);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,p2star);
          //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 );
          *icount = *icount + 1;
//
//  Retain reflection?
//
          if ( y2star <= ystar )
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = p2star[i];
            }
            y[ihi] = y2star;
          }
          else
          {
            for ( i = 0; i < n; i++ )
            {
              p[i+ihi*n] = pstar[i];
            }
            y[ihi] = ystar;
          }
        }
      }
//
//  Check if YLO improved.
//
      if ( y[ihi] < ylo )
      {
        ylo = y[ihi];
        ilo = ihi;
      }
      jcount = jcount - 1;

      if ( 0 < jcount )
      {
        continue;
      }
//
//  Check to see if minimum reached.
//
      if ( *icount <= kcount )
      {
        jcount = konvge;

        z = 0.0;
        for ( i = 0; i < nn; i++ )
        {
          z = z + y[i];
        }
        x = z / dnn;

        z = 0.0;
        for ( i = 0; i < nn; i++ )
        {
          z = z + pow ( y[i] - x, 2 );
        }

        if ( z <= rq )
        {
          break;
        }
      }
    }
//
//  Factorial tests to check that YNEWLO is a local minimum.
//
    for ( i = 0; i < n; i++ )
    {
      xmin[i] = p[i+ilo*n];
    }
    *ynewlo = y[ilo];

    if ( kcount < *icount )
    {
      *ifault = 2;
      break;
    }

    *ifault = 0;

    for ( i = 0; i < n; i++ )
    {
      del = step[i] * eps;
      xmin[i] = xmin[i] + del;
      z = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
      //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 );
      *icount = *icount + 1;
      if ( z < *ynewlo )
      {
        *ifault = 2;
        break;
      }
      xmin[i] = xmin[i] - del - del;
      z = FittingErrorPlane(valX, valY, valZ, number_points,xmin);//ContourFittingError(contour_pointX, contour_pointY,number_contour_points,model,xmin);
      //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 );
      *icount = *icount + 1;
      if ( z < *ynewlo )
      {
        *ifault = 2;
        break;
      }
      xmin[i] = xmin[i] + del;
    }

    if ( *ifault == 0 )
    {
      break;
    }
//
//  Restart the procedure.
//
    for ( i = 0; i < n; i++ )
    {
      start[i] = xmin[i];
    }
    del = eps;
    *numres = *numres + 1;
  }
  delete [] p;
  delete [] pstar;
  delete [] p2star;
  delete [] pbar;
  delete [] y;

  return;
}




double * leaf_parameters(int ** clusters, image<rgb> *pc, int num_clusters, double * fit_score_list) 
{
  double * parameter_list = (double*)malloc(sizeof(double)*9); 
  int width = pc->width();
  int height = pc->height();
  
  
  int label_select = 0;
  double best_fit = 1;
  double fit_value;
  
  for (int j=0;j<num_clusters-1;j++)
  {
    fit_value = fit_score_list[j];
    if (fit_value<best_fit)
    {
      best_fit = fit_value;
      label_select = j+1;
    }
  }

  printf("best_fit %f x_c \n", best_fit);
  double * coorX = (double*)malloc(sizeof(double));
  double * coorY = (double*)malloc(sizeof(double));
  double * coorZ = (double*)malloc(sizeof(double));
  int comp_size = 0;
  double meanX = 0;
  double meanY = 0;
  double mean_depth = 0;
  double depth_value;
  
  for (int x=0;x<width;x++){
    for (int y=0;y<height;y++){
      if (clusters[y][x]==label_select){
        depth_value = (double)(imRef(pc, x, y).b);
        if (depth_value>0){
          // add component
          coorX = (double*)realloc(coorX,(comp_size+1)*sizeof(double));
          coorX[comp_size] = (double)(imRef(pc, x, y).r);
          
          coorY = (double*)realloc(coorY,(comp_size+1)*sizeof(double));
          coorY[comp_size] = (double)(imRef(pc, x, y).g);
          
          coorZ = (double*)realloc(coorZ,(comp_size+1)*sizeof(double));
          coorZ[comp_size] = (double)(imRef(pc, x, y).b);
          mean_depth = mean_depth + (double)(imRef(pc, x, y).b);
          
          comp_size = comp_size+1;
          
          meanX = meanX + x;
          meanY = meanY + y;
        }
      }
    }
  }
      
  meanX = meanX/(double)(comp_size);
  meanY = meanY/(double)(comp_size);
  mean_depth = mean_depth/(double)(comp_size);
  
  int mx = round(meanX);
  int my = round(meanY);
  printf("mean position %d center_index_x \n", mx);
  printf("mean position %d center_index_y \n", my);
  
  double x_c = 0;
  double y_c = 0;
  double z_c = 0;
  int j_count = 0;
  for (int i_x=mx-10;i_x<(mx+10);i_x++){
    for (int i_y=my-10;i_y<(my+10);i_y++){
      if ((i_x>=0)&&(i_x<width)&&(i_y>=0)&&(i_y<height)){
              double x_value = (double)(imRef(pc, i_x, i_y).r);
              double y_value = (double)(imRef(pc, i_x, i_y).g);
              depth_value = (double)(imRef(pc, i_x, i_y).b);
        if (depth_value>0 && x_value<500 && x_value>-500 && y_value<500 && y_value>-500 ){
          if (clusters[i_y][i_x]==label_select){
            x_c = x_c + (double)(imRef(pc, i_x, i_y).r);
                  y_c = y_c + (double)(imRef(pc, i_x, i_y).g);
                  z_c = z_c + (double)(imRef(pc, i_x, i_y).b);
                  j_count = j_count +1;
          }
              }
      }
    }
  }
  
  if (j_count>0)
  {
    x_c = x_c/(double)(j_count);
    y_c = y_c/(double)(j_count);
    z_c = z_c/(double)(j_count);
  }
  
  parameter_list[0] = x_c;
  parameter_list[1] = y_c;
  parameter_list[2] = z_c;
  
  


double initial_parameters [3] = {-0.1,-0.1,mean_depth-5};//mean_depth};scale,trans_x,trans_y,angle
double parameters_at_minimum [3] = {100,100,100};
double min_fit_value [1] = {100}; 
double required_min = 0.1;
double initial_simplex_step [3] = {0.1,0.1,10};
int konvergence_check = 100;
int max_number_evaluations = 2000;
int used_number_evaluations [1] = {0};
int number_restarts [1] = {0};
int error_detected [1] = {0};
int parameter_num = 3;  
double length_normal;

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);
  length_normal = sqrt(pow(parameters_at_minimum[0],2) + pow(parameters_at_minimum[1],2) +1);
  parameter_list[3] = parameters_at_minimum[0]/length_normal;
  parameter_list[4] = parameters_at_minimum[1]/length_normal;
  parameter_list[5] = 1/length_normal;//parameters_at_minimum[2];
  //parameter_list[5] = parameters_at_minimum[2]/length_normal;
  parameter_list[6] = label_select;
  parameter_list[7] = mx;
  parameter_list[8] = my;
  
  free(coorX);
  free(coorY);
  free(coorZ);
  
  return(parameter_list);
}

#endif