1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 // $Id: dbreg.cpp,v 1.31 2011/06/17 14:04:32 mbansal Exp $ 18 #include "dbreg.h" 19 #include <string.h> 20 #include <stdio.h> 21 22 23 #if PROFILE 24 #endif 25 26 //#include <iostream> 27 28 db_FrameToReferenceRegistration::db_FrameToReferenceRegistration() : 29 m_initialized(false),m_nr_matches(0),m_over_allocation(256),m_nr_bins(20),m_max_cost_pix(30), m_quarter_resolution(false) 30 { 31 m_reference_image = NULL; 32 m_aligned_ins_image = NULL; 33 34 m_quarter_res_image = NULL; 35 m_horz_smooth_subsample_image = NULL; 36 37 m_x_corners_ref = NULL; 38 m_y_corners_ref = NULL; 39 40 m_x_corners_ins = NULL; 41 m_y_corners_ins = NULL; 42 43 m_match_index_ref = NULL; 44 m_match_index_ins = NULL; 45 46 m_inlier_indices = NULL; 47 48 m_num_inlier_indices = 0; 49 50 m_temp_double = NULL; 51 m_temp_int = NULL; 52 53 m_corners_ref = NULL; 54 m_corners_ins = NULL; 55 56 m_sq_cost = NULL; 57 m_cost_histogram = NULL; 58 59 profile_string = NULL; 60 61 db_Identity3x3(m_K); 62 db_Identity3x3(m_H_ref_to_ins); 63 db_Identity3x3(m_H_dref_to_ref); 64 65 m_sq_cost_computed = false; 66 m_reference_set = false; 67 68 m_reference_update_period = 0; 69 m_nr_frames_processed = 0; 70 71 return; 72 } 73 74 db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration() 75 { 76 Clean(); 77 } 78 79 void db_FrameToReferenceRegistration::Clean() 80 { 81 if ( m_reference_image ) 82 db_FreeImage_u(m_reference_image,m_im_height); 83 84 if ( m_aligned_ins_image ) 85 db_FreeImage_u(m_aligned_ins_image,m_im_height); 86 87 if ( m_quarter_res_image ) 88 { 89 db_FreeImage_u(m_quarter_res_image, m_im_height); 90 } 91 92 if ( m_horz_smooth_subsample_image ) 93 { 94 db_FreeImage_u(m_horz_smooth_subsample_image, m_im_height*2); 95 } 96 97 delete [] m_x_corners_ref; 98 delete [] m_y_corners_ref; 99 100 delete [] m_x_corners_ins; 101 delete [] m_y_corners_ins; 102 103 delete [] m_match_index_ref; 104 delete [] m_match_index_ins; 105 106 delete [] m_temp_double; 107 delete [] m_temp_int; 108 109 delete [] m_corners_ref; 110 delete [] m_corners_ins; 111 112 delete [] m_sq_cost; 113 delete [] m_cost_histogram; 114 115 delete [] m_inlier_indices; 116 117 if(profile_string) 118 delete [] profile_string; 119 120 m_reference_image = NULL; 121 m_aligned_ins_image = NULL; 122 123 m_quarter_res_image = NULL; 124 m_horz_smooth_subsample_image = NULL; 125 126 m_x_corners_ref = NULL; 127 m_y_corners_ref = NULL; 128 129 m_x_corners_ins = NULL; 130 m_y_corners_ins = NULL; 131 132 m_match_index_ref = NULL; 133 m_match_index_ins = NULL; 134 135 m_inlier_indices = NULL; 136 137 m_temp_double = NULL; 138 m_temp_int = NULL; 139 140 m_corners_ref = NULL; 141 m_corners_ins = NULL; 142 143 m_sq_cost = NULL; 144 m_cost_histogram = NULL; 145 } 146 147 void db_FrameToReferenceRegistration::Init(int width, int height, 148 int homography_type, 149 int max_iterations, 150 bool linear_polish, 151 bool quarter_resolution, 152 double scale, 153 unsigned int reference_update_period, 154 bool do_motion_smoothing, 155 double motion_smoothing_gain, 156 int nr_samples, 157 int chunk_size, 158 int cd_target_nr_corners, 159 double cm_max_disparity, 160 bool cm_use_smaller_matching_window, 161 int cd_nr_horz_blocks, 162 int cd_nr_vert_blocks 163 ) 164 { 165 Clean(); 166 167 m_reference_update_period = reference_update_period; 168 m_nr_frames_processed = 0; 169 170 m_do_motion_smoothing = do_motion_smoothing; 171 m_motion_smoothing_gain = motion_smoothing_gain; 172 173 m_stab_smoother.setSmoothingFactor(m_motion_smoothing_gain); 174 175 m_quarter_resolution = quarter_resolution; 176 177 profile_string = new char[10240]; 178 179 if (m_quarter_resolution == true) 180 { 181 width = width/2; 182 height = height/2; 183 184 m_horz_smooth_subsample_image = db_AllocImage_u(width,height*2,m_over_allocation); 185 m_quarter_res_image = db_AllocImage_u(width,height,m_over_allocation); 186 } 187 188 m_im_width = width; 189 m_im_height = height; 190 191 double temp[9]; 192 db_Approx3DCalMat(m_K,temp,m_im_width,m_im_height); 193 194 m_homography_type = homography_type; 195 m_max_iterations = max_iterations; 196 m_scale = 2/(m_K[0]+m_K[4]); 197 m_nr_samples = nr_samples; 198 m_chunk_size = chunk_size; 199 200 double outlier_t1 = 5.0; 201 202 m_outlier_t2 = outlier_t1*outlier_t1;//*m_scale*m_scale; 203 204 m_current_is_reference = false; 205 206 m_linear_polish = linear_polish; 207 208 m_reference_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation); 209 m_aligned_ins_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation); 210 211 // initialize feature detection and matching: 212 //m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,0.0,0.0); 213 m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,DB_DEFAULT_ABS_CORNER_THRESHOLD/500.0,0.0); 214 215 int use_21 = 0; 216 m_max_nr_matches = m_cm.Init(m_im_width,m_im_height,cm_max_disparity,m_max_nr_corners,DB_DEFAULT_NO_DISPARITY,cm_use_smaller_matching_window,use_21); 217 218 // allocate space for corner feature locations for reference and inspection images: 219 m_x_corners_ref = new double [m_max_nr_corners]; 220 m_y_corners_ref = new double [m_max_nr_corners]; 221 222 m_x_corners_ins = new double [m_max_nr_corners]; 223 m_y_corners_ins = new double [m_max_nr_corners]; 224 225 // allocate space for match indices: 226 m_match_index_ref = new int [m_max_nr_matches]; 227 m_match_index_ins = new int [m_max_nr_matches]; 228 229 m_temp_double = new double [12*DB_DEFAULT_NR_SAMPLES+10*m_max_nr_matches]; 230 m_temp_int = new int [db_maxi(DB_DEFAULT_NR_SAMPLES,m_max_nr_matches)]; 231 232 // allocate space for homogenous image points: 233 m_corners_ref = new double [3*m_max_nr_corners]; 234 m_corners_ins = new double [3*m_max_nr_corners]; 235 236 // allocate cost array and histogram: 237 m_sq_cost = new double [m_max_nr_matches]; 238 m_cost_histogram = new int [m_nr_bins]; 239 240 // reserve array: 241 //m_inlier_indices.reserve(m_max_nr_matches); 242 m_inlier_indices = new int[m_max_nr_matches]; 243 244 m_initialized = true; 245 246 m_max_inlier_count = 0; 247 } 248 249 250 #define MB 0 251 // Save the reference image, detect features and update the dref-to-ref transformation 252 int db_FrameToReferenceRegistration::UpdateReference(const unsigned char * const * im, bool subsample, bool detect_corners) 253 { 254 double temp[9]; 255 db_Multiply3x3_3x3(temp,m_H_dref_to_ref,m_H_ref_to_ins); 256 db_Copy9(m_H_dref_to_ref,temp); 257 258 const unsigned char * const * imptr = im; 259 260 if (m_quarter_resolution && subsample) 261 { 262 GenerateQuarterResImage(im); 263 imptr = m_quarter_res_image; 264 } 265 266 // save the reference image, detect features and quit 267 db_CopyImage_u(m_reference_image,imptr,m_im_width,m_im_height,m_over_allocation); 268 269 if(detect_corners) 270 { 271 #if MB 272 m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref); 273 int nr = 0; 274 for(int k=0; k<m_nr_corners_ref; k++) 275 { 276 if(m_x_corners_ref[k]>m_im_width/3) 277 { 278 m_x_corners_ref[nr] = m_x_corners_ref[k]; 279 m_y_corners_ref[nr] = m_y_corners_ref[k]; 280 nr++; 281 } 282 283 } 284 m_nr_corners_ref = nr; 285 #else 286 m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref); 287 #endif 288 } 289 else 290 { 291 m_nr_corners_ref = m_nr_corners_ins; 292 293 for(int k=0; k<m_nr_corners_ins; k++) 294 { 295 m_x_corners_ref[k] = m_x_corners_ins[k]; 296 m_y_corners_ref[k] = m_y_corners_ins[k]; 297 } 298 299 } 300 301 db_Identity3x3(m_H_ref_to_ins); 302 303 m_max_inlier_count = 0; // Reset to 0 as no inliers seen until now 304 m_sq_cost_computed = false; 305 m_reference_set = true; 306 m_current_is_reference = true; 307 return 1; 308 } 309 310 void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9]) 311 { 312 db_Copy9(H,m_H_dref_to_ref); 313 } 314 315 void db_FrameToReferenceRegistration::Get_H_dref_to_ins(double H[9]) 316 { 317 db_Multiply3x3_3x3(H,m_H_dref_to_ref,m_H_ref_to_ins); 318 } 319 320 void db_FrameToReferenceRegistration::Set_H_dref_to_ins(double H[9]) 321 { 322 double H_ins_to_ref[9]; 323 324 db_Identity3x3(H_ins_to_ref); // Ensure it has proper values 325 db_InvertAffineTransform(H_ins_to_ref,m_H_ref_to_ins); // Invert to get ins to ref 326 db_Multiply3x3_3x3(m_H_dref_to_ref,H,H_ins_to_ref); // Update dref to ref using the input H from dref to ins 327 } 328 329 330 void db_FrameToReferenceRegistration::ResetDisplayReference() 331 { 332 db_Identity3x3(m_H_dref_to_ref); 333 } 334 335 bool db_FrameToReferenceRegistration::NeedReferenceUpdate() 336 { 337 // If less than 50% of the starting number of inliers left, then its time to update the reference. 338 if(m_max_inlier_count>0 && float(m_num_inlier_indices)/float(m_max_inlier_count)<0.5) 339 return true; 340 else 341 return false; 342 } 343 344 int db_FrameToReferenceRegistration::AddFrame(const unsigned char * const * im, double H[9],bool force_reference,bool prewarp) 345 { 346 m_current_is_reference = false; 347 if(!m_reference_set || force_reference) 348 { 349 db_Identity3x3(m_H_ref_to_ins); 350 db_Copy9(H,m_H_ref_to_ins); 351 352 UpdateReference(im,true,true); 353 return 0; 354 } 355 356 const unsigned char * const * imptr = im; 357 358 if (m_quarter_resolution) 359 { 360 if (m_quarter_res_image) 361 { 362 GenerateQuarterResImage(im); 363 } 364 365 imptr = (const unsigned char * const* )m_quarter_res_image; 366 } 367 368 double H_last[9]; 369 db_Copy9(H_last,m_H_ref_to_ins); 370 db_Identity3x3(m_H_ref_to_ins); 371 372 m_sq_cost_computed = false; 373 374 // detect corners on inspection image and match to reference image features:s 375 376 // @jke - Adding code to time the functions. TODO: Remove after test 377 #if PROFILE 378 double iTimer1, iTimer2; 379 char str[255]; 380 strcpy(profile_string,"\n"); 381 sprintf(str,"[%dx%d] %p\n",m_im_width,m_im_height,im); 382 strcat(profile_string, str); 383 #endif 384 385 // @jke - Adding code to time the functions. TODO: Remove after test 386 #if PROFILE 387 iTimer1 = now_ms(); 388 #endif 389 m_cd.DetectCorners(imptr, m_x_corners_ins,m_y_corners_ins,&m_nr_corners_ins); 390 // @jke - Adding code to time the functions. TODO: Remove after test 391 # if PROFILE 392 iTimer2 = now_ms(); 393 double elapsedTimeCorner = iTimer2 - iTimer1; 394 sprintf(str,"Corner Detection [%d corners] = %g ms\n",m_nr_corners_ins, elapsedTimeCorner); 395 strcat(profile_string, str); 396 #endif 397 398 // @jke - Adding code to time the functions. TODO: Remove after test 399 #if PROFILE 400 iTimer1 = now_ms(); 401 #endif 402 if(prewarp) 403 m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref, 404 m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins, 405 m_match_index_ref,m_match_index_ins,&m_nr_matches,H,0); 406 else 407 m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref, 408 m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins, 409 m_match_index_ref,m_match_index_ins,&m_nr_matches); 410 // @jke - Adding code to time the functions. TODO: Remove after test 411 # if PROFILE 412 iTimer2 = now_ms(); 413 double elapsedTimeMatch = iTimer2 - iTimer1; 414 sprintf(str,"Matching [%d] = %g ms\n",m_nr_matches,elapsedTimeMatch); 415 strcat(profile_string, str); 416 #endif 417 418 419 // copy out matching features: 420 for ( int i = 0; i < m_nr_matches; ++i ) 421 { 422 int offset = 3*i; 423 m_corners_ref[offset ] = m_x_corners_ref[m_match_index_ref[i]]; 424 m_corners_ref[offset+1] = m_y_corners_ref[m_match_index_ref[i]]; 425 m_corners_ref[offset+2] = 1.0; 426 427 m_corners_ins[offset ] = m_x_corners_ins[m_match_index_ins[i]]; 428 m_corners_ins[offset+1] = m_y_corners_ins[m_match_index_ins[i]]; 429 m_corners_ins[offset+2] = 1.0; 430 } 431 432 // @jke - Adding code to time the functions. TODO: Remove after test 433 #if PROFILE 434 iTimer1 = now_ms(); 435 #endif 436 // perform the alignment: 437 db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int, 438 m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale, 439 m_nr_samples, m_chunk_size); 440 // @jke - Adding code to time the functions. TODO: Remove after test 441 # if PROFILE 442 iTimer2 = now_ms(); 443 double elapsedTimeHomography = iTimer2 - iTimer1; 444 sprintf(str,"Homography = %g ms\n",elapsedTimeHomography); 445 strcat(profile_string, str); 446 #endif 447 448 449 SetOutlierThreshold(); 450 451 // Compute the inliers for the db compute m_H_ref_to_ins 452 ComputeInliers(m_H_ref_to_ins); 453 454 // Update the max inlier count 455 m_max_inlier_count = (m_max_inlier_count > m_num_inlier_indices)?m_max_inlier_count:m_num_inlier_indices; 456 457 // Fit a least-squares model to just the inliers and put it in m_H_ref_to_ins 458 if(m_linear_polish) 459 Polish(m_inlier_indices, m_num_inlier_indices); 460 461 if (m_quarter_resolution) 462 { 463 m_H_ref_to_ins[2] *= 2.0; 464 m_H_ref_to_ins[5] *= 2.0; 465 } 466 467 #if PROFILE 468 sprintf(str,"#Inliers = %d \n",m_num_inlier_indices); 469 strcat(profile_string, str); 470 #endif 471 /* 472 ///// CHECK IF CURRENT TRANSFORMATION GOOD OR BAD //// 473 ///// IF BAD, then update reference to the last correctly aligned inspection frame; 474 if(m_num_inlier_indices<5)//0.9*m_nr_matches || m_nr_matches < 20) 475 { 476 db_Copy9(m_H_ref_to_ins,H_last); 477 UpdateReference(imptr,false); 478 // UpdateReference(m_aligned_ins_image,false); 479 } 480 else 481 { 482 ///// IF GOOD, then update the last correctly aligned inspection frame to be this; 483 //db_CopyImage_u(m_aligned_ins_image,imptr,m_im_width,m_im_height,m_over_allocation); 484 */ 485 if(m_do_motion_smoothing) 486 SmoothMotion(); 487 488 // Disable debug printing 489 // db_PrintDoubleMatrix(m_H_ref_to_ins,3,3); 490 491 db_Copy9(H, m_H_ref_to_ins); 492 493 m_nr_frames_processed++; 494 { 495 if ( (m_nr_frames_processed % m_reference_update_period) == 0 ) 496 { 497 //UpdateReference(imptr,false, false); 498 499 #if MB 500 UpdateReference(imptr,false, true); 501 #else 502 UpdateReference(imptr,false, false); 503 #endif 504 } 505 506 507 } 508 509 510 511 return 1; 512 } 513 514 //void db_FrameToReferenceRegistration::ComputeInliers(double H[9],std::vector<int> &inlier_indices) 515 void db_FrameToReferenceRegistration::ComputeInliers(double H[9]) 516 { 517 double totnummatches = m_nr_matches; 518 int inliercount=0; 519 520 m_num_inlier_indices = 0; 521 // inlier_indices.clear(); 522 523 for(int c=0; c < totnummatches; c++ ) 524 { 525 if (m_sq_cost[c] <= m_outlier_t2) 526 { 527 m_inlier_indices[inliercount] = c; 528 inliercount++; 529 } 530 } 531 532 m_num_inlier_indices = inliercount; 533 double frac=inliercount/totnummatches; 534 } 535 536 //void db_FrameToReferenceRegistration::Polish(std::vector<int> &inlier_indices) 537 void db_FrameToReferenceRegistration::Polish(int *inlier_indices, int &num_inlier_indices) 538 { 539 db_Zero(m_polish_C,36); 540 db_Zero(m_polish_D,6); 541 for (int i=0;i<num_inlier_indices;i++) 542 { 543 int j = 3*inlier_indices[i]; 544 m_polish_C[0]+=m_corners_ref[j]*m_corners_ref[j]; 545 m_polish_C[1]+=m_corners_ref[j]*m_corners_ref[j+1]; 546 m_polish_C[2]+=m_corners_ref[j]; 547 m_polish_C[7]+=m_corners_ref[j+1]*m_corners_ref[j+1]; 548 m_polish_C[8]+=m_corners_ref[j+1]; 549 m_polish_C[14]+=1; 550 m_polish_D[0]+=m_corners_ref[j]*m_corners_ins[j]; 551 m_polish_D[1]+=m_corners_ref[j+1]*m_corners_ins[j]; 552 m_polish_D[2]+=m_corners_ins[j]; 553 m_polish_D[3]+=m_corners_ref[j]*m_corners_ins[j+1]; 554 m_polish_D[4]+=m_corners_ref[j+1]*m_corners_ins[j+1]; 555 m_polish_D[5]+=m_corners_ins[j+1]; 556 } 557 558 double a=db_maxd(m_polish_C[0],m_polish_C[7]); 559 m_polish_C[0]/=a; m_polish_C[1]/=a; m_polish_C[2]/=a; 560 m_polish_C[7]/=a; m_polish_C[8]/=a; m_polish_C[14]/=a; 561 562 m_polish_D[0]/=a; m_polish_D[1]/=a; m_polish_D[2]/=a; 563 m_polish_D[3]/=a; m_polish_D[4]/=a; m_polish_D[5]/=a; 564 565 566 m_polish_C[6]=m_polish_C[1]; 567 m_polish_C[12]=m_polish_C[2]; 568 m_polish_C[13]=m_polish_C[8]; 569 570 m_polish_C[21]=m_polish_C[0]; m_polish_C[22]=m_polish_C[1]; m_polish_C[23]=m_polish_C[2]; 571 m_polish_C[28]=m_polish_C[7]; m_polish_C[29]=m_polish_C[8]; 572 m_polish_C[35]=m_polish_C[14]; 573 574 575 double d[6]; 576 db_CholeskyDecomp6x6(m_polish_C,d); 577 db_CholeskyBacksub6x6(m_H_ref_to_ins,m_polish_C,d,m_polish_D); 578 } 579 580 void db_FrameToReferenceRegistration::EstimateSecondaryModel(double H[9]) 581 { 582 /* if ( m_current_is_reference ) 583 { 584 db_Identity3x3(H); 585 return; 586 } 587 */ 588 589 // select the outliers of the current model: 590 SelectOutliers(); 591 592 // perform the alignment: 593 db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int, 594 m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale, 595 m_nr_samples, m_chunk_size); 596 597 db_Copy9(H,m_H_ref_to_ins); 598 } 599 600 void db_FrameToReferenceRegistration::ComputeCostArray() 601 { 602 if ( m_sq_cost_computed ) return; 603 604 for( int c=0, k=0 ;c < m_nr_matches; c++, k=k+3) 605 { 606 m_sq_cost[c] = SquaredInhomogenousHomographyError(m_corners_ins+k,m_H_ref_to_ins,m_corners_ref+k); 607 } 608 609 m_sq_cost_computed = true; 610 } 611 612 void db_FrameToReferenceRegistration::SelectOutliers() 613 { 614 int nr_outliers=0; 615 616 ComputeCostArray(); 617 618 for(int c=0, k=0 ;c<m_nr_matches;c++,k=k+3) 619 { 620 if (m_sq_cost[c] > m_outlier_t2) 621 { 622 int offset = 3*nr_outliers++; 623 db_Copy3(m_corners_ref+offset,m_corners_ref+k); 624 db_Copy3(m_corners_ins+offset,m_corners_ins+k); 625 } 626 } 627 628 m_nr_matches = nr_outliers; 629 } 630 631 void db_FrameToReferenceRegistration::ComputeCostHistogram() 632 { 633 ComputeCostArray(); 634 635 for ( int b = 0; b < m_nr_bins; ++b ) 636 m_cost_histogram[b] = 0; 637 638 for(int c = 0; c < m_nr_matches; c++) 639 { 640 double error = db_SafeSqrt(m_sq_cost[c]); 641 int bin = (int)(error/m_max_cost_pix*m_nr_bins); 642 if ( bin < m_nr_bins ) 643 m_cost_histogram[bin]++; 644 else 645 m_cost_histogram[m_nr_bins-1]++; 646 } 647 648 /* 649 for ( int i = 0; i < m_nr_bins; ++i ) 650 std::cout << m_cost_histogram[i] << " "; 651 std::cout << std::endl; 652 */ 653 } 654 655 void db_FrameToReferenceRegistration::SetOutlierThreshold() 656 { 657 ComputeCostHistogram(); 658 659 int i = 0, last=0; 660 for (; i < m_nr_bins-1; ++i ) 661 { 662 if ( last > m_cost_histogram[i] ) 663 break; 664 last = m_cost_histogram[i]; 665 } 666 667 //std::cout << "I " << i << std::endl; 668 669 int max = m_cost_histogram[i]; 670 671 for (; i < m_nr_bins-1; ++i ) 672 { 673 if ( m_cost_histogram[i] < (int)(0.1*max) ) 674 //if ( last < m_cost_histogram[i] ) 675 break; 676 last = m_cost_histogram[i]; 677 } 678 //std::cout << "J " << i << std::endl; 679 680 m_outlier_t2 = db_sqr(i*m_max_cost_pix/m_nr_bins); 681 682 //std::cout << "m_outlier_t2 " << m_outlier_t2 << std::endl; 683 } 684 685 void db_FrameToReferenceRegistration::SmoothMotion(void) 686 { 687 VP_MOTION inmot,outmot; 688 689 double H[9]; 690 691 Get_H_dref_to_ins(H); 692 693 MXX(inmot) = H[0]; 694 MXY(inmot) = H[1]; 695 MXZ(inmot) = H[2]; 696 MXW(inmot) = 0.0; 697 698 MYX(inmot) = H[3]; 699 MYY(inmot) = H[4]; 700 MYZ(inmot) = H[5]; 701 MYW(inmot) = 0.0; 702 703 MZX(inmot) = H[6]; 704 MZY(inmot) = H[7]; 705 MZZ(inmot) = H[8]; 706 MZW(inmot) = 0.0; 707 708 MWX(inmot) = 0.0; 709 MWY(inmot) = 0.0; 710 MWZ(inmot) = 0.0; 711 MWW(inmot) = 1.0; 712 713 inmot.type = VP_MOTION_AFFINE; 714 715 int w = m_im_width; 716 int h = m_im_height; 717 718 if(m_quarter_resolution) 719 { 720 w = w*2; 721 h = h*2; 722 } 723 724 #if 0 725 m_stab_smoother.smoothMotionAdaptive(w,h,&inmot,&outmot); 726 #else 727 m_stab_smoother.smoothMotion(&inmot,&outmot); 728 #endif 729 730 H[0] = MXX(outmot); 731 H[1] = MXY(outmot); 732 H[2] = MXZ(outmot); 733 734 H[3] = MYX(outmot); 735 H[4] = MYY(outmot); 736 H[5] = MYZ(outmot); 737 738 H[6] = MZX(outmot); 739 H[7] = MZY(outmot); 740 H[8] = MZZ(outmot); 741 742 Set_H_dref_to_ins(H); 743 } 744 745 void db_FrameToReferenceRegistration::GenerateQuarterResImage(const unsigned char* const* im) 746 { 747 int input_h = m_im_height*2; 748 int input_w = m_im_width*2; 749 750 for (int j = 0; j < input_h; j++) 751 { 752 const unsigned char* in_row_ptr = im[j]; 753 unsigned char* out_row_ptr = m_horz_smooth_subsample_image[j]+1; 754 755 for (int i = 2; i < input_w-2; i += 2) 756 { 757 int smooth_val = ( 758 6*in_row_ptr[i] + 759 ((in_row_ptr[i-1]+in_row_ptr[i+1])<<2) + 760 in_row_ptr[i-2]+in_row_ptr[i+2] 761 ) >> 4; 762 *out_row_ptr++ = (unsigned char) smooth_val; 763 764 if ( (smooth_val < 0) || (smooth_val > 255)) 765 { 766 return; 767 } 768 769 } 770 } 771 772 for (int j = 2; j < input_h-2; j+=2) 773 { 774 775 unsigned char* in_row_ptr = m_horz_smooth_subsample_image[j]; 776 unsigned char* out_row_ptr = m_quarter_res_image[j/2]; 777 778 for (int i = 1; i < m_im_width-1; i++) 779 { 780 int smooth_val = ( 781 6*in_row_ptr[i] + 782 ((in_row_ptr[i-m_im_width]+in_row_ptr[i+m_im_width]) << 2)+ 783 in_row_ptr[i-2*m_im_width]+in_row_ptr[i+2*m_im_width] 784 ) >> 4; 785 *out_row_ptr++ = (unsigned char)smooth_val; 786 787 if ( (smooth_val < 0) || (smooth_val > 255)) 788 { 789 return; 790 } 791 792 } 793 } 794 } 795
