segment-image.h

Go to the documentation of this file.

/*
Copyright (C) 2012 Babette Dellen 
(except for color segmentation and Nelder Mead function minimization parts)

Algorithm for depth segmentation of ToF data and surface model fitting

*/

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

// random color
// rgb random_rgb(uchar in_value){ 
//   rgb c;
//   double r;
//   
//   c.r = in_value;//(uchar)random();
//   c.g = in_value;//(uchar)random();
//   c.b = in_value;//(uchar)random();
// 
//   return c;
// }

rgb random_rgb(){ 
  rgb c;
  double r;
  
  c.r = (uchar)random();
  c.g = (uchar)random();
  c.b = (uchar)random();

  return c;
}



// dissimilarity measure between pixels
static inline float diff(image<float> *r, image<float> *g, image<float> *b,
                         int x1, int y1, int x2, int y2) {
  return sqrt(square(imRef(r, x1, y1)-imRef(r, x2, y2)) +
              square(imRef(g, x1, y1)-imRef(g, x2, y2)) +
              square(imRef(b, x1, y1)-imRef(b, x2, y2)));
}





double *** setArray3D (image<rgb> *image_input)
{
    //allocate memory:, 
    double *** Array3D;
    
     int n_set = image_input->width();
     int m_set = image_input->height();
     int t_set = 3;
    
    
    Array3D=(double***)malloc(sizeof(double**)*m_set);
    
    
    
     
        for (int n=0; n<m_set; n++)
        {
                Array3D[n] = (double**)malloc(sizeof(double*)*n_set);
        for (int m=0; m<n_set; m++)
        {
        Array3D[n][m] = (double*)malloc(sizeof(double)*t_set);
        }
                
        }
    
    
//     for(int t=0;t<t_set;t++)
//             {
              
    for(int i=0;i<n_set;i++)
    {
         
        for(int j=0;j<m_set;j++)
        {
           
           Array3D[j][i][0]=imRef(image_input, i, j).r;//a_setSequence[(i*m_set + m_set*n_set*t +j)];
           Array3D[j][i][1]=imRef(image_input, i, j).g;
           Array3D[j][i][2]=imRef(image_input, i, j).b;
          }
            
        }
            
   //}
    
    
    return (Array3D);
}


void BubbleSort(vector<double> *in_values, vector<int> *indices)
{

//unsigned int vector_length = in_values.size();
int swap_cond = 1;
unsigned int j;
unsigned int j_a;
int swap_count;
double value_1;
double value_2;
int vector_length = in_values->size();
int index_1;
int index_2;


for (j_a=0;j_a<in_values->size();j_a++)
    {
    
    indices->push_back(j_a);
    }


  while (swap_cond>0)
  {  
    swap_count = 0;
    for (j=0;j<in_values->size()-1;j++)
    {
      value_1 = in_values->at(j);
      index_1 = indices->at(j);
      value_2 = in_values->at(j+1);
      index_2 = indices->at(j+1);
      if (value_2<value_1)
      {//swap
      in_values->at(j) = value_2;
      in_values->at(j+1) = value_1;
      indices->at(j) = index_2;
      indices->at(j+1) = index_1;
      swap_count = 1;
      }
      
    }
    
    if (swap_count==0)
    {swap_cond = 0;}
  }  
  //return;
}  

