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 28db_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 74db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration() 75{ 76 Clean(); 77} 78 79void 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 147void 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 252int 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 310void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9]) 311{ 312 db_Copy9(H,m_H_dref_to_ref); 313} 314 315void 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 320void 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 330void db_FrameToReferenceRegistration::ResetDisplayReference() 331{ 332 db_Identity3x3(m_H_dref_to_ref); 333} 334 335bool 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 344int 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 db_PrintDoubleMatrix(m_H_ref_to_ins,3,3); 489 490 db_Copy9(H, m_H_ref_to_ins); 491 492 m_nr_frames_processed++; 493{ 494 if ( (m_nr_frames_processed % m_reference_update_period) == 0 ) 495 { 496 //UpdateReference(imptr,false, false); 497 498 #if MB 499 UpdateReference(imptr,false, true); 500 #else 501 UpdateReference(imptr,false, false); 502 #endif 503 } 504 505 506 } 507 508 509 510 return 1; 511} 512 513//void db_FrameToReferenceRegistration::ComputeInliers(double H[9],std::vector<int> &inlier_indices) 514void db_FrameToReferenceRegistration::ComputeInliers(double H[9]) 515{ 516 double totnummatches = m_nr_matches; 517 int inliercount=0; 518 519 m_num_inlier_indices = 0; 520// inlier_indices.clear(); 521 522 for(int c=0; c < totnummatches; c++ ) 523 { 524 if (m_sq_cost[c] <= m_outlier_t2) 525 { 526 m_inlier_indices[inliercount] = c; 527 inliercount++; 528 } 529 } 530 531 m_num_inlier_indices = inliercount; 532 double frac=inliercount/totnummatches; 533} 534 535//void db_FrameToReferenceRegistration::Polish(std::vector<int> &inlier_indices) 536void db_FrameToReferenceRegistration::Polish(int *inlier_indices, int &num_inlier_indices) 537{ 538 db_Zero(m_polish_C,36); 539 db_Zero(m_polish_D,6); 540 for (int i=0;i<num_inlier_indices;i++) 541 { 542 int j = 3*inlier_indices[i]; 543 m_polish_C[0]+=m_corners_ref[j]*m_corners_ref[j]; 544 m_polish_C[1]+=m_corners_ref[j]*m_corners_ref[j+1]; 545 m_polish_C[2]+=m_corners_ref[j]; 546 m_polish_C[7]+=m_corners_ref[j+1]*m_corners_ref[j+1]; 547 m_polish_C[8]+=m_corners_ref[j+1]; 548 m_polish_C[14]+=1; 549 m_polish_D[0]+=m_corners_ref[j]*m_corners_ins[j]; 550 m_polish_D[1]+=m_corners_ref[j+1]*m_corners_ins[j]; 551 m_polish_D[2]+=m_corners_ins[j]; 552 m_polish_D[3]+=m_corners_ref[j]*m_corners_ins[j+1]; 553 m_polish_D[4]+=m_corners_ref[j+1]*m_corners_ins[j+1]; 554 m_polish_D[5]+=m_corners_ins[j+1]; 555 } 556 557 double a=db_maxd(m_polish_C[0],m_polish_C[7]); 558 m_polish_C[0]/=a; m_polish_C[1]/=a; m_polish_C[2]/=a; 559 m_polish_C[7]/=a; m_polish_C[8]/=a; m_polish_C[14]/=a; 560 561 m_polish_D[0]/=a; m_polish_D[1]/=a; m_polish_D[2]/=a; 562 m_polish_D[3]/=a; m_polish_D[4]/=a; m_polish_D[5]/=a; 563 564 565 m_polish_C[6]=m_polish_C[1]; 566 m_polish_C[12]=m_polish_C[2]; 567 m_polish_C[13]=m_polish_C[8]; 568 569 m_polish_C[21]=m_polish_C[0]; m_polish_C[22]=m_polish_C[1]; m_polish_C[23]=m_polish_C[2]; 570 m_polish_C[28]=m_polish_C[7]; m_polish_C[29]=m_polish_C[8]; 571 m_polish_C[35]=m_polish_C[14]; 572 573 574 double d[6]; 575 db_CholeskyDecomp6x6(m_polish_C,d); 576 db_CholeskyBacksub6x6(m_H_ref_to_ins,m_polish_C,d,m_polish_D); 577} 578 579void db_FrameToReferenceRegistration::EstimateSecondaryModel(double H[9]) 580{ 581 /* if ( m_current_is_reference ) 582 { 583 db_Identity3x3(H); 584 return; 585 } 586 */ 587 588 // select the outliers of the current model: 589 SelectOutliers(); 590 591 // perform the alignment: 592 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, 593 m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale, 594 m_nr_samples, m_chunk_size); 595 596 db_Copy9(H,m_H_ref_to_ins); 597} 598 599void db_FrameToReferenceRegistration::ComputeCostArray() 600{ 601 if ( m_sq_cost_computed ) return; 602 603 for( int c=0, k=0 ;c < m_nr_matches; c++, k=k+3) 604 { 605 m_sq_cost[c] = SquaredInhomogenousHomographyError(m_corners_ins+k,m_H_ref_to_ins,m_corners_ref+k); 606 } 607 608 m_sq_cost_computed = true; 609} 610 611void db_FrameToReferenceRegistration::SelectOutliers() 612{ 613 int nr_outliers=0; 614 615 ComputeCostArray(); 616 617 for(int c=0, k=0 ;c<m_nr_matches;c++,k=k+3) 618 { 619 if (m_sq_cost[c] > m_outlier_t2) 620 { 621 int offset = 3*nr_outliers++; 622 db_Copy3(m_corners_ref+offset,m_corners_ref+k); 623 db_Copy3(m_corners_ins+offset,m_corners_ins+k); 624 } 625 } 626 627 m_nr_matches = nr_outliers; 628} 629 630void db_FrameToReferenceRegistration::ComputeCostHistogram() 631{ 632 ComputeCostArray(); 633 634 for ( int b = 0; b < m_nr_bins; ++b ) 635 m_cost_histogram[b] = 0; 636 637 for(int c = 0; c < m_nr_matches; c++) 638 { 639 double error = db_SafeSqrt(m_sq_cost[c]); 640 int bin = (int)(error/m_max_cost_pix*m_nr_bins); 641 if ( bin < m_nr_bins ) 642 m_cost_histogram[bin]++; 643 else 644 m_cost_histogram[m_nr_bins-1]++; 645 } 646 647/* 648 for ( int i = 0; i < m_nr_bins; ++i ) 649 std::cout << m_cost_histogram[i] << " "; 650 std::cout << std::endl; 651*/ 652} 653 654void db_FrameToReferenceRegistration::SetOutlierThreshold() 655{ 656 ComputeCostHistogram(); 657 658 int i = 0, last=0; 659 for (; i < m_nr_bins-1; ++i ) 660 { 661 if ( last > m_cost_histogram[i] ) 662 break; 663 last = m_cost_histogram[i]; 664 } 665 666 //std::cout << "I " << i << std::endl; 667 668 int max = m_cost_histogram[i]; 669 670 for (; i < m_nr_bins-1; ++i ) 671 { 672 if ( m_cost_histogram[i] < (int)(0.1*max) ) 673 //if ( last < m_cost_histogram[i] ) 674 break; 675 last = m_cost_histogram[i]; 676 } 677 //std::cout << "J " << i << std::endl; 678 679 m_outlier_t2 = db_sqr(i*m_max_cost_pix/m_nr_bins); 680 681 //std::cout << "m_outlier_t2 " << m_outlier_t2 << std::endl; 682} 683 684void db_FrameToReferenceRegistration::SmoothMotion(void) 685{ 686 VP_MOTION inmot,outmot; 687 688 double H[9]; 689 690 Get_H_dref_to_ins(H); 691 692 MXX(inmot) = H[0]; 693 MXY(inmot) = H[1]; 694 MXZ(inmot) = H[2]; 695 MXW(inmot) = 0.0; 696 697 MYX(inmot) = H[3]; 698 MYY(inmot) = H[4]; 699 MYZ(inmot) = H[5]; 700 MYW(inmot) = 0.0; 701 702 MZX(inmot) = H[6]; 703 MZY(inmot) = H[7]; 704 MZZ(inmot) = H[8]; 705 MZW(inmot) = 0.0; 706 707 MWX(inmot) = 0.0; 708 MWY(inmot) = 0.0; 709 MWZ(inmot) = 0.0; 710 MWW(inmot) = 1.0; 711 712 inmot.type = VP_MOTION_AFFINE; 713 714 int w = m_im_width; 715 int h = m_im_height; 716 717 if(m_quarter_resolution) 718 { 719 w = w*2; 720 h = h*2; 721 } 722 723#if 0 724 m_stab_smoother.smoothMotionAdaptive(w,h,&inmot,&outmot); 725#else 726 m_stab_smoother.smoothMotion(&inmot,&outmot); 727#endif 728 729 H[0] = MXX(outmot); 730 H[1] = MXY(outmot); 731 H[2] = MXZ(outmot); 732 733 H[3] = MYX(outmot); 734 H[4] = MYY(outmot); 735 H[5] = MYZ(outmot); 736 737 H[6] = MZX(outmot); 738 H[7] = MZY(outmot); 739 H[8] = MZZ(outmot); 740 741 Set_H_dref_to_ins(H); 742} 743 744void db_FrameToReferenceRegistration::GenerateQuarterResImage(const unsigned char* const* im) 745{ 746 int input_h = m_im_height*2; 747 int input_w = m_im_width*2; 748 749 for (int j = 0; j < input_h; j++) 750 { 751 const unsigned char* in_row_ptr = im[j]; 752 unsigned char* out_row_ptr = m_horz_smooth_subsample_image[j]+1; 753 754 for (int i = 2; i < input_w-2; i += 2) 755 { 756 int smooth_val = ( 757 6*in_row_ptr[i] + 758 ((in_row_ptr[i-1]+in_row_ptr[i+1])<<2) + 759 in_row_ptr[i-2]+in_row_ptr[i+2] 760 ) >> 4; 761 *out_row_ptr++ = (unsigned char) smooth_val; 762 763 if ( (smooth_val < 0) || (smooth_val > 255)) 764 { 765 return; 766 } 767 768 } 769 } 770 771 for (int j = 2; j < input_h-2; j+=2) 772 { 773 774 unsigned char* in_row_ptr = m_horz_smooth_subsample_image[j]; 775 unsigned char* out_row_ptr = m_quarter_res_image[j/2]; 776 777 for (int i = 1; i < m_im_width-1; i++) 778 { 779 int smooth_val = ( 780 6*in_row_ptr[i] + 781 ((in_row_ptr[i-m_im_width]+in_row_ptr[i+m_im_width]) << 2)+ 782 in_row_ptr[i-2*m_im_width]+in_row_ptr[i+2*m_im_width] 783 ) >> 4; 784 *out_row_ptr++ = (unsigned char)smooth_val; 785 786 if ( (smooth_val < 0) || (smooth_val > 255)) 787 { 788 return; 789 } 790 791 } 792 } 793} 794