00001
00002
00003
00004
00005
00006
00007
00008 #ifndef COMBINATORIAL_LEAF_FITTING
00009 #define COMBINATORIAL_LEAF_FITTING
00010 #include <iostream>
00011
00012 #include <cstdlib>
00013 #include <image.h>
00014 #include <misc.h>
00015 #include <filter.h>
00016 #include <leaf-fitting.h>
00017 #include <vector>
00018 #define PI 3.14159265
00019 using std::vector;
00020
00021 image<rgb> *combinatorial_leaf_fitting(int ** clusters, image<rgb> *model,image<rgb> *model_area, int num_clusters, double *& fit_score_list)
00022 {
00023 int width = model->width();
00024 int height = model->height();
00025 int ** segment_contoursX;
00026 int ** segment_contoursY;
00027 int ** segment_contoursLabel;
00028 double ** transformation_params;
00029
00030 int current_label;
00031 int * segment_contour_size;
00032 int edge_flag;
00033 int j_x;
00034 int j_y;
00035 int u_x;
00036 int u_y;
00037 int length_model_bound = 0;
00038
00039 double center_modelX=0;
00040 double center_modelY=0;
00041 double model_size = 0;
00042
00043
00045 for (j_x=0;j_x<width;j_x++)
00046 {
00047 for (j_y=0;j_y<height;j_y++)
00048 {
00049 edge_flag = 0;
00050
00051 if (imRef(model_area,j_x,j_y).b>10)
00052 {
00053 center_modelX = center_modelX + j_x;
00054 center_modelY = center_modelY + j_y;
00055 model_size = model_size +1;
00056
00057
00058
00059 for (u_y = j_y-1; u_y< j_y+1+1; u_y++)
00060 {
00061 for (u_x = j_x-1; u_x < j_x+1+1; u_x++)
00062 {
00063
00064 if ((u_y>=0)&&(u_x>=0)&&(u_x<width)&&(u_y<height))
00065 {
00066 if (imRef(model_area,u_x,u_y).b<10)
00067 {
00068 edge_flag = 1;
00069 }
00070
00071 }
00072
00073
00074
00075 }
00076 }
00077
00078
00079 }
00080
00081 if (edge_flag==1)
00082 {
00083 length_model_bound = length_model_bound + 1;
00084
00085 }
00086
00087
00088
00089
00090 }
00091
00092
00093 }
00094
00095 center_modelX = center_modelX/model_size;
00096 center_modelY = center_modelY/model_size;
00098
00100
00101
00102 segment_contoursX = (int**)malloc(sizeof(int*)*(num_clusters));
00103 segment_contoursY = (int**)malloc(sizeof(int*)*(num_clusters));
00104 segment_contoursLabel = (int**)malloc(sizeof(int*)*(num_clusters));
00105 segment_contour_size = (int*)malloc(sizeof(int)*num_clusters);
00106 double * segment_centerX = (double*)malloc(sizeof(double)*num_clusters);
00107 double * segment_centerY = (double*)malloc(sizeof(double)*num_clusters);
00108 double * segment_size = (double*)malloc(sizeof(double)*num_clusters);
00109 transformation_params = (double**)malloc(sizeof(double*)*(num_clusters));
00110
00111 int u_label;
00112 for (u_label=0;u_label<num_clusters;u_label++)
00113 {
00114 segment_contour_size[u_label]=0;
00115 segment_size[u_label]=0;
00116 transformation_params[u_label] = (double*)malloc(sizeof(double)*4);
00117 segment_contoursX[u_label] = (int*)malloc(sizeof(int)*1);
00118 segment_contoursY[u_label] = (int*)malloc(sizeof(int)*1);
00119 segment_contoursLabel[u_label] = (int*)malloc(sizeof(int)*1);
00120 }
00121
00122
00123 int contour_size;
00124
00125 int encountered_neighbor_labels [8];
00126 int number_encountered_labels [8];
00127 for (j_x=0;j_x<width;j_x++)
00128 {
00129 for (j_y=0;j_y<height;j_y++)
00130 {
00131 current_label = clusters[j_y][j_x];
00132
00133 if (current_label>0)
00134 {
00135 segment_centerX[current_label-1] = segment_centerX[current_label-1] + j_x;
00136 segment_centerY[current_label-1] = segment_centerY[current_label-1] + j_y;
00137 segment_size[current_label-1] = segment_size[current_label-1] + 1;
00138 edge_flag = 0;
00139
00140 int n_count = 0
00141 for (u_y = j_y-1; u_y< j_y+1+1; u_y++)
00142 {
00143 for (u_x = j_x-1; u_x < j_x+1+1; u_x++)
00144 {
00145
00146 if ((u_y>=0)&&(u_x>=0)&&(u_x<width)&&(u_y<height))
00147 {
00148 if (clusters[u_y][u_x]!=current_label)
00149 {
00150 edge_flag = 1;
00151 encountered_neighbor_labels[n_count] = clusters[u_y][u_x];
00152 n_count = n_count +1;
00153 }
00154
00155 }
00156
00157
00158
00159 }
00160 }
00161
00162
00163 encountered_neighbor_labels = {0,0,0,0,0,0,0,0};
00164 number_encountered_labels = {0,0,0,0,0,0,0,0};
00165
00166
00167
00168 if (edge_flag==1)
00169 {
00170 for (int j_n1=1;j_n1<n_count;j_n1++)
00171 {
00172 encountered_label_1 = encountered_neighbor_labels[j_n1];
00173 if (encountered_label_1>0)
00174 {
00175
00176
00177 for (int j_n2=1;j_n2<n_count;j_n2++)
00178 {
00179 encountered_label_2 = encountered_neighbor_labels[j_n2];
00180 if (encountered_label_2==encountered_label_1)
00181 {
00182 number_encountered_labels[j_n1] = number_encountered_labels[j_n1] +1;
00183 }
00184
00185 }
00186
00187 }
00188 }
00189
00190 int max_encountered_label=0;
00191 int label_of_neighbor;
00192 for (int j_n3=1;j_n3<n_count;j_n3++)
00193 {
00194 if (number_encountered_labels[j_n3]>max_encountered_label)
00195 {max_encountered_label = number_encountered_labels[j_n3];
00196 label_of_neighbor = encountered_neighbor_labels[j_n3];
00197 }
00198
00199
00200 }
00201
00202
00203
00204
00205 contour_size = segment_contour_size[current_label-1];
00206
00207 segment_contoursX[current_label-1] = (int*) realloc(segment_contoursX[current_label-1],(contour_size+1)*sizeof(int));
00208 segment_contoursX[current_label-1][contour_size] = j_x;
00209 segment_contoursY[current_label-1] = (int*) realloc(segment_contoursY[current_label-1],(contour_size+1)*sizeof(int));
00210 segment_contoursY[current_label-1][contour_size] = j_y;
00211 segment_contoursLabel[current_label-1] = (int*) realloc(segment_contoursLabel[current_label-1],(contour_size+1)*sizeof(int));
00212 segment_contoursLabel[current_label-1][contour_size] = label_of_neighbor;
00213 segment_contour_size[current_label-1] = contour_size+1;
00214
00215
00216
00217
00218
00219
00220
00221
00222 }
00223
00224
00225 }
00226
00227
00228
00229 }
00230
00231
00232 }
00233
00234
00236 int is_boundary;
00237
00238
00239
00240 int ** NeighborGraph;
00241 int j_count = num_clusters;
00242 int delta = 1;
00243 double depth;
00244 double depth_neighbor;
00245 double depth_dist;
00246 double depth_thres = 5;
00247 int label_neighbor;
00248 int k_1;
00249 int k_2;
00250 NeighborGraph=(int**)malloc(sizeof(int*)*j_count);
00251
00252 for (int k=0; k<j_count; k++)
00253 {
00254 NeighborGraph[k] = (int*)malloc(sizeof(int)*j_count);
00255
00256
00257 }
00258
00259
00260 for (k_1=0; k_1<j_count; k_1++)
00261 {
00262 for (k_2=0; k_2<j_count; k_2++)
00263
00264
00265 {
00266 NeighborGraph[k_1][k_2]=0;
00267 }
00268
00269 }
00270
00271
00272
00273 int y;
00274 int x;
00275 int j_y;
00276 int j_x;
00277 for (y = 0; y < height; y++) {
00278 for (x = 0; x < width; x++) {
00279
00280 is_boundary = 0;
00281 label = clusters[y][x];
00282
00283 depth = (double)(imRef(fitted_depth, x, y).b);
00284
00285
00287 for (j_y = y-1; j_y<y+2; j_y++) {
00288 for (j_x = x-1; j_x<x+2; j_x++) {
00289
00290 if ((j_y>=0)&&(j_x>=0)&&(j_x<width)&&(j_y<height))
00291 {
00292 label_neighbor = clusters[j_y][j_x];
00293
00294 if (label!=label_neighbor)
00295 {
00296 is_boundary = 1;
00297 }
00298
00299 }
00300
00301
00302 }
00303 }
00304
00305
00306 if (is_boundary==1)
00307 {
00308 for (j_y = y-delta; j_y< y+1+delta; j_y++) {
00309 for (j_x = x-delta; j_x < x+1+delta; j_x++) {
00310
00311 if ((j_y>=0)&&(j_x>=0)&&(j_x<width)&&(j_y<height))
00312 {
00313
00314
00315 label_neighbor = clusters[j_y][j_x];
00316
00317
00318
00319 depth_neighbor = (double)(imRef(fitted_depth, j_x, j_y).b);
00320 depth_dist = abs(depth - depth_neighbor);
00321 if ((label!=label_neighbor)&&(depth_dist<depth_thres))
00322 {NeighborGraph[label][label_neighbor]=1;
00323 NeighborGraph[label_neighbor][label]=1;
00324
00325 }
00326
00327
00328
00329
00330 }
00331
00332 }
00333 }
00334
00335
00336
00337 }
00338
00339
00340
00341
00342 }
00343 }
00344
00345
00346
00348
00349 vector<int> graph_label_1;
00350 vector<int> graph_label_2;
00351
00352
00353 for (k_1=0; k_1<j_count; k_1++)
00354 {
00355 for (k_2=0; k_2<j_count; k_2++)
00356 {
00357 if (k_1<k_2){
00358
00359 if (NeighborGraph[k_1][k_2]==1)
00360 {
00361
00362 graph_label_1.push_back (k_1);
00363 graph_label_2.push_back (k_2);
00364
00365 }
00366 }
00367
00368 }
00369
00370 }
00371
00372
00373
00374
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393 int * labels_new;
00394 labels_new = (int*)malloc(sizeof(int)*j_count);
00395
00396 int * comp_size_list_new;
00397 comp_size_list_new = (int*)malloc(sizeof(int)*j_count);
00398
00399 double * comp_depth_list_new;
00400 comp_depth_list_new = (double*)malloc(sizeof(double)*j_count);
00401
00402
00403 for (int label_j=0;label_j<j_count-1;label_j++)
00404 {labels_new[label_j]=label_j;
00405 comp_size_list_new[label_j]=comp_size_list[label_j];
00406 comp_depth_list_new[label_j]=comp_depth_list[label_j];
00407 }
00408
00409
00410 int ** label_list;
00411 label_list = (int**)malloc(sizeof(int*)*j_count);
00412
00413
00414 int * number_of_labels_in_list;
00415 number_of_labels_in_list = (int*)malloc(sizeof(int)*j_count);
00416 for (int k_b=0; k_b<j_count; k_b++)
00417 {
00418 label_list[k_b] = (int*)malloc(sizeof(int));
00419
00420 }
00421
00422 for (int label_j=0;label_j<j_count-1;label_j++)
00423 {label_list[label_j][0]=label_j;
00424 number_of_labels_in_list[label_j] = 1;
00425 }
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436 int u_1;
00437 int u_2;
00438 int label_1;
00439 int label_2;
00440 int error_1;
00441 int error_2;
00442 int size_1;
00443 int size_2;
00444 int label_1_total_size;
00445 int label_2_total_size;
00446 double thres_merge = 15;
00447 int number_merged_labels;
00448 double min_error;
00449 int number_of_links = graph_links.size();
00450 int label_keep;
00451 int label_replace;
00452
00453
00454 for (int link_index=0;link_index<number_of_links;link_index++)
00455 {
00456 u_1 = graph_label_1.at(link_index);
00457 u_2 = graph_label_2.at(link_index);
00458
00459 label_1=labels_new[u_1];
00460 label_2=labels_new[u_2];
00461
00462
00463
00464
00465 number_merged_labels = number_of_labels_in_list[label_1]+number_of_labels_in_list[label_2];
00466 int * merged_labels = (int*)malloc(sizeof(int)*number_merged_labels);
00467
00468 for (int j_merge_1=0;j_merge_1<number_of_labels_in_list[label_1];j_merge_1++)
00469 {
00470 merged_labels[j_merge_1]= label_list[label_1][j_merge_1];
00471 }
00472
00473 for (int j_merge_2=0;j_merge_2<number_of_labels_in_list[label_2];j_merge_2++)
00474 {
00475 merged_labels[j_merge_2+number_of_labels_in_list[label_1]]= label_list[label_2][j_merge_2];
00476 }
00477
00478 number_of_labels_in_list[label_1]=number_merged_labels;
00479 number_of_labels_in_list[label_2]=number_merged_labels;
00480
00481 if (label_1<label_2)
00482 {
00483 label_replace = label_2;
00484 label_keep = label_1;
00485 }
00486 else
00487 {
00488 label_replace = label_1;
00489 label_keep = label_2;
00490 }
00491
00492
00493 for (int w_2=0;w_2<j_count-1;w_2++)
00494 {
00495 if (labels_new[w_2]==label_replace)
00496 {
00497 labels_new[w_2]=label_keep;
00498 }
00499 }
00500
00501
00502 label_list[label_keep] = (int*) realloc (label_list[label_keep], number_merged_labels * sizeof(int));
00503 for (int j_merge=0;j_merge<number_merged_labels;j_merge++)
00504 {
00505 label_list[label_keep][j_merge]=merged_labels[j_merge];
00506 }
00507
00508
00509
00510
00511 free (merged_labels);
00512
00513 }
00514
00515
00517
00519
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546 double initial_parameters_trans [4] = {1,0,0,0};
00547 double parameters_at_minimum [4] = {100,100,100,100};
00548 double min_fit_value [1] = {100};
00549 double required_min = 0.1;
00550 double initial_simplex_step [4] = {1,10,10,0.2};
00551 int konvergence_check = 100;
00552 int max_number_evaluations = 4000;
00553 int used_number_evaluations [1] = {0};
00554 int number_restarts [1] = {0};
00555 int error_detected [1] = {0};
00556 int parameter_trans = 4;
00557 double final_params [4];
00558 double final_min;
00559 double dist;
00560 double x_shift;
00561 double y_shift;
00562
00563 fit_score_list = (double*)malloc(sizeof(double)*num_clusters);
00564 for (u_label=0;u_label<num_clusters-1;u_label++)
00565 {
00566 if (((model_size/segment_size[u_label])<10)&&((model_size/segment_size[u_label])>0.1)){
00567
00568 final_min = 1000;
00569 final_params[0]=0;
00570 final_params[1]=0;
00571 final_params[2]=0;
00572 final_params[3]=0;
00573
00574
00575 for (int u_rot=0;u_rot<18;u_rot++)
00576 {
00577
00578 x_shift = center_modelX - segment_centerX[u_label]/segment_size[u_label];
00579 y_shift = center_modelY - segment_centerY[u_label]/segment_size[u_label];
00580 initial_parameters_trans[0]=model_size/segment_size[u_label];
00581
00582
00583
00584
00585
00586 initial_parameters_trans[1]= x_shift;
00587 initial_parameters_trans[2]= y_shift;
00588
00589
00590 initial_parameters_trans[3]=PI*(u_rot*20)/((double)(180));
00591
00592
00593
00594
00595 nelmin_contour(parameter_trans,initial_parameters_trans, parameters_at_minimum, min_fit_value, required_min, initial_simplex_step, konvergence_check, max_number_evaluations,used_number_evaluations,number_restarts, error_detected,segment_contoursX[u_label],segment_contoursY[u_label],segment_contour_size[u_label],model);
00596
00597 if (min_fit_value[0]<final_min)
00598 {
00599 final_min = min_fit_value[0];
00600 final_params[0]=parameters_at_minimum[0];
00601 final_params[1]=parameters_at_minimum[1];
00602 final_params[2]=parameters_at_minimum[2];
00603 final_params[3]=parameters_at_minimum[3];
00604
00605 }
00606
00607
00608
00609
00610 }
00611
00612
00613
00614
00615 fit_score_list[u_label] = final_min;
00616 transformation_params[u_label][0]=final_params[0];
00617 transformation_params[u_label][1]=final_params[1];
00618 transformation_params[u_label][2]=final_params[2];
00619 transformation_params[u_label][3]=final_params[3];
00620
00621
00622
00623
00624
00625 }
00626 else {fit_score_list[u_label] = 1;}
00627
00628 }
00629
00630
00631 image<rgb> *output = new image<rgb>(width, height);
00632
00633 for (j_x=0;j_x<width;j_x++)
00634 {
00635 for (j_y=0;j_y<height;j_y++)
00636 {
00637 current_label = clusters[j_y][j_x];
00638 if (current_label>0){
00639 imRef(output, j_x, j_y).r = (uchar)((1-fit_score_list[current_label-1])*250);
00640 imRef(output, j_x, j_y).g = 0;
00641 imRef(output, j_x, j_y).b = 0;
00642 }
00643
00644 else
00645 {
00646 imRef(output, j_x, j_y).r = (uchar)(0);
00647 imRef(output, j_x, j_y).g = (uchar)(0);
00648 imRef(output, j_x, j_y).b = (uchar)(0);
00649
00650 }
00651
00652 }
00653 }
00654
00655
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683 return(output);
00684 }
00685
00686 #endif