void ReArrange (vector<int> *label_values_sort,vector<int> *label_values, vector<int> *indices)
{
int index_new;
int vector_length = indices->size();
label_values_sort->assign (vector_length,0);
  int j_a;
  for (j_a=0;j_a<vector_length;j_a++)
    {
    index_new =  indices->at(j_a);
    //label_values_sort->at(index_new) = label_values->at(j_a);
    label_values_sort->at(j_a) = label_values->at(index_new);
    }
  
//return;
}
//comp_size_list_new,label_list[label_1],number_of_labels_in_list[label_1])
double FittingError(double ** x_val, double ** y_val, double ** z_val, double * param_cur, int * components_elements_number, int * list_of_labels, int num_labels_in_list)//int current_label)
{
int num_p;
int current_label;
int total_number_values=0;
int j_current;
double surf_out = 0;  

// a=num_p[0], b=num_p[1], c=num_p[2], d=num_p[3], e=num_p[4]
  // surf_out = |a*x_val^2 + b*y_val^2 + c*x_val + d*y_val + e - z_val|^2 
for (j_current=0;j_current<num_labels_in_list;j_current++)
    {  
      
    current_label = list_of_labels[j_current];
    num_p = components_elements_number[current_label];
    for (int j=0;j<num_p; j++)
    {
      
//      if (x_val[current_label][j]>194)
//      {
//              printf("WARNING!!!");
//              printf("WARNING!!!");
//              printf("WARNING!!!");
//              printf("%8d\n",current_label);
//              printf("%8d\n",j);
//              printf("%8f\n",x_val[current_label][j]);
//      }
    surf_out = surf_out +  pow((param_cur[0]*pow(x_val[current_label][j],2) + param_cur[1]*pow(y_val[current_label][j],2) + param_cur[2]*x_val[current_label][j] + param_cur[3]*y_val[current_label][j] + param_cur[4] - z_val[current_label][j]),2);
    } 

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

double FittedDepthValue(double x_val, double y_val, double z_val, double ** param_models,int label)
{
  
double depth_current = 0;  
// a=num_p[0], b=num_p[1], c=num_p[2], d=num_p[3], e=num_p[4]
  // surf_out = |a*x_val^2 + b*y_val^2 + c*x_val + d*y_val + e - z_val|^2 
  
depth_current  = param_models[label][0]*pow(x_val,2) + param_models[label][1]*pow(y_val,2) + param_models[label][2]*x_val + param_models[label][3]*y_val+ param_models[label][4];

return (depth_current);  
  
}




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

void nelmin (int n, double * start, double * xmin, 
  double * ynewlo, double reqmin, double * step, int konvge, int kcount, 
  int *icount, int *numres, int *ifault,double ** x_val, double ** y_val, double ** z_val, int * number_of_elements_in_segment, int * current_label_list, int number_current_labels)

//****************************************************************************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] = 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] = 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 = FittingError(x_val,y_val,z_val,pstar,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 = 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 = 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] = 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 = 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 = 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 = 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;
}



/* Color segmentation: 
Copyright (C) 2006 Pedro Felzenszwalb

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
*/
  
  
image<rgb> *segment_image(image<rgb> *im, image<rgb> *pc, float sigma, float c, int min_size,
                          int *num_ccs, int *num_ccs_fin, int **& ClusterArray) {
  int width = im->width();
  int height = im->height();

  image<float> *r = new image<float>(width, height);
  image<float> *g = new image<float>(width, height);
  image<float> *b = new image<float>(width, height);

  // smooth each color channel  
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      imRef(r, x, y) = imRef(im, x, y).r;
      imRef(g, x, y) = imRef(im, x, y).g;
      imRef(b, x, y) = imRef(im, x, y).b;
    }
  }
  image<float> *smooth_r = smooth(r, sigma);
  image<float> *smooth_g = smooth(g, sigma);
  image<float> *smooth_b = smooth(b, sigma);
  delete r;
  delete g;
  delete b;
 
  // build graph
  edge *edges = new edge[width*height*4];
  int num = 0;
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      if (x < width-1) {
        edges[num].a = y * width + x;
        edges[num].b = y * width + (x+1);
        edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y);
        num++;
      }

      if (y < height-1) {
        edges[num].a = y * width + x;
        edges[num].b = (y+1) * width + x;
        edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x, y+1);
        num++;
      }

      if ((x < width-1) && (y < height-1)) {
        edges[num].a = y * width + x;
        edges[num].b = (y+1) * width + (x+1);
        edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y+1);
        num++;
      }

      if ((x < width-1) && (y > 0)) {
        edges[num].a = y * width + x;
        edges[num].b = (y-1) * width + (x+1);
        edges[num].w = diff(smooth_r, smooth_g, smooth_b, x, y, x+1, y-1);
        num++;
      }
    }
  }
  delete smooth_r;
  delete smooth_g;
  delete smooth_b;
  
  // segment
  universe *u = segment_graph(width*height, num, edges, c);
  
  // post process small components
  for (int i = 0; i < num; i++) {
    int a = u->find(edges[i].a);
    int b = u->find(edges[i].b);
    if ((a != b) && ((u->size(a) < min_size) || (u->size(b) < min_size)))
      u->join(a, b);
  }
  delete [] edges;
  *num_ccs = u->num_sets();

  
 //int ** ClusterArray;
 
  ClusterArray=(int**)malloc(sizeof(int*)*height);
    
        for (int k=0; k<height; k++){
                ClusterArray[k] = (int*)malloc(sizeof(int)*width);
        }
   
  int * comp_label_list; 
  comp_label_list =  (int*)malloc(sizeof(int)*height*width);
 
  for (int j = 0; j<(height*width); j++){
    comp_label_list[j] = 0;
  }
  
  int j_count = 1;
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      int comp = u->find(y * width + x);
      if (comp_label_list[comp]==0){
        comp_label_list[comp]=j_count;
        ClusterArray[y][x] = j_count;
        //printf ("%8d\n",j_count);
        //printf ("%8d\n",j_count);
        j_count = j_count +1;
      } else if (comp_label_list[comp]>0){
        ClusterArray[y][x] = comp_label_list[comp];
        //printf ("%8d\n",comp_label_list[comp]);
      }
    }
  }  
  
 /* 
 double *** PointCloud;
 
 PointCloud = setArray3D(pc);*/
  
  

