find-approach.h
Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2012 Babette Dellen
00003 
00004 Approach vector computation algorithm 
00005 
00006 */
00007 
00008 #ifndef FIND_APPROACH
00009 #define FIND_APPROACH
00010 #include <iostream>
00011 //#include "cv.h"
00012 #include <cstdlib>
00013 #include <image.h>
00014 #include <misc.h>
00015 #include <filter.h>
00016 #include <vector>
00017 #define PI 3.14159265
00018 using std::vector;
00019 
00020 
00021 void TransformPoint(const int pointX, const int pointY, const double * current_trans, const double m_x, const double m_y, double *transformed_edge_point)
00022 {
00023   printf("scale %f scale \n", current_trans[0]);
00024   printf("shift x %f x_shift \n", current_trans[1]);
00025   printf("shift y %f y_shift \n", current_trans[2]);
00026   printf("rot %f rot \n", current_trans[3]);
00027   // rotating + scaling + shifting
00028   double rotX;
00029   double rotY;
00030   rotX = ((pointX-current_trans[1]-m_x)*cos(current_trans[3]) + (pointY-current_trans[2]-m_y)*sin(current_trans[3]))/current_trans[0] + m_x;
00031   rotY = (-(pointX-current_trans[1]-m_x)*sin(current_trans[3]) + (pointY-current_trans[2]-m_y)*cos(current_trans[3]))/current_trans[0] + m_y;
00032   /* 
00033     rotX = current_trans[0]*((contour_pointX[j]-meanX)*cos(current_trans[3]) - (contour_pointY[j]-meanY)*sin(current_trans[3])) + meanX + current_trans[1];
00034     rotY = current_trans[0]*((contour_pointX[j]-meanX)*sin(current_trans[3]) + (contour_pointY[j]-meanY)*cos(current_trans[3])) + meanY + current_trans[2];
00035   */
00036   transformed_edge_point[0] = round(rotX);
00037   transformed_edge_point[1] = round(rotY);
00038 }
00039 
00040 
00041 double * find_approach(int ** clusters, image<rgb> *pc, double * goal_parameters, double ** trans_params, int * model_probing_point) 
00042 {
00043   double * parameter_list = (double*)malloc(sizeof(double)*6); 
00044   int width = pc->width();
00045   int height = pc->height();
00046   int selected_label = (int)goal_parameters[6];
00047 //  double x_c = goal_parameters[0];
00048 //  double y_c = goal_parameters[1];
00049 //  double z_c = goal_parameters[2];
00050   double m_x = goal_parameters[7];
00051   double m_y = goal_parameters[8];
00052   //double * transformed_edge_point = (double*)malloc(sizeof(double)*2);
00053   //transformed_edge_point[0]=0.0;
00054   //transformed_edge_point[1]=0.0;
00055   double transformed_edge_point[2] = {0.0, 0.0};
00056   TransformPoint(model_probing_point[0], model_probing_point[1], trans_params[selected_label-1], m_x, m_y, transformed_edge_point);
00057 
00059   // find contour
00061   
00062   int * segment_contoursX = (int*)malloc(sizeof(int));
00063   int * segment_contoursY = (int*)malloc(sizeof(int));
00064   int contour_size = 0;
00065   int current_label;
00066   double current_depth;
00067   for (int j_x=0;j_x<width;j_x++){
00068     for (int j_y=0;j_y<height;j_y++){
00069       current_label = clusters[j_y][j_x];
00070       current_depth = (double)(imRef(pc, j_x, j_y).b);
00071       //printf("%8d\n",current_label);
00072       if ((current_label==selected_label)&&(current_depth>0))
00073       {
00074         int edge_flag = 0;
00075         //test if edge point
00076         for (int u_y = j_y-1; u_y< j_y+1+1; u_y++){
00077           for (int u_x = j_x-1; u_x < j_x+1+1; u_x++){
00078             if ((u_y>=0)&&(u_x>=0)&&(u_x<width)&&(u_y<height)){
00079               if (clusters[u_y][u_x]!=current_label){
00080                 edge_flag = 1;
00081               }
00082             }
00083           }
00084         }
00085 
00086         if (edge_flag>0)
00087         {//add to boundary list
00088           segment_contoursX = (int*) realloc(segment_contoursX,(contour_size+1)*sizeof(int));
00089           segment_contoursX[contour_size] = j_x;
00090           segment_contoursY = (int*) realloc(segment_contoursY,(contour_size+1)*sizeof(int));
00091           segment_contoursY[contour_size] = j_y;
00092           contour_size = contour_size+1;
00093         }
00094       }
00095     }
00096   }
00097   
00099   // find closest boundary point to transformed edge point of the segment 
00101 
00102   printf("[DEBUG] Model_probing_poing   : (%d, %d)\n", model_probing_point[0], model_probing_point[1]);
00103   printf("[DEBUG] Transformed_edge_point: (%f, %f)\n", transformed_edge_point[0], transformed_edge_point[1]);
00104 
00105   double dist_min = 1000;
00106   int edgeX=0;
00107   int edgeY=0;
00108   for (int u_c=0; u_c<contour_size; u_c++)  {
00109     int dist = pow(segment_contoursX[u_c]-transformed_edge_point[0],2) + pow(segment_contoursY[u_c]-transformed_edge_point[1],2);
00110     if (dist < dist_min){
00111       edgeX = round(segment_contoursX[u_c]);
00112       edgeY = round(segment_contoursY[u_c]);
00113       dist_min = dist;
00114     }
00115   }
00116   
00117   if (dist_min == 1000){
00118     printf("[ERROR][find-approach.h]: Edge point determination failed\n");
00119   }
00120    
00121 double px;
00122 double py;
00123 double pz;
00125 
00126 double edgeX_data = 0.0;
00127 double edgeY_data = 0.0;
00128 double edgeZ_data = 0.0;
00129 
00130 int j_count = 0;
00131 for (int i_x=edgeX-5; i_x<(edgeX+5);i_x++){
00132   for (int i_y=edgeY-5;i_y<(edgeY+5);i_y++){
00133     if ((i_x>=0)&&(i_x<width)&&(i_y>=0)&&(i_y<height)){
00134       double x_value = (double)(imRef(pc, i_x, i_y).r);
00135       double y_value = (double)(imRef(pc, i_x, i_y).g);
00136       double z_value = (double)(imRef(pc, i_x, i_y).b);
00137       printf("(%d, %d) = (%f, %f, %f)\n", i_x, i_y, x_value, y_value, z_value);
00138       if (z_value>0 && x_value<150 && x_value>0 && y_value<150 && y_value>0 ){
00139         if (clusters[i_y][i_x] == selected_label){
00140           edgeX_data = edgeX_data + x_value;
00141           edgeY_data = edgeY_data + y_value;
00142           edgeZ_data = edgeZ_data + z_value;
00143           j_count = j_count + 1;
00144         }
00145       }
00146     }
00147   }
00148 }
00149 if (j_count!=0){
00150  edgeX_data = edgeX_data/j_count; 
00151  edgeY_data = edgeY_data/j_count;
00152  edgeZ_data = edgeZ_data/j_count;
00153 }
00154 
00155 printf("j_count = %d, edge_X = %f, edge_Y = %f, edge_Z = %f\n", j_count, edgeX_data, edgeY_data, edgeZ_data);
00156 
00158 // int a_x = round(m_x);
00159 // int a_y = round(m_y);
00160 
00161 
00162 px= edgeX_data  - goal_parameters[0];//(double)(imRef(pc,a_x,a_y).r);
00163 py= edgeY_data  - goal_parameters[1];//(double)(imRef(pc,a_x,a_y).g);
00164 pz= edgeZ_data  - goal_parameters[2];//(double)(imRef(pc,a_x,a_y).b);
00165 
00166 
00167 double p_abs = sqrt(pow(px,2) + pow(py,2) + pow(pz,2));
00168 
00169 double dist_from_edge = 2;
00170 double probingX = edgeX_data - dist_from_edge*px/p_abs;
00171 double probingY = edgeY_data - dist_from_edge*py/p_abs;
00172 double probingZ = edgeZ_data - dist_from_edge*pz/p_abs;
00173 
00174 //printf("probing %f m_x \n", double(m_x));
00175 
00176 // approaching vector
00177 parameter_list[0]=px/p_abs;
00178 parameter_list[1]=py/p_abs;
00179 parameter_list[2]=pz/p_abs;
00180 // probing point 
00181 parameter_list[3]=probingX;
00182 parameter_list[4]=probingY;
00183 parameter_list[5]=probingZ;
00184 
00185 //printf("parameter_list %f p_x \n", parameter_list[3]);
00186 //printf("parameter_list %f p_y \n", parameter_list[4]);
00187 //printf("parameter_list %f p_z \n", parameter_list[5]);
00188 //printf("parameter_list %f a_x \n", parameter_list[0]);
00189 //printf("parameter_list %f a_y \n", parameter_list[1]);
00190 //printf("parameter_list %f a_z \n", parameter_list[2]);
00191 
00192  
00193 return parameter_list;
00194   
00195 }
00196 
00197 
00198 #endif


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