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
db_FrameToReferenceRegistration()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
~db_FrameToReferenceRegistration()74 db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration()
75 {
76 Clean();
77 }
78
Clean()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
Init(int width,int height,int homography_type,int max_iterations,bool linear_polish,bool quarter_resolution,double scale,unsigned int reference_update_period,bool do_motion_smoothing,double motion_smoothing_gain,int nr_samples,int chunk_size,int cd_target_nr_corners,double cm_max_disparity,bool cm_use_smaller_matching_window,int cd_nr_horz_blocks,int cd_nr_vert_blocks)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
UpdateReference(const unsigned char * const * im,bool subsample,bool detect_corners)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
Get_H_dref_to_ref(double H[9])310 void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9])
311 {
312 db_Copy9(H,m_H_dref_to_ref);
313 }
314
Get_H_dref_to_ins(double H[9])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
Set_H_dref_to_ins(double H[9])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
ResetDisplayReference()330 void db_FrameToReferenceRegistration::ResetDisplayReference()
331 {
332 db_Identity3x3(m_H_dref_to_ref);
333 }
334
NeedReferenceUpdate()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
AddFrame(const unsigned char * const * im,double H[9],bool force_reference,bool prewarp)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 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)
ComputeInliers(double H[9])514 void 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)
Polish(int * inlier_indices,int & num_inlier_indices)536 void 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
EstimateSecondaryModel(double H[9])579 void 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
ComputeCostArray()599 void 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
SelectOutliers()611 void 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
ComputeCostHistogram()630 void 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
SetOutlierThreshold()654 void 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
SmoothMotion(void)684 void 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
GenerateQuarterResImage(const unsigned char * const * im)744 void 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