double ** ComponentListX;
double ** ComponentListY;
double ** ComponentListZ;

ComponentListX = (double**)malloc(sizeof(double*)*j_count);
ComponentListY = (double**)malloc(sizeof(double*)*j_count);
ComponentListZ = (double**)malloc(sizeof(double*)*j_count);

for (int k=0; k<j_count; k++){
  ComponentListX[k] = (double*)malloc(sizeof(double));
        ComponentListY[k] = (double*)malloc(sizeof(double));
        ComponentListZ[k] = (double*)malloc(sizeof(double));
}


int * comp_size_list; 
double * comp_depth_list; 
comp_size_list =  (int*)malloc(sizeof(int)*j_count);
comp_depth_list =  (double*)malloc(sizeof(double)*j_count);
for (int j = 0; j<j_count; j++){
  comp_size_list[j] = 0;
  comp_depth_list[j] = 0;
}
  
// 
// 

int size_thres = 20;
int label;
int comp_size;
double comp_depth;
double depth_value;
for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      depth_value = (double)(imRef(pc, x, y).b);
      if (depth_value>0){
      label = ClusterArray[y][x];
      
      comp_size = comp_size_list[label-1];
      comp_depth = comp_depth_list[label-1];
      ComponentListX[label-1] = (double*) realloc(ComponentListX[label-1],(comp_size+1)*sizeof(double));
      ComponentListX[label-1][comp_size]= (double)(imRef(pc, x, y).r);
      
      ComponentListY[label-1] = (double*) realloc(ComponentListY[label-1],(comp_size+1)*sizeof(double));
      ComponentListY[label-1][comp_size]= (double)(imRef(pc, x, y).g);
      
      ComponentListZ[label-1] = (double*) realloc(ComponentListZ[label-1],(comp_size+1)*sizeof(double));
      ComponentListZ[label-1][comp_size]= (double)(imRef(pc, x, y).b);
      
      comp_size_list[label-1] = comp_size + 1;
      comp_depth_list[label-1] = comp_depth + (double)(imRef(pc, x, y).b);
      }
    }
}


double ** SurfaceModelsParameters;
double ** SurfaceModelsParameters_new;
;
SurfaceModelsParameters= (double**)malloc(sizeof(double*)*j_count);
SurfaceModelsParameters_new= (double**)malloc(sizeof(double*)*j_count);


for (int k=0; k<j_count; k++)
        {
                SurfaceModelsParameters[k] = (double*)malloc(sizeof(double)*5);
                SurfaceModelsParameters_new[k] = (double*)malloc(sizeof(double)*5);
                
        }


