00001
00002
00003
00004
00005
00006
00007
00008 #ifndef FIND_APPROACH
00009 #define FIND_APPROACH
00010 #include <iostream>
00011
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
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
00034
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
00048
00049
00050 double m_x = goal_parameters[7];
00051 double m_y = goal_parameters[8];
00052
00053
00054
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
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
00072 if ((current_label==selected_label)&&(current_depth>0))
00073 {
00074 int edge_flag = 0;
00075
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 {
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
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
00159
00160
00161
00162 px= edgeX_data - goal_parameters[0];
00163 py= edgeY_data - goal_parameters[1];
00164 pz= edgeZ_data - goal_parameters[2];
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
00175
00176
00177 parameter_list[0]=px/p_abs;
00178 parameter_list[1]=py/p_abs;
00179 parameter_list[2]=pz/p_abs;
00180
00181 parameter_list[3]=probingX;
00182 parameter_list[4]=probingY;
00183 parameter_list[5]=probingZ;
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 return parameter_list;
00194
00195 }
00196
00197
00198 #endif