double * ErrorFit; 
ErrorFit = (double*)malloc(sizeof(double)*j_count);;
double * ErrorFit_new; 
ErrorFit_new = (double*)malloc(sizeof(double)*j_count);;


double mean_depth;
int list_of_labels_a [1] = {0};


double initial_parameters_quadric [5] = {0,0,-0.05,-0.05,0};//mean_depth};
double parameters_at_minimum [5] = {0,0,0,0,0};
double min_fit_value [1] = {100}; 
double required_min = 0.1;
double initial_simplex_step [5] = {0.01,0.01,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_quadric = 5;  


for (int label_j=0;label_j<j_count-1;label_j++){
  mean_depth = comp_depth_list[label_j]/((double)(comp_size_list[label_j]));

  initial_parameters_quadric[0]=0;
  initial_parameters_quadric[1]=0;
  initial_parameters_quadric[2]=-0.05;
  initial_parameters_quadric[3]=-0.05;
  initial_parameters_quadric[4]=mean_depth-5;

  list_of_labels_a[0] = label_j;
  // double fit_error;
  // int label_test = j_count-2;//j_count -1;
  // 
  // fit_error = FittingError(ComponentListX, ComponentListY, ComponentListZ, initial_parameters_quadric,parameter_quadric,label_test);
  
  // printf ("%8d\n",comp_size_list[label_j]);
  if (comp_size_list[label_j]>size_thres){
    nelmin (parameter_quadric,initial_parameters_quadric, parameters_at_minimum, min_fit_value, required_min, initial_simplex_step, konvergence_check, max_number_evaluations, used_number_evaluations,number_restarts, error_detected,ComponentListX, ComponentListY, ComponentListZ, comp_size_list,list_of_labels_a,1);//comp_size_list[label_j],label_j);

  //printf ("%8f\n",round(min_fit_value[0]));

    for (int k_a=0; k_a<5; k_a++){
      SurfaceModelsParameters[label_j][k_a] = parameters_at_minimum[k_a];
        SurfaceModelsParameters_new[label_j][k_a] = parameters_at_minimum[k_a];
    }
    ErrorFit[label_j] = min_fit_value[0];
    ErrorFit_new[label_j] = min_fit_value[0];
  } else {
    for (int k_a=0; k_a<5; k_a++){
      SurfaceModelsParameters[label_j][k_a] = 0;//parameters_at_minimum[k_a];
                  SurfaceModelsParameters_new[label_j][k_a] = 0;//parameters_at_minimum[k_a];
                }
  }

  ErrorFit[label_j] = 100;//min_fit_value[0];
  ErrorFit_new[label_j] = 100;//min_fit_value[0];
}



/*
double ** FittedDepthArray;
 
   FittedDepthArray=(double**)malloc(sizeof(double*)*height);
    
        for (int k=0; k<height; k++)
        {
                 FittedDepthArray[k] = (double*)malloc(sizeof(double)*width);
        }
*/


image<rgb> *fitted_depth = new image<rgb>(width, height);
 
double x_val;
double y_val;
double z_val;
     
int cluster_label;
 for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      x_val = (double)(imRef(pc, x, y).r);
      y_val = (double)(imRef(pc, x, y).g);
      z_val = (double)(imRef(pc, x, y).b);
      
     
     cluster_label = ClusterArray[y][x]-1;
     
    // FittedDepthArray[y][x] = FittedDepthValue(x_val, y_val,z_val,SurfaceModelsParameters,cluster_label);
    imRef(fitted_depth, x, y).r = (uchar)(x_val);
    imRef(fitted_depth, x, y).g = (uchar)(y_val);
    imRef(fitted_depth, x, y).b = (uchar)(FittedDepthValue(x_val,y_val,z_val,SurfaceModelsParameters,cluster_label));//FittepdDepthValue;//(uchar)(comp_depth_list[cluster_label]/(double)(comp_size_list[cluster_label]));//
     
    }
  }  


//printf ("Surfaces Fitted to Segments!");
int is_boundary;

//int is_neighbor;

int ** NeighborGraph;

int delta = 4;
double depth;
double depth_neighbor;
double depth_dist;
double depth_thres = 5;
int label_neighbor;
int k_1;
int k_2;
  NeighborGraph=(int**)malloc(sizeof(int*)*j_count);
    
        for (int k=0; k<j_count; k++)
        {
                NeighborGraph[k] = (int*)malloc(sizeof(int)*j_count);
                
                
        }


for (k_1=0; k_1<j_count; k_1++)
        {
          for (k_2=0; k_2<j_count; k_2++)
        
          
          {
        NeighborGraph[k_1][k_2]=0;
        }
        
        }
          
          

int y;
int x;
int j_y;
int j_x;
for (y = 0; y < height; y++) {
  for (x = 0; x < width; x++) {
    is_boundary = 0;
    label = ClusterArray[y][x]-1;
    if (comp_size_list[label]>size_thres){
      depth = (double)(imRef(fitted_depth, x, y).b);
      //is_neighbor = 0;
      // check if boundary point      
      for (j_y = y-1; j_y<y+2; j_y++) {
        for (j_x = x-1; j_x<x+2; j_x++) {
          if ((j_y>=0)&&(j_x>=0)&&(j_x<width)&&(j_y<height)){
              label_neighbor = ClusterArray[j_y][j_x]-1;
            //printf ("%8d\n",j_x);
            if (label!=label_neighbor){ 
              is_boundary = 1;
            }
          }  
        }
      }
    }
    if (is_boundary==1){
      for (j_y = y-delta; j_y< y+1+delta; j_y++) {
        for (j_x = x-delta; j_x < x+1+delta; j_x++) {
          if ((j_y>=0)&&(j_x>=0)&&(j_x<width)&&(j_y<height)){
                  //check if neighbor
                              label_neighbor = ClusterArray[j_y][j_x]-1;
                  if (comp_size_list[label_neighbor]>size_thres){
            //printf ("%8d\n",j_x);
                  //if (label_neighbor>label){
                    depth_neighbor = (double)(imRef(fitted_depth, j_x, j_y).b);
                    depth_dist = abs(depth - depth_neighbor);
                    if ((label!=label_neighbor)&&(depth_dist<depth_thres)){
                NeighborGraph[label][label_neighbor]=1;
                      NeighborGraph[label_neighbor][label]=1;
                            //add labels to label vector
                //NeighborGraph[label-1][label_neighbor-1] = 1;
                      //is_neighbor = is_neighbor + 1;
                    }
            }
          }
        }
      }
    }
  }
}

//printf ("Segment Graph Constructed!");     

vector<double> graph_links;
vector<int> graph_label_1;
vector<int> graph_label_2;
double e_fit_12;
double e_fit_21;
double e_fit_min;
double param_fit[5] = {0,0,0,0,0};
int p_c;
int label_dummy;

for (k_1=0; k_1<j_count; k_1++) {
  for (k_2=0; k_2<j_count; k_2++){
          if (k_1<k_2){
                  if (NeighborGraph[k_1][k_2]==1){
                    //compute fitting error
                    for (p_c=0;p_c<5;p_c++){
                      param_fit[p_c]= SurfaceModelsParameters[k_1][p_c];
                    }
        //e_fit_12 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,param_fit,comp_size_list[label-1],label_dummy);//label_neighbor-1);
                    list_of_labels_a[0] = k_2;//label_neighbor-1;
                    
                    e_fit_12 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters[k_1],comp_size_list,list_of_labels_a,1);
                    list_of_labels_a[0] =k_1;//label-1;
        e_fit_21 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters[k_2],comp_size_list,list_of_labels_a,1);
        //e_fit_12 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters[label-1],comp_size_list[label-1],label_neighbor-1);
        //e_fit_21 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters[label_neighbor-1],comp_size_list[label_neighbor-1],label-1);
        if (e_fit_12<e_fit_21){
          e_fit_min = e_fit_12;
        } else {
          e_fit_min = e_fit_21;
        }
        //add error to link vector                
                    graph_links.push_back (e_fit_min);
                    graph_label_1.push_back (k_1);
                    graph_label_2.push_back (k_2);
                        }
          }
        }
}



// sort graph links in ascending order      
      
 vector<int> graph_index;
 vector<int> graph_label_1_sort;
 vector<int> graph_label_2_sort;
 
 BubbleSort(&graph_links,&graph_index);
 ReArrange(&graph_label_1_sort,&graph_label_1,&graph_index);
 ReArrange(&graph_label_2_sort,&graph_label_2,&graph_index);
      

//printf ("Links Sorted!");     

// ////// Do graph-based merging


int * labels_new;
labels_new = (int*)malloc(sizeof(int)*j_count);

int * comp_size_list_new;
comp_size_list_new = (int*)malloc(sizeof(int)*j_count);

double * comp_depth_list_new;
comp_depth_list_new = (double*)malloc(sizeof(double)*j_count);


for (int label_j=0;label_j<j_count-1;label_j++)
{labels_new[label_j]=label_j;
comp_size_list_new[label_j]=comp_size_list[label_j];//label_j;
comp_depth_list_new[label_j]=comp_depth_list[label_j];
}

int ** label_list;
label_list = (int**)malloc(sizeof(int*)*j_count);

int * number_of_labels_in_list;
number_of_labels_in_list = (int*)malloc(sizeof(int)*j_count);
for (int k_b=0; k_b<j_count; k_b++){
  label_list[k_b] = (int*)malloc(sizeof(int));
}

for (int label_j=0;label_j<j_count-1;label_j++){
  label_list[label_j][0]=label_j;
  number_of_labels_in_list[label_j] = 1;
}

// int aa** = (int**) new int*[10];
// for (i=0..9)
//   aa[i]=(int*) new int[100];
// 
// for (i=0..9)
//   delete [] aa[i];
// delete [] aa;


int u_1;
int u_2;
int label_1;
int label_2;
int error_1;
int error_2;
int size_1;
int size_2;
int label_1_total_size;
int label_2_total_size;
double thres_merge = 15;
int number_merged_labels;
double min_error;
int number_of_links = graph_links.size();
int label_keep;
int label_replace;


for (int link_index=0;link_index<number_of_links;link_index++)
{
u_1 = graph_label_1_sort.at(link_index);
u_2 = graph_label_2_sort.at(link_index);

label_1=labels_new[u_1];
label_2=labels_new[u_2];

error_1 = ErrorFit_new[label_1];
error_2 = ErrorFit_new[label_2];

size_1 = comp_size_list_new[label_1];
size_2 = comp_size_list_new[label_2];

//printf ("%8f\n",(graph_links.at(link_index)));
// printf ("%8d\n",(graph_label_1_sort.at(link_index)));
// printf ("%8d\n",(graph_label_2_sort.at(link_index)));


if ((size_1>size_thres)&&(size_2>size_thres)&&(label_1!=label_2))
{
// printf("Test!");
// 
// printf("%8d\n",label_1);
// 
// printf("%8d\n",number_of_labels_in_list[label_1]);
//   
// for (int j_test=0;j_test<number_of_labels_in_list[label_1];j_test++)
// {
// printf("%8d\n",label_list[label_1][j_test]);
// 
// }
// for (int i_test=0;i_test<number_of_labels_in_list[label_1];i_test++)
// {
// 
// printf("%8d\n",comp_size_list_new[label_list[label_1][i_test]]);
// }
  /*
for (int u_test=0;u_test<5;u_test++)
{
printf("Models!");
printf("%8f\n",SurfaceModelsParameters_new[label_2][u_test]);
}*/
  
e_fit_12 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters_new[label_1],comp_size_list,label_list[label_2],number_of_labels_in_list[label_2]);    
e_fit_21 =FittingError(ComponentListX,ComponentListY,ComponentListZ,SurfaceModelsParameters_new[label_2],comp_size_list,label_list[label_1],number_of_labels_in_list[label_1]);//list_of_labels_a,1);

//list_of_labels_a[0] = k_2;//label_neighbor-1;
                  
//                e_fit_12 =  FittingError(ComponentListX, ComponentListY, ComponentListZ,SurfaceModelsParameters[k_1],comp_size_list,list_of_labels_a,1);


min_error = e_fit_12;
if (e_fit_21<e_fit_12)
{min_error = e_fit_21;}

// printf("MinError!");
// printf ("%8f\n",(min_error));


if (min_error<thres_merge)
{
  
// merged label list
number_merged_labels = number_of_labels_in_list[label_1]+number_of_labels_in_list[label_2];
int * merged_labels = (int*)malloc(sizeof(int)*number_merged_labels);
 
for (int j_merge_1=0;j_merge_1<number_of_labels_in_list[label_1];j_merge_1++)
{
merged_labels[j_merge_1]= label_list[label_1][j_merge_1]; 
}

for (int j_merge_2=0;j_merge_2<number_of_labels_in_list[label_2];j_merge_2++)
{
merged_labels[j_merge_2+number_of_labels_in_list[label_1]]= label_list[label_2][j_merge_2]; 
}



  
//mean_depth = (comp_depth_list_new[label_1]*comp_size_list_new[label_1] +  comp_depth_list_new[label_2]*comp_size_list_new[label_2])/((double)(comp_size_list_new[label_1]+comp_size_list_new[label_2]));

mean_depth = (comp_depth_list_new[label_1] +  comp_depth_list_new[label_2])/((double)(comp_size_list_new[label_1]+comp_size_list_new[label_2]));

initial_parameters_quadric[0]=0;
initial_parameters_quadric[1]=0;
initial_parameters_quadric[2]=-0.05;
initial_parameters_quadric[3]=-0.05;
initial_parameters_quadric[4]=mean_depth-5;
// printf("MeanDepth!");
// printf ("%8f\n",mean_depth);
nelmin (parameter_quadric,initial_parameters_quadric, parameters_at_minimum, 
 min_fit_value, required_min, initial_simplex_step, konvergence_check, max_number_evaluations, 
 used_number_evaluations,number_restarts, error_detected,ComponentListX, ComponentListY, ComponentListZ,comp_size_list,merged_labels,number_merged_labels);//comp_size_list[label_j],label_j);

 
 
//  nelmin (parameter_quadric,initial_parameters_quadric, parameters_at_minimum, 
//  min_fit_value, required_min, initial_simplex_step, konvergence_check, max_number_evaluations, 
//  used_number_evaluations,number_restarts, error_detected,ComponentListX, ComponentListY, ComponentListZ, comp_size_list,list_of_labels_a,1);
//  
// printf("FitValue!");
// printf ("%8f\n",round(min_fit_value[0]));

if (min_fit_value[0]<10)//<500)
{//do merge
// udpdate arrays

comp_depth_list_new[label_1] = comp_depth_list_new[label_1] +  comp_depth_list_new[label_2];//mean_depth;
comp_depth_list_new[label_2] = comp_depth_list_new[label_1] +  comp_depth_list_new[label_2];//mean_depth;
comp_size_list_new[label_1] = comp_size_list_new[label_1]+comp_size_list_new[label_2];//size_1 + size_2;
comp_size_list_new[label_2] = comp_size_list_new[label_1]+comp_size_list_new[label_2];//size_1 + size_2;
ErrorFit_new[label_1]=min_fit_value[0];
ErrorFit_new[label_2]=min_fit_value[0];
number_of_labels_in_list[label_1]=number_merged_labels;
number_of_labels_in_list[label_2]=number_merged_labels;
// update SurfaceModelParameters_new
for (int w_1=0;w_1<5;w_1++)
{
SurfaceModelsParameters_new[label_1][w_1] = parameters_at_minimum[w_1];
SurfaceModelsParameters_new[label_2][w_1] = parameters_at_minimum[w_1];
}

if (label_1<label_2)
{
  label_replace = label_2;
  label_keep = label_1;
}
else
  {
  label_replace = label_1;
  label_keep = label_2;
}
  
// update label_new
for (int w_2=0;w_2<j_count-1;w_2++)
{
if (labels_new[w_2]==label_replace)
{
 labels_new[w_2]=label_keep;
}
} 

//update label_list
label_list[label_keep] = (int*) realloc (label_list[label_keep], number_merged_labels * sizeof(int));
for (int j_merge=0;j_merge<number_merged_labels;j_merge++)
{
label_list[label_keep][j_merge]=merged_labels[j_merge];
}

}

// free number_merged_labels
free (merged_labels);

}


}


}


   image<rgb> *output = new image<rgb>(width, height);

  // pick random colors for each component
  rgb *colors = new rgb[width*height];
  for (int i = 0; i < width*height; i++)
    colors[i] = random_rgb();



  int * comp_label_list_fin; 
  comp_label_list_fin =  (int*)malloc(sizeof(int)*height*width);
  
 
  for (int j = 0; j<(height*width); j++)
  {comp_label_list_fin[j] = 0;
  }
  
  int comp;
  int j_count_fin = 1;
  int comp_fin;
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      comp = ClusterArray[y][x]-1;
      // printf ("comp %8d\n",comp);
      comp_fin = labels_new[comp];
      if (comp_size_list_new[comp_fin]>size_thres){
        if (comp_label_list_fin[comp_fin]==0){
          comp_label_list_fin[comp_fin]=j_count_fin;
          ClusterArray[y][x] = j_count_fin;
          j_count_fin = j_count_fin +1;
        }else{
          ClusterArray[y][x] = comp_label_list_fin[comp_fin];
        }
        imRef(output, x, y) =colors[comp_fin];
        //ClusterArray[y][x] =comp_label_list_fin[comp_fin];//comp_fin;
        imRef(output, x, y) =colors[comp_label_list_fin[comp_fin]];//colors[comp_fin];// 
        imRef(output, x, y).r = (uchar) ClusterArray[y][x];//comp_label_list_fin[comp_fin];//colors[comp_fin];
     }
     else
     {ClusterArray[y][x] = 0;
     imRef(output, x, y) = colors[0];
     imRef(output, x, y).r = (uchar) 0;
     }
      
      /*
      
      int comp = ClusterArray[y][x]-1;
      int comp_fin = labels_new[comp];
     if (comp_size_list_new[comp_fin]>size_thres)
     {
     if (comp_label_list_fin[comp_fin]==0)
     {comp_label_list_fin[comp_fin]=j_count_fin;
     ClusterArray[y][x] = j_count_fin;
     j_count_fin = j_count_fin +1;}
     else
     {
     ClusterArray[y][x] = comp_label_list_fin[comp_fin];
     imRef(output, x, y) = colors[comp_fin];
     }
     
    
     }
     else
     {ClusterArray[y][x] = 0;
     imRef(output, x, y) = colors[0];
     }*/
     
//      if (comp_label_list_fin[comp_fin]>0)
//      {
//      ClusterArray[y][x] = comp_label_list_fin[comp_fin];
//      imRef(output, x, y) = colors[comp_fin];
//      //printf ("%8d\n",comp_label_list[comp]);
//      }
     
    }
  }  
  
// printf("cluster %d labels\n", ClusterArray[120][60]); 
  

*num_ccs_fin = j_count_fin;

  delete [] colors;  
  delete u;
  delete fitted_depth;
  
free(comp_size_list);
free(comp_depth_list);
free(comp_depth_list_new);
free(comp_label_list);
free(comp_size_list_new);
free(labels_new);
free(comp_label_list_fin);
free(ErrorFit);
free(ErrorFit_new);
  
 for (int k=0; k<j_count; k++)
        {
                free(SurfaceModelsParameters[k]);
                free(SurfaceModelsParameters_new[k]);
                free(label_list[k]);
                free(NeighborGraph[k]);
                free(ComponentListX[k]);
                free(ComponentListY[k]);
                free(ComponentListZ[k]);
        }
        
free(SurfaceModelsParameters_new);
free(SurfaceModelsParameters);
free(label_list);
free(NeighborGraph);
free(ComponentListX);
free(ComponentListY);
free(ComponentListZ);


        
        
  
  //return fitted_depth;
  return output;
}

